Fawkes API
Fawkes Development Version
|
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 register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
double * linear_acceleration_covariance() const
Get linear_acceleration_covariance value.
void read()
Read from BlackBoard into local copy.
virtual ~RosIMUThread()
Destructor.
float * angular_velocity() const
Get angular_velocity value.
virtual void init()
Initialize the thread.
LockPtr< ros::NodeHandle > rosnode
double * orientation_covariance() const
Get orientation_covariance value.
virtual void log_info(const char *component, const char *format,...)=0
char * frame() const
Get frame value.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
const char * name() const
virtual InterfaceInfoList * list(const char *type_pattern, const char *id_pattern)=0
virtual void bb_interface_data_changed(fawkes::Interface *interface)
Handle interface changes.
float * linear_acceleration() const
Get linear_acceleration value.
RosIMUThread()
Constructor.
float * orientation() const
Get orientation value.
virtual void close(Interface *interface)=0
virtual void log_error(const char *component, const char *format,...)=0
double * angular_velocity_covariance() const
Get angular_velocity_covariance value.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual std::string get_string(const char *path)=0
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
virtual void finalize()
Close all interfaces and ROS handles.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.