Go to the documentation of this file.00001 #include "ros/ros.h"
00002 #include "std_msgs/Float32MultiArray.h"
00003 #include "sensor_msgs/JointState.h"
00004
00005 std_msgs::Float32MultiArray joints_cmd;
00006 ros::Publisher cmd_pub;
00007
00008
00009 void chatterCallback1(const sensor_msgs::JointState::ConstPtr &msg)
00010 {
00011 ROS_INFO("Callback1[%f,%f,%f,%f,%f,%f]",msg->position[0],msg->position[1],msg->position[2],msg->position[3],msg->position[4],msg->position[5]);
00012 joints_cmd.data[0] = msg->position[0];
00013 joints_cmd.data[1] = msg->position[1];
00014 joints_cmd.data[2] = msg->position[2];
00015 joints_cmd.data[3] = msg->position[3];
00016 joints_cmd.data[4] = msg->position[4];
00017 joints_cmd.data[5] = msg->position[5];
00018 cmd_pub.publish(joints_cmd);
00019 }
00020
00021
00022
00023 int main(int argc, char **argv)
00024 {
00025
00026 ros::init(argc, argv, "joint_state_sub");
00027
00028 ros::NodeHandle nh;
00029
00030 sleep(3.0);
00031
00032 ros::Subscriber sub1 = nh.subscribe("joint_states", 1000, chatterCallback1);
00033 cmd_pub = nh.advertise<std_msgs::Float32MultiArray> ("movej_cmd", 1);
00034
00035
00036 joints_cmd.data.resize(6);
00037
00038 ros::spin();
00039
00040 return 0;
00041 }