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.
long get_usec() const
Get microseconds.
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.
const Time * timestamp() const
Get timestamp of last write.
long get_sec() const
Get seconds.
void read()
Read from BlackBoard into local copy.
float * mot_velocity() const
Get mot_velocity value.
virtual void finalize()
Finalize the thread.
int32_t * mot_position() const
Get mot_position value.
virtual void init()
Initialize the thread.
bool changed() const
Check if data has been changed.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.