publish orientation as Pose to vis on rviz

Author: adivardi

imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h

@@ -29,7 +29,9 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/MagneticField.h>
 #include <geometry_msgs/Vector3Stamped.h>
+#include <tf2_ros/buffer.h>
 #include "tf2_ros/transform_broadcaster.h"
+#include <tf2_ros/transform_listener.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/sync_policies/approximate_time.h>
@@ -74,8 +76,11 @@ class ImuFilterRos
 
     ros::Publisher rpy_filtered_debug_publisher_;
     ros::Publisher rpy_raw_debug_publisher_;
+    ros::Publisher orientation_filtered_publisher_;
     ros::Publisher imu_publisher_;
     tf2_ros::TransformBroadcaster tf_broadcaster_;
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
 
     boost::shared_ptr<FilterConfigServer> config_server_;
     ros::Timer check_topics_timer_;
@@ -111,6 +116,7 @@ class ImuFilterRos
     void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw);
 
     void publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw);
+    void publishOrientationFiltered(const ImuMsg::ConstPtr& imu_msg);
     void publishTransform(const ImuMsg::ConstPtr& imu_msg_raw);
 
     void publishRawMsg(const ros::Time& t, float roll, float pitch, float yaw);

imu_filter_madgwick/src/imu_filter_ros.cpp

@@ -25,11 +25,15 @@
 #include "imu_filter_madgwick/imu_filter_ros.h"
 #include "imu_filter_madgwick/stateless_orientation.h"
 #include "geometry_msgs/TransformStamped.h"
+#include "geometry_msgs/PoseStamped.h"
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/LinearMath/Matrix3x3.h>
 
 ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private)
-    : nh_(nh), nh_private_(nh_private), initialized_(false)
+    : nh_(nh),
+      nh_private_(nh_private),
+      initialized_(false),
+      tf_listener_(tf_buffer_)
 {
     ROS_INFO("Starting ImuFilter");
 
@@ -121,6 +125,10 @@ ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private)
 
         rpy_raw_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
             ros::names::resolve("imu") + "/rpy/raw", 5);
+
+        orientation_filtered_publisher_ =
+            nh_private.advertise<geometry_msgs::PoseStamped>(
+                ros::names::resolve("imu") + "/orientation_filtered", 5);
     }
 
     // **** register subscribers
@@ -407,9 +415,37 @@ void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)
 
         rpy.header = imu_msg_raw->header;
         rpy_filtered_debug_publisher_.publish(rpy);
+
+        publishOrientationFiltered(imu_msg);
     }
 }
 
+void ImuFilterRos::publishOrientationFiltered(const ImuMsg::ConstPtr& imu_msg)
+{
+    geometry_msgs::PoseStamped pose_msg;
+    pose_msg.header.stamp = imu_msg->header.stamp;
+    pose_msg.header.frame_id = fixed_frame_;
+    pose_msg.pose.orientation = imu_msg->orientation;
+
+    // get the current transform from the fixed frame to the imu frame
+    geometry_msgs::TransformStamped transform;
+    try
+    {
+        transform = tf_buffer_.lookupTransform(
+            fixed_frame_, imu_msg->header.frame_id, imu_msg->header.stamp);
+    } catch (tf2::TransformException& ex)
+    {
+        ROS_WARN("%s", ex.what());
+        return;
+    }
+
+    pose_msg.pose.position.x = transform.transform.translation.x;
+    pose_msg.pose.position.y = transform.transform.translation.y;
+    pose_msg.pose.position.z = transform.transform.translation.z;
+
+    orientation_filtered_publisher_.publish(pose_msg);
+}
+
 void ImuFilterRos::publishRawMsg(const ros::Time& t, float roll, float pitch,
                                  float yaw)
 {