Publish filtered orientation as Pose

enwaytech · Jul 9, 2024 · efd9b89 · efd9b89
1 parent c6b73da
commit efd9b89
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 0 deletions.
diff --git a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h
@@ -26,7 +26,10 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 
+#include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.h>
@@ -44,6 +47,7 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode
     typedef sensor_msgs::msg::Imu ImuMsg;
     typedef sensor_msgs::msg::MagneticField MagMsg;
     typedef geometry_msgs::msg::Vector3Stamped RpyVectorMsg;
+    typedef geometry_msgs::msg::PoseStamped PoseMsg;
 
     typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg>
         SyncPolicy;
@@ -68,7 +72,10 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode
     rclcpp::Publisher<RpyVectorMsg>::SharedPtr rpy_filtered_debug_publisher_;
     rclcpp::Publisher<RpyVectorMsg>::SharedPtr rpy_raw_debug_publisher_;
     rclcpp::Publisher<ImuMsg>::SharedPtr imu_publisher_;
+    rclcpp::Publisher<PoseMsg>::SharedPtr orientation_filtered_publisher_;
     tf2_ros::TransformBroadcaster tf_broadcaster_;
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
 
     rclcpp::TimerBase::SharedPtr check_topics_timer_;
 
@@ -104,6 +111,7 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode
     // **** member functions
     void publishFilteredMsg(ImuMsg::ConstSharedPtr imu_msg_raw);
     void publishTransform(ImuMsg::ConstSharedPtr imu_msg_raw);
+    void publishOrientationFiltered(const ImuMsg& imu_msg);
 
     void publishRawMsg(const rclcpp::Time& t, float roll, float pitch,
                        float yaw);

diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp
@@ -37,6 +37,8 @@ using namespace std::placeholders;
 ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options)
     : BaseNode("imu_filter_madgwick", options),
       tf_broadcaster_(this),
+      tf_buffer_(this->get_clock()),
+      tf_listener_(tf_buffer_),
       initialized_(false)
 {
     RCLCPP_INFO(get_logger(), "Starting ImuFilter");
@@ -194,6 +196,10 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options)
         rpy_raw_debug_publisher_ =
             create_publisher<geometry_msgs::msg::Vector3Stamped>("imu/rpy/raw",
                                                                  5);
+
+        orientation_filtered_publisher_ =
+            create_publisher<geometry_msgs::msg::PoseStamped>(
+                "imu/orientation/filtered", 5);
     }
 
     // **** register subscribers
@@ -483,7 +489,38 @@ void ImuFilterMadgwickRos::publishFilteredMsg(
 
         rpy.header = imu_msg_raw->header;
         rpy_filtered_debug_publisher_->publish(rpy);
+
+        publishOrientationFiltered(imu_msg);
+    }
+}
+
+void ImuFilterMadgwickRos::publishOrientationFiltered(const ImuMsg &imu_msg)
+{
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.stamp = imu_msg.header.stamp;
+    pose.header.frame_id = fixed_frame_;
+    pose.pose.orientation = imu_msg.orientation;
+
+    // get the current transform from the fixed frame to the imu frame
+    geometry_msgs::msg::TransformStamped transform;
+    try
+    {
+        transform = tf_buffer_.lookupTransform(
+            fixed_frame_, imu_msg.header.frame_id, imu_msg.header.stamp,
+            rclcpp::Duration::from_seconds(0.1));
+    } catch (tf2::TransformException &ex)
+    {
+        RCLCPP_WARN(
+            this->get_logger(), "Could not get transform from %s to %s: %s",
+            fixed_frame_.c_str(), imu_msg.header.frame_id.c_str(), ex.what());
+        return;
     }
+
+    pose.pose.position.x = transform.transform.translation.x;
+    pose.pose.position.y = transform.transform.translation.y;
+    pose.pose.position.z = transform.transform.translation.z;
+
+    orientation_filtered_publisher_->publish(pose);
 }
 
 void ImuFilterMadgwickRos::publishRawMsg(const rclcpp::Time &t, float roll,