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 <interfaces/RobotinoSensorInterface.h>
26#include <ros/node_handle.h>
27#include <utils/time/time.h>
28
29using 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
45void
47{
49 sens_if_->read();
50
51 pub_joints_ = rosnode->advertise<sensor_msgs::JointState>("joint_states", 1);
52
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";
59}
60
61void
63{
64 blackboard->close(sens_if_);
65 pub_joints_.shutdown();
66}
67
68void
70{
71 // update sensor values in interface
72 sens_if_->read();
73
74 if (sens_if_->refreshed()) {
75 const Time *ct = sens_if_->timestamp();
76 float * mot_velocity = sens_if_->mot_velocity();
77 int32_t * mot_position = sens_if_->mot_position();
78
79 joint_state_msg_.header.seq += 1;
80 joint_state_msg_.header.stamp = ros::Time(ct->get_sec(), ct->get_usec() * 1e3);
81
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;
85
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);
89
90 pub_joints_.publish(joint_state_msg_);
91 }
92}
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.
Definition: blackboard.h:44
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.
Definition: interface.cpp:714
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool refreshed() const
Check if data has been refreshed.
Definition: interface.cpp:811
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
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.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
long get_usec() const
Get microseconds.
Definition: time.h:127
long get_sec() const
Get seconds.
Definition: time.h:117
Fawkes library namespace.