Fawkes API  Fawkes Development Version
odometry_thread.cpp
1 /***************************************************************************
2  * odometry_thread.h - Thread to publish odometry to ROS
3  *
4  * Created: Fri Jun 1 13:29:39 CEST
5  * Copyright 2012 Sebastian Reuter
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "odometry_thread.h"
22 #include <interfaces/MotorInterface.h>
23 #include <nav_msgs/Odometry.h>
24 #include <tf/types.h>
25 
26 using namespace fawkes;
27 
28 /** @class ROSOdometryThread "odometry_thread.h"
29  * Thread to publish odometry to ROS.
30  * @author Sebastian Reuter
31  */
32 
33 /** Constructor. */
35  : Thread("OdometryThread", Thread::OPMODE_WAITFORWAKEUP),
36  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP)
37 {
38 }
39 
40 void
42 {
43  std::string motor_if_id = config->get_string("/ros/odometry/motor_interface_id");
44  cfg_odom_frame_id_ = config->get_string("/ros/odometry/odom_frame_id");
45  cfg_base_frame_id_ = config->get_string("/ros/odometry/base_frame_id");
46  motor_if_ = blackboard->open_for_reading<MotorInterface>(motor_if_id.c_str());
47  pub_ = rosnode->advertise<nav_msgs::Odometry>("odom", 10);
48 }
49 
50 void
52 {
53  blackboard->close(motor_if_);
54  pub_.shutdown();
55 }
56 
57 
58 void
59 ROSOdometryThread::publish_odom()
60 {
61  nav_msgs::Odometry odom;
62  //Header
63  odom.header.stamp = ros::Time::now();
64  odom.header.frame_id = cfg_odom_frame_id_;
65  //Position
66  odom.pose.pose.position.x = (double)motor_if_->odometry_position_x();
67  odom.pose.pose.position.y = (double) motor_if_->odometry_position_y();
68  odom.pose.pose.position.z = 0.0;
69  fawkes::tf::Quaternion q(motor_if_->odometry_orientation(), 0, 0);
70  geometry_msgs::Quaternion odom_quat;
71  odom_quat.x = q.x();
72  odom_quat.y = q.y();
73  odom_quat.z = q.z();
74  odom_quat.w = q.w();
75  odom.pose.pose.orientation = odom_quat;
76  //Motion
77  odom.child_frame_id = cfg_base_frame_id_;
78  odom.twist.twist.linear.x = (double)motor_if_->vx();
79  odom.twist.twist.linear.y = (double)motor_if_->vy();
80  odom.twist.twist.angular.z = (double)motor_if_->omega();
81  pub_.publish(odom);
82 }
83 
85  motor_if_->read();
86  publish_odom();
87 }
virtual void finalize()
Finalize the thread.
float odometry_position_x() const
Get odometry_position_x value.
Fawkes library namespace.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void loop()
Code to execute in the thread.
float vx() const
Get vx value.
float odometry_orientation() const
Get odometry_orientation value.
Thread aspect to use blocked timing.
float odometry_position_y() const
Get odometry_position_y value.
float vy() const
Get vy value.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
ROSOdometryThread()
Constructor.
float omega() const
Get omega value.
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
MotorInterface Fawkes BlackBoard Interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual void init()
Initialize the thread.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
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.