added rel pose from origin pub to subscriberimu

include/subscriber_imu.h

@@ -31,6 +31,7 @@
 #include <core/utils/params_server.h>
 #include <imu/capture/capture_imu.h>
 #include <imu/sensor/sensor_imu.h>
+#include <core/processor/processor_motion.h>
 
 /**************************
  *      ROS includes      *
@@ -59,9 +60,10 @@ class SubscriberImu : public Subscriber
         bool in_degrees_;
         std::string cov_source_;
         bool publish_bias_;
-        ros::Publisher pub_bias_accel_, pub_bias_gyro_, pub_imu_corrected_, pub_imu_remapped_;
+        ros::Publisher pub_bias_accel_, pub_bias_gyro_, pub_imu_corrected_, pub_imu_remapped_, pub_pose_from_origin_;
         int imu_x_axis_, imu_y_axis_, imu_z_axis_;
         bool imu_x_neg_, imu_y_neg_, imu_z_neg_;
+        ProcessorMotionPtr processor_motion_ptr_;
 
     public:
         // Constructor

src/subscriber_imu.cpp

@@ -21,6 +21,9 @@
 //--------LICENSE_END--------
 #include "../include/subscriber_imu.h"
 #include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/PoseStamped.h>
+#include "core/math/SE3.h"
+#include "core/math/SE2.h"
 
 using namespace wolf;
 
@@ -90,10 +93,11 @@ void SubscriberImu::initialize(ros::NodeHandle& nh, const std::string& topic)
 {
     sub_     = nh.subscribe(topic, 100, &SubscriberImu::callback, this);
 
-    pub_bias_accel_     = nh.advertise<geometry_msgs::Vector3>  (topic + "_bias_accel", 1);
-    pub_bias_gyro_      = nh.advertise<geometry_msgs::Vector3>  (topic + "_bias_gyro", 1);
-    pub_imu_corrected_  = nh.advertise<sensor_msgs::Imu>        (topic + "_corrected", 1);
-    pub_imu_remapped_   = nh.advertise<sensor_msgs::Imu>        (topic + "_remapped", 1);
+    pub_bias_accel_         = nh.advertise<geometry_msgs::Vector3>     (topic + "_bias_accel", 1);
+    pub_bias_gyro_          = nh.advertise<geometry_msgs::Vector3>     (topic + "_bias_gyro", 1);
+    pub_imu_corrected_      = nh.advertise<sensor_msgs::Imu>           (topic + "_corrected", 1);
+    pub_imu_remapped_       = nh.advertise<sensor_msgs::Imu>           (topic + "_remapped", 1);
+    pub_pose_from_origin_   = nh.advertise<geometry_msgs::PoseStamped> (topic + "_pose_from_origin", 1);
 }
 
 void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
@@ -236,8 +240,6 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
             imu_corrected.angular_velocity.z    = data(5) - bias(2);
 
             pub_imu_corrected_.publish(imu_corrected);
-            
-
         }
     }
     // Publish IMU message with axis remapping
@@ -245,7 +247,8 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
     {
         // fill and publish
         sensor_msgs::Imu imu_remapped = *msg;
-        if(imu_remapped.header.frame_id == "") imu_remapped.header.frame_id = "IMU";
+        if(imu_remapped.header.frame_id == "") 
+            imu_remapped.header.frame_id = "imu";
 
         imu_remapped.linear_acceleration.x = data(0);
         imu_remapped.linear_acceleration.y = data(1);
@@ -256,4 +259,57 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
 
         pub_imu_remapped_.publish(imu_remapped);
     }
+    // Publish current relative pose w.r.t. origin pose
+    if (pub_pose_from_origin_.getNumSubscribers() != 0)
+    {
+        // initialize processor_motion pointer
+        if (not processor_motion_ptr_)
+            for (auto proc : sensor_ptr_->getProcessorList())
+                if (std::dynamic_pointer_cast<ProcessorMotion>(proc) != nullptr)
+                {
+                    processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(proc);
+                    break;
+                }
+
+        // get current and origin states
+        auto origin_state = processor_motion_ptr_->getOrigin()->getState("PO").vector("PO");
+        auto current_state = sensor_ptr_->getProblem()->getState("PO").vector("PO");
+        auto current_ts = sensor_ptr_->getProblem()->getTimeStamp();
+
+        // Fill message
+        geometry_msgs::PoseStamped pose_msg;
+        pose_msg.header.stamp = ros::Time(current_ts.getSeconds(), current_ts.getNanoSeconds());
+
+        // 3D
+        if(sensor_ptr_->getProblem()->getDim() == 3)
+        {
+            auto pose_from_origin = SE3::between(origin_state, current_state);
+
+            pose_msg.pose.position.x    = pose_from_origin(0);
+            pose_msg.pose.position.y    = pose_from_origin(1);
+            pose_msg.pose.position.z    = pose_from_origin(2);
+            pose_msg.pose.orientation.x = pose_from_origin(3);
+            pose_msg.pose.orientation.y = pose_from_origin(4);
+            pose_msg.pose.orientation.z = pose_from_origin(5);
+            pose_msg.pose.orientation.w = pose_from_origin(6);
+        }
+        // 2D
+        else
+        {
+            auto pose_from_origin = SE2::between(origin_state, current_state);
+
+            auto q_from_origin = Eigen::Quaterniond(Eigen::AngleAxisd(pose_from_origin(2),
+                                                    Eigen::Vector3d::UnitZ()));
+
+            pose_msg.pose.position.x    = pose_from_origin(0);
+            pose_msg.pose.position.y    = pose_from_origin(1);
+            pose_msg.pose.position.z    = 0;
+            pose_msg.pose.orientation.x = q_from_origin.x();
+            pose_msg.pose.orientation.y = q_from_origin.y();
+            pose_msg.pose.orientation.z = q_from_origin.z();
+            pose_msg.pose.orientation.w = q_from_origin.w();
+        }
+
+        pub_pose_from_origin_.publish(pose_msg);
+    }
 }