21 #include "imu_thread.h"
23 #include <interface/interface_info.h>
24 #include <ros/this_node.h>
25 #include <sensor_msgs/Imu.h>
54 std::string iface_name;
59 if (imu_ifaces->empty()) {
63 iface_name = imu_ifaces->front().id();
65 "%s. Using first IMU interface '%s'.",
70 std::string ros_topic =
"/imu/data";
77 "Publishing IMU '%s' to ROS topic '%s'.",
80 ros_pub_ =
rosnode->advertise<sensor_msgs::Imu>(ros_topic, 100);
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++) {
119 for (uint i = 0; i < 9u; i++) {
125 for (uint i = 0; i < 9u; i++) {
128 ros_pub_.publish(ros_imu);
virtual void finalize()
Close all interfaces and ROS handles.
virtual void init()
Initialize the thread.
virtual void bb_interface_data_changed(fawkes::Interface *interface)
Handle interface changes.
virtual ~RosIMUThread()
Destructor.
RosIMUThread()
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
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.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
IMUInterface Fawkes BlackBoard Interface.
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.
void read()
Read from BlackBoard into local copy.
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.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.