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 }
virtual void finalize()
Close all interfaces and ROS handles.
Definition: imu_thread.cpp:89
virtual void init()
Initialize the thread.
Definition: imu_thread.cpp:52
virtual ~RosIMUThread()
Destructor.
Definition: imu_thread.cpp:43
RosIMUThread()
Constructor.
Definition: imu_thread.cpp:37
virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept
Handle interface changes.
Definition: imu_thread.cpp:101
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual InterfaceInfoList * list(const char *type_pattern, const char *id_pattern)=0
Get list of interfaces matching type and ID patterns.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
float * linear_acceleration() const
Get linear_acceleration value.
char * frame() const
Get frame value.
double * linear_acceleration_covariance() const
Get linear_acceleration_covariance value.
double * angular_velocity_covariance() const
Get angular_velocity_covariance value.
double * orientation_covariance() const
Get orientation_covariance value.
float * angular_velocity() const
Get angular_velocity value.
float * orientation() const
Get orientation value.
Interface information list.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.