23 #include "ros_joints_thread.h" 25 #include <utils/time/time.h> 26 #include <interfaces/RobotinoSensorInterface.h> 27 #include <ros/node_handle.h> 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_);
Fawkes library namespace.
int32_t * mot_position() const
Get mot_position value.
A class for handling time.
Thread class encapsulation of pthreads.
RobotinoRosJointsThread()
Constructor.
RobotinoSensorInterface Fawkes BlackBoard Interface.
virtual void loop()
Code to execute in the thread.
Thread aspect to use blocked timing.
void read()
Read from BlackBoard into local copy.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
const Time * timestamp() const
Get timestamp of last write.
bool changed() const
Check if data has been changed.
long get_sec() const
Get seconds.
long get_usec() const
Get microseconds.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
float * mot_velocity() const
Get mot_velocity value.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.