23 #include "ros_joints_thread.h"
25 #include <utils/time/time.h>
26 #include <interfaces/RobotinoSensorInterface.h>
27 #include <ros/node_handle.h>
29 using namespace fawkes;
40 :
Thread(
"RobotinoRosJointsThread",
Thread::OPMODE_WAITFORWAKEUP),
52 pub_joints_ =
rosnode->advertise<sensor_msgs::JointState>(
"joint_states", 1);
54 joint_state_msg_.name.resize(3);
55 joint_state_msg_.position.resize(3, 0.0);
56 joint_state_msg_.velocity.resize(3, 0.0);
57 joint_state_msg_.name[0] =
"wheel2_joint";
58 joint_state_msg_.name[1] =
"wheel0_joint";
59 joint_state_msg_.name[2] =
"wheel1_joint";
68 pub_joints_.shutdown();
82 joint_state_msg_.header.seq += 1;
83 joint_state_msg_.header.stamp = ros::Time(ct->
get_sec(), ct->
get_usec() * 1e3);
85 joint_state_msg_.velocity[0] = (mot_velocity[2] / 16) * (2 * M_PI) / 60;
86 joint_state_msg_.velocity[1] = (mot_velocity[0] / 16) * (2 * M_PI) / 60;
87 joint_state_msg_.velocity[2] = (mot_velocity[1] / 16) * (2 * M_PI) / 60;
89 joint_state_msg_.position[0] = (mot_position[2] / 16) * (2 * M_PI);
90 joint_state_msg_.position[1] = (mot_position[0] / 16) * (2 * M_PI);
91 joint_state_msg_.position[2] = (mot_position[1] / 16) * (2 * M_PI);
93 pub_joints_.publish(joint_state_msg_);