Fawkes API  Fawkes Development Version
ros_joints_thread.cpp
1 
2 /***************************************************************************
3  * ros_joints_thread.cpp - Publish Robotino joint info via ROS
4  *
5  * Created: Fri Mar 30 11:08:03 2012
6  * Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "ros_joints_thread.h"
24 
25 #include <utils/time/time.h>
26 #include <interfaces/RobotinoSensorInterface.h>
27 #include <ros/node_handle.h>
28 
29 using namespace fawkes;
30 
31 /** @class RobotinoRosJointsThread "ir_pcl_thread.h"
32  * Robotino IR distances as point cloud.
33  * This thread integrates into the Fawkes main loop at the SENSOR_PROCESS
34  * hook and converts sensor data to a pointcloud
35  * @author Tim Niemueller
36  */
37 
38 /** Constructor. */
40  : Thread("RobotinoRosJointsThread", Thread::OPMODE_WAITFORWAKEUP),
41  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
42 {
43 }
44 
45 
46 void
48 {
49  sens_if_ = blackboard->open_for_reading<RobotinoSensorInterface>("Robotino");
50  sens_if_->read();
51 
52  pub_joints_ = rosnode->advertise<sensor_msgs::JointState>("joint_states", 1);
53 
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";
60 
61 }
62 
63 
64 void
66 {
67  blackboard->close(sens_if_);
68  pub_joints_.shutdown();
69 }
70 
71 void
73 {
74  // update sensor values in interface
75  sens_if_->read();
76 
77  if (sens_if_->changed()) {
78  const Time *ct = sens_if_->timestamp();
79  float *mot_velocity = sens_if_->mot_velocity();
80  int32_t *mot_position = sens_if_->mot_position();
81 
82  joint_state_msg_.header.seq += 1;
83  joint_state_msg_.header.stamp = ros::Time(ct->get_sec(), ct->get_usec() * 1e3);
84 
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;
88 
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);
92 
93  pub_joints_.publish(joint_state_msg_);
94  }
95 }
Fawkes library namespace.
int32_t * mot_position() const
Get mot_position value.
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
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.
Definition: interface.cpp:477
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:718
bool changed() const
Check if data has been changed.
Definition: interface.cpp:796
long get_sec() const
Get seconds.
Definition: time.h:110
long get_usec() const
Get microseconds.
Definition: time.h:112
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.
Definition: ros.h:48
float * mot_velocity() const
Get mot_velocity value.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.