Go to the documentation of this file.00001 #include "carl_dynamixel/front_joints_node.h"
00002
00003
00004
00005
00006 front_joints::front_joints()
00007 {
00008 std::string servo_name;
00009 int num, id;
00010
00011 ros::NodeHandle private_node_handle_("~");
00012 node.getParam("front_servos/num_servos", num);
00013 for(int s = 1; s <= num; s++){
00014 std::stringstream ss;
00015 ss<<s;
00016 std::string servo_num = ss.str();
00017 std::string param_id ("front_servos/" + servo_num + "/id");
00018 std::string param_name ("front_servos/" + servo_num + "/link_name");
00019 node.getParam(param_id,id);
00020 node.getParam(param_name,servo_name);
00021 front_servos[id] = servo_name;
00022 }
00023
00024
00025 creative_sub = node.subscribe("motor_states/front_port", 1000, &front_joints::joints, this);
00026
00027
00028 joint_states_pub = node.advertise<sensor_msgs::JointState>("dynamixel_front", 1);
00029
00030
00031 }
00032
00033 void front_joints::joints(const dynamixel_msgs::MotorStateList::ConstPtr& state){
00034 sensor_msgs::JointState front_states;
00035 for(int s = 0; s < state->motor_states.size(); s++){
00036 dynamixel_msgs::MotorState servo = state->motor_states[s];
00037 std::string name = front_servos[servo.id];
00038 if(name.size() == 0){
00039 name = "UNDEFINED";
00040 }
00041 front_states.name.push_back(name);
00042 float servo_pos_deg = ((float)servo.position-512.0)*0.2929;
00043 front_states.position.push_back((servo_pos_deg*M_PI)/180.0);
00044 }
00045 front_states.header.stamp = ros::Time::now();
00046 joint_states_pub.publish(front_states);
00047 }
00048
00049
00050
00051
00052 int main(int argc, char **argv)
00053 {
00054
00055 ros::init(argc, argv, "front_dynamixel");
00056
00057
00058 front_joints front;
00059
00060
00061 ros::Rate loop_rate(30);
00062 while (ros::ok())
00063 {
00064 ros::spinOnce();
00065 loop_rate.sleep();
00066 }
00067
00068 return EXIT_SUCCESS;
00069 }