front_joint_node.cpp
Go to the documentation of this file.
00001 #include "carl_dynamixel/front_joints_node.h"
00002 
00003 /*
00004  * Constructor to inialize the node
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         //setup the subscriber
00025     creative_sub = node.subscribe("motor_states/front_port", 1000, &front_joints::joints, this);
00026     
00027     //setup the publisher
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  * main function
00051  */
00052 int main(int argc, char **argv)
00053 {
00054         //initialize the node
00055         ros::init(argc, argv, "front_dynamixel");
00056 
00057         // initialize the joystick controller
00058         front_joints front;
00059 
00060         //main loop
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 }


carl_dynamixel
Author(s): Chris Dunkers , David Kent
autogenerated on Thu Jun 6 2019 21:09:46