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  * 2017 Till Hofmann
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "odometry_thread.h"
23 
24 #include <interfaces/MotorInterface.h>
25 #include <nav_msgs/Odometry.h>
26 #include <tf/types.h>
27 
28 using namespace fawkes;
29 
30 /** @class ROSOdometryThread "odometry_thread.h"
31  * Thread to publish odometry to ROS.
32  * @author Sebastian Reuter
33  */
34 
35 /** Constructor. */
37 : Thread("OdometryThread", Thread::OPMODE_WAITFORWAKEUP),
38  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP)
39 {
40 }
41 
42 void
44 {
45  std::string motor_if_id = config->get_string("/ros/odometry/motor_interface_id");
46  cfg_odom_frame_id_ = config->get_string("/ros/odometry/odom_frame_id");
47  cfg_base_frame_id_ = config->get_string("/ros/odometry/base_frame_id");
48  if (config->exists("/ros/odometry/odom/covariance")) {
49  odom_covariance_.assign(0.);
50  std::vector<float> cfg_odom_covariance = config->get_floats("/ros/odometry/odom/covariance");
51  for (uint i = 0; i < cfg_odom_covariance.size() && i < odom_covariance_.size(); i++) {
52  odom_covariance_[i] = cfg_odom_covariance.size();
53  }
54  } else {
55  odom_covariance_ = {1e-3, 0., 0., 0., 0., 0., 0., 1e-3, 0., 0., 0., 0.,
56  0., 0., 1e-3, 0., 0., 0., 0., 0., 0., 1e-3, 0., 0.,
57  0., 0., 0., 0., 1e-3, 0., 0., 0., 0., 0., 0., 1e-3};
58  }
59  motor_if_ = blackboard->open_for_reading<MotorInterface>(motor_if_id.c_str());
60  pub_ = rosnode->advertise<nav_msgs::Odometry>("odom", 10);
61 }
62 
63 void
65 {
66  blackboard->close(motor_if_);
67  pub_.shutdown();
68 }
69 
70 void
71 ROSOdometryThread::publish_odom()
72 {
73  nav_msgs::Odometry odom;
74  //Header
75  odom.header.stamp = ros::Time::now();
76  odom.header.frame_id = cfg_odom_frame_id_;
77  //Position
78  odom.pose.pose.position.x = (double)motor_if_->odometry_position_x();
79  odom.pose.pose.position.y = (double)motor_if_->odometry_position_y();
80  odom.pose.pose.position.z = 0.0;
81  odom.pose.covariance = odom_covariance_;
82  fawkes::tf::Quaternion q(motor_if_->odometry_orientation(), 0, 0);
83  geometry_msgs::Quaternion odom_quat;
84  odom_quat.x = q.x();
85  odom_quat.y = q.y();
86  odom_quat.z = q.z();
87  odom_quat.w = q.w();
88  odom.pose.pose.orientation = odom_quat;
89  //Motion
90  odom.child_frame_id = cfg_base_frame_id_;
91  odom.twist.twist.linear.x = (double)motor_if_->vx();
92  odom.twist.twist.linear.y = (double)motor_if_->vy();
93  odom.twist.twist.angular.z = (double)motor_if_->omega();
94  odom.twist.covariance = odom_covariance_;
95  pub_.publish(odom);
96 }
97 
98 void
100 {
101  motor_if_->read();
102  publish_odom();
103 }
ROSOdometryThread::finalize
virtual void finalize()
Finalize the thread.
Definition: odometry_thread.cpp:64
fawkes::MotorInterface::vx
float vx() const
Get vx value.
Definition: MotorInterface.cpp:420
fawkes::MotorInterface::omega
float omega() const
Get omega value.
Definition: MotorInterface.cpp:488
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
fawkes::ROSAspect::rosnode
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
ROSOdometryThread::loop
virtual void loop()
Code to execute in the thread.
Definition: odometry_thread.cpp:99
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::MotorInterface
MotorInterface Fawkes BlackBoard Interface.
Definition: MotorInterface.h:34
fawkes::MotorInterface::odometry_position_y
float odometry_position_y() const
Get odometry_position_y value.
Definition: MotorInterface.cpp:352
fawkes::Configuration::get_floats
virtual std::vector< float > get_floats(const char *path)=0
Get list of values from configuration which is of type float.
ROSOdometryThread::ROSOdometryThread
ROSOdometryThread()
Constructor.
Definition: odometry_thread.cpp:36
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes
Fawkes library namespace.
fawkes::MotorInterface::odometry_orientation
float odometry_orientation() const
Get odometry_orientation value.
Definition: MotorInterface.cpp:386
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
fawkes::MotorInterface::odometry_position_x
float odometry_position_x() const
Get odometry_position_x value.
Definition: MotorInterface.cpp:318
fawkes::Configuration::exists
virtual bool exists(const char *path)=0
Check if a given value exists.
fawkes::MotorInterface::vy
float vy() const
Get vy value.
Definition: MotorInterface.cpp:454
ROSOdometryThread::init
virtual void init()
Initialize the thread.
Definition: odometry_thread.cpp:43