Fawkes API  Fawkes Development Version
imu_thread.cpp
1 /***************************************************************************
2  * imu_thread.cpp - Thread to publish IMU data to ROS
3  *
4  * Created: Mon 03 Apr 2017 12:41:33 CEST 12:41
5  * Copyright 2017 Till Hofmann <hofmann@kbsg.rwth-aachen.de>
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 "imu_thread.h"
22 
23 #include <interface/interface_info.h>
24 #include <ros/this_node.h>
25 #include <sensor_msgs/Imu.h>
26 
27 using namespace fawkes;
28 
29 /** @class RosIMUThread "imu_thread.h"
30  * Thread to publish IMU data to ROS.
31  * This thread reads data from the IMU blackboard interface and publishes the
32  * data to ROS.
33  * @author Till Hofmann
34  */
35 
36 /** Constructor. */
38 : Thread("RosIMUThread", Thread::OPMODE_WAITFORWAKEUP), BlackBoardInterfaceListener("RosIMUThread")
39 {
40 }
41 
42 /** Destructor. */
44 {
45 }
46 
47 /** Initialize the thread.
48  * Open the blackboard interface and advertise the ROS topic.
49  * Register as listener for the blackboard interface.
50  */
51 void
53 {
54  std::string iface_name;
55  try {
56  iface_name = config->get_string("/ros/imu/interface");
57  } catch (Exception &e) {
58  InterfaceInfoList *imu_ifaces = blackboard->list("IMUInterface", "*");
59  if (imu_ifaces->empty()) {
60  logger->log_error(name(), "Cannot use default IMUInterface: No IMUInterface found!");
61  throw;
62  } else {
63  iface_name = imu_ifaces->front().id();
64  logger->log_info(name(),
65  "%s. Using first IMU interface '%s'.",
67  iface_name.c_str());
68  }
69  }
70  std::string ros_topic = "/imu/data";
71  try {
72  ros_topic = config->get_string("/ros/imu/topic");
73  } catch (Exception &e) {
74  // ignore, use default
75  }
76  logger->log_info(name(),
77  "Publishing IMU '%s' to ROS topic '%s'.",
78  iface_name.c_str(),
79  ros_topic.c_str());
80  ros_pub_ = rosnode->advertise<sensor_msgs::Imu>(ros_topic, 100);
81 
82  iface_ = blackboard->open_for_reading<IMUInterface>(iface_name.c_str());
85 }
86 
87 /** Close all interfaces and ROS handles. */
88 void
90 {
92  blackboard->close(iface_);
93  ros_pub_.shutdown();
94 }
95 
96 /** Handle interface changes.
97  * If the IMU interface changes, publish updated data to ROS.
98  * @param interface interface instance that you supplied to bbil_add_data_interface()
99  */
100 void
102 {
103  IMUInterface *imu_iface = dynamic_cast<IMUInterface *>(interface);
104  if (!imu_iface)
105  return;
106  imu_iface->read();
107  sensor_msgs::Imu ros_imu;
108  ros_imu.header.frame_id = imu_iface->frame();
109  ros_imu.orientation.x = imu_iface->orientation()[0];
110  ros_imu.orientation.y = imu_iface->orientation()[1];
111  ros_imu.orientation.z = imu_iface->orientation()[2];
112  ros_imu.orientation.w = imu_iface->orientation()[3];
113  for (uint i = 0; i < 9u; i++) {
114  ros_imu.orientation_covariance[i] = imu_iface->orientation_covariance()[i];
115  }
116  ros_imu.angular_velocity.x = imu_iface->angular_velocity()[0];
117  ros_imu.angular_velocity.y = imu_iface->angular_velocity()[1];
118  ros_imu.angular_velocity.z = imu_iface->angular_velocity()[2];
119  for (uint i = 0; i < 9u; i++) {
120  ros_imu.angular_velocity_covariance[i] = imu_iface->angular_velocity_covariance()[i];
121  }
122  ros_imu.linear_acceleration.x = imu_iface->linear_acceleration()[0];
123  ros_imu.linear_acceleration.y = imu_iface->linear_acceleration()[1];
124  ros_imu.linear_acceleration.z = imu_iface->linear_acceleration()[2];
125  for (uint i = 0; i < 9u; i++) {
126  ros_imu.linear_acceleration_covariance[i] = imu_iface->linear_acceleration_covariance()[i];
127  }
128  ros_pub_.publish(ros_imu);
129 }
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:188
fawkes::IMUInterface::linear_acceleration_covariance
double * linear_acceleration_covariance() const
Get linear_acceleration_covariance value.
Definition: IMUInterface.cpp:476
fawkes::IMUInterface
Definition: IMUInterface.h:37
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:475
RosIMUThread::~RosIMUThread
virtual ~RosIMUThread()
Destructor.
Definition: imu_thread.cpp:42
fawkes::IMUInterface::angular_velocity
float * angular_velocity() const
Get angular_velocity value.
Definition: IMUInterface.cpp:272
RosIMUThread::init
virtual void init()
Initialize the thread.
Definition: imu_thread.cpp:51
fawkes::ROSAspect::rosnode
LockPtr< ros::NodeHandle > rosnode
Definition: ros.h:46
fawkes::IMUInterface::orientation_covariance
double * orientation_covariance() const
Get orientation_covariance value.
Definition: IMUInterface.cpp:204
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::IMUInterface::frame
char * frame() const
Get frame value.
Definition: IMUInterface.cpp:100
fawkes::BlackBoard::unregister_listener
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:215
fawkes::BlackBoardInterfaceListener
Definition: interface_listener.h:45
fawkes::Thread::name
const char * name() const
Definition: thread.h:99
fawkes::BlackBoard::list
virtual InterfaceInfoList * list(const char *type_pattern, const char *id_pattern)=0
RosIMUThread::bb_interface_data_changed
virtual void bb_interface_data_changed(fawkes::Interface *interface)
Handle interface changes.
Definition: imu_thread.cpp:100
fawkes::IMUInterface::linear_acceleration
float * linear_acceleration() const
Get linear_acceleration value.
Definition: IMUInterface.cpp:408
RosIMUThread::RosIMUThread
RosIMUThread()
Constructor.
Definition: imu_thread.cpp:36
fawkes::IMUInterface::orientation
float * orientation() const
Get orientation value.
Definition: IMUInterface.cpp:136
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:50
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
fawkes
fawkes::InterfaceInfoList
Definition: interface_info.h:79
fawkes::Interface
Definition: interface.h:77
fawkes::IMUInterface::angular_velocity_covariance
double * angular_velocity_covariance() const
Get angular_velocity_covariance value.
Definition: IMUInterface.cpp:340
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:50
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:662
fawkes::Thread
Definition: thread.h:44
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:47
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
RosIMUThread::finalize
virtual void finalize()
Close all interfaces and ROS handles.
Definition: imu_thread.cpp:88
fawkes::BlackBoardInterfaceListener::bbil_add_data_interface
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
Definition: interface_listener.cpp:236
fawkes::Exception
Definition: exception.h:39