setJointPosition.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2013 University of Jaume-I.
00003  * All rights reserved. This program and the accompanying materials
00004  * are made available under the terms of the GNU Public License v3.0
00005  * which accompanies this distribution, and is available at
00006  * http://www.gnu.org/licenses/gpl.html
00007  * 
00008  * Contributors:
00009  *     Mario Prats
00010  *     Javier Perez
00011  */ 
00012 
00013 #include <ros/ros.h>
00014 #include <sensor_msgs/JointState.h>
00015 #include <stdlib.h>
00016 
00017 int main(int argc, char **argv) {
00018         if (argc != 7) {
00019                 std::cerr << "Usage: " << argv[0] << "<topic> <q1> <q2> <q3> <q4> <q5>" << std::endl;
00020                 std::cerr << "Units are radians" << std::endl;
00021                 exit(0);
00022         }
00023         std::string topic(argv[1]);
00024 
00025         ros::init(argc, argv, "setJointPosition");
00026         ros::NodeHandle nh;
00027         ros::Publisher position_pub;
00028         position_pub=nh.advertise<sensor_msgs::JointState>(topic,1);
00029         ros::Rate rate(30);
00030 
00031         double q[5];
00032         for (int i=0; i<5; i++) q[i]=atof(argv[i+2]);
00033 
00034         while (ros::ok()) {
00035                 
00036                 sensor_msgs::JointState js;
00037                 js.name.push_back(std::string("Slew"));
00038                 js.position.push_back(q[0]);
00039                 js.name.push_back(std::string("Shoulder"));
00040                 js.position.push_back(q[1]);
00041                 js.name.push_back(std::string("Elbow"));
00042                 js.position.push_back(q[2]);
00043                 js.name.push_back(std::string("JawRotate"));
00044                 js.position.push_back(q[3]);
00045                 js.name.push_back(std::string("JawOpening"));
00046                 js.position.push_back(q[4]);
00047 
00048                 position_pub.publish(js);
00049 
00050                 ros::spinOnce();
00051                 rate.sleep();
00052         }
00053 
00054         return 0;
00055 }


uwsim
Author(s): Mario Prats , Javier Perez
autogenerated on Fri Aug 28 2015 13:28:58