23 #include "ros_joints_thread.h"
25 #include <interfaces/RobotinoSensorInterface.h>
26 #include <ros/node_handle.h>
27 #include <utils/time/time.h>
40 :
Thread(
"RobotinoRosJointsThread",
Thread::OPMODE_WAITFORWAKEUP),
51 pub_joints_ =
rosnode->advertise<sensor_msgs::JointState>(
"joint_states", 1);
53 joint_state_msg_.name.resize(3);
54 joint_state_msg_.position.resize(3, 0.0);
55 joint_state_msg_.velocity.resize(3, 0.0);
56 joint_state_msg_.name[0] =
"wheel2_joint";
57 joint_state_msg_.name[1] =
"wheel0_joint";
58 joint_state_msg_.name[2] =
"wheel1_joint";
65 pub_joints_.shutdown();
79 joint_state_msg_.header.seq += 1;
80 joint_state_msg_.header.stamp = ros::Time(ct->
get_sec(), ct->
get_usec() * 1e3);
82 joint_state_msg_.velocity[0] = (mot_velocity[2] / 16) * (2 * M_PI) / 60;
83 joint_state_msg_.velocity[1] = (mot_velocity[0] / 16) * (2 * M_PI) / 60;
84 joint_state_msg_.velocity[2] = (mot_velocity[1] / 16) * (2 * M_PI) / 60;
86 joint_state_msg_.position[0] = (mot_position[2] / 16) * (2 * M_PI);
87 joint_state_msg_.position[1] = (mot_position[0] / 16) * (2 * M_PI);
88 joint_state_msg_.position[2] = (mot_position[1] / 16) * (2 * M_PI);
90 pub_joints_.publish(joint_state_msg_);
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
RobotinoRosJointsThread()
Constructor.
virtual void loop()
Code to execute in the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
const Time * timestamp() const
Get timestamp of last write.
void read()
Read from BlackBoard into local copy.
bool refreshed() const
Check if data has been refreshed.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
RobotinoSensorInterface Fawkes BlackBoard Interface.
int32_t * mot_position() const
Get mot_position value.
float * mot_velocity() const
Get mot_velocity value.
Thread class encapsulation of pthreads.
A class for handling time.
long get_usec() const
Get microseconds.
long get_sec() const
Get seconds.
Fawkes library namespace.