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
27using 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 */
51void
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();
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 }
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. */
88void
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 */
100void
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.