pose_transform implementation

include/odometry_transformer/odometry_transformer.h

@@ -6,7 +6,10 @@
 
 #include <Eigen/Geometry>
 #include <dynamic_reconfigure/server.h>
+
 #include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
 #include <ros/ros.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 
@@ -23,6 +26,7 @@ class OdometryTransformer {
 
 private:
   void receiveOdometry(const nav_msgs::OdometryConstPtr &source_odometry);
+  void receivePose(const geometry_msgs::PoseWithCovarianceStampedPtr &source_odometry);
   void broadcastCalibration();
 
   void initializeDynamicReconfigure();
@@ -33,6 +37,7 @@ class OdometryTransformer {
   std::string target_frame_ = "";
   int queue_size_ = 1;
   bool tcp_no_delay_ = false;
+  bool pose_transform_ = false;
 
   // Optionally publish TF and offer dynamic reconfigure if calibration is set
   // from ROS parameter server.
@@ -49,6 +54,9 @@ class OdometryTransformer {
 
   ros::Subscriber odometry_sub_;
   ros::Publisher odometry_pub_;
+
+  ros::Subscriber pose_sub_;
+  ros::Publisher pose_pub_;
 };
 } // namespace odometry_transformer
 

launch/odometry_transformer.launch

@@ -3,6 +3,14 @@
   <arg name="source_odometry" default="source_odometry"/>
   <!-- The desired odometry target topic -->
   <arg name="target_odometry" default="target_odometry"/>
+
+  <!-- Additionally convert Pose message (PoseWithCovarianceStamped) from the same source to target fragem -->
+  <arg name="pose_transform" default="false"/>
+  <!-- The original pose source topic -->
+  <arg name="source_pose" default="source_pose"/>
+  <!-- The desired pose target topic -->
+  <arg name="target_pose" default="target_pose"/>
+
   <!-- The original odometry source frame id. -->
   <arg name="source_frame" default="source"/>
   <!-- The desired odometry target frame id. -->
@@ -26,6 +34,9 @@
     <remap from="source_odometry" to="$(arg source_odometry)"/>
     <remap from="target_odometry" to="$(arg target_odometry)"/>
 
+    <remap from="source_pose" to="$(arg source_pose)"/>
+    <remap from="target_pose" to="$(arg target_pose)"/>
+
     <param name="source_frame" value="$(arg source_frame)"/>
     <param name="target_frame" value="$(arg target_frame)"/>
 
@@ -36,5 +47,7 @@
     <param name="queue_size" value="$(arg queue_size)"/>
     <param name="tcp_no_delay" value="$(arg tcp_no_delay)"/>
 
+    <param name="pose_transform" value="$(arg pose_transform)"/>
+
   </node>
 </launch>

src/odometry_transformer.cc

@@ -85,6 +85,14 @@ void OdometryTransformer::getRosParameters() {
   } else {
     ROS_INFO("TCP no delay de-activated.");
   }
+
+  // Pose transform as well
+  nh_private_.getParam("pose_transform", pose_transform_);
+  if (pose_transform_) {
+    ROS_INFO("Pose transform activated.");
+  } else {
+    ROS_INFO("Pose transform de-activated.");
+  }
 }
 
 void OdometryTransformer::subscribeToRosTopics() {
@@ -96,12 +104,27 @@ void OdometryTransformer::subscribeToRosTopics() {
                                 ? ros::TransportHints().tcpNoDelay()
                                 : ros::TransportHints());
   ROS_INFO("Subscribed to %s", odometry_sub_.getTopic().c_str());
+
+  if(pose_transform_){
+      pose_sub_ = nh_.subscribe("source_pose",
+                                queue_size_,
+                                &OdometryTransformer::receivePose,
+                                this,
+                                tcp_no_delay_
+                                ? ros::TransportHints().tcpNoDelay()
+                                : ros::TransportHints());
+  ROS_INFO("Subscribed to %s", pose_sub_.getTopic().c_str());
+  }
 }
 
 void OdometryTransformer::advertiseRosTopics() {
   odometry_pub_ =
       nh_.advertise<nav_msgs::Odometry>("target_odometry", queue_size_);
   ROS_INFO("Advertising %s", odometry_pub_.getTopic().c_str());
+
+  pose_pub_ =
+      nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("target_pose", queue_size_);
+  ROS_INFO("Advertising %s", pose_pub_.getTopic().c_str());
 }
 
 void OdometryTransformer::broadcastCalibration() {
@@ -213,4 +236,26 @@ void OdometryTransformer::receiveOdometry(
   odometry_pub_.publish(target_odometry);
 }
 
+void OdometryTransformer::receivePose(
+    const geometry_msgs::PoseWithCovarianceStampedPtr &source_pose) {
+  ROS_INFO_ONCE("Received first pose message.");
+
+  // Get source pose in inertial/world coordinate frame.
+  Eigen::Affine3d T_IS;
+  Eigen::fromMsg(source_pose->pose.pose, T_IS);
+
+  // Compute pose of target frame.
+  const Eigen::Affine3d T_IT = T_IS * T_ST_;
+
+  // Convert to target odometry.
+  geometry_msgs::PoseWithCovarianceStamped target_pose;
+  target_pose.header = source_pose->header;
+  target_pose.pose.pose = Eigen::toMsg(T_IT);
+
+  // TODO(TimonMathis): Transform covariance.
+
+  // Publish transformed odometry.
+  pose_pub_.publish(target_pose);
+}
+
 } // namespace odometry_transformer

test/launch/test_pose_transform.launch

@@ -0,0 +1,53 @@
+<launch>
+  <!-- The original odometry source topic -->
+  <arg name="source_odometry" default="/msf_core/odometry"/>
+  <!-- The desired odometry target topic -->
+  <arg name="target_odometry" default="/msf_core/odometry_prism"/>
+
+  <!-- Additionally convert Pose message (PoseWithCovarianceStamped) from the same source to target fragem -->
+  <arg name="pose_transform" default="true"/>
+  <!-- The original pose source topic -->
+  <arg name="source_pose" default="/msf_core/pose"/>
+  <!-- The desired pose target topic -->
+  <arg name="target_pose" default="/msf_core/pose_prism"/>
+
+  <!-- The original odometry source frame id. -->
+  <arg name="source_frame" default="state"/>
+  <!-- The desired odometry target frame id. -->
+  <arg name="target_frame" default="prism"/>
+  <!-- The static calibration from the target frame to the source frame in the target coordinate frame. -->
+  <!-- Rotation from target frame to source frame. Quaternion as [x,y,z,w] -->
+  <!-- See also https://www.andre-gaschler.com/rotationconverter/ -->
+  <arg name="q_TS" default="[0.0, 0.0, 0.0, 1.0]"/>
+  <!-- Translation from target frame to source frame. -->
+  <arg name="T_r_TS" default="[0.0, 0.0, 0.0]"/>
+  <!-- Instead of parametric calibration, the node can lookup the sensor calibration from TF. -->
+  <arg name="lookup_tf" default="true"/>
+  <!-- Queue size for odometry messages. -->
+  <arg name="queue_size" default="1"/>
+  <!-- Use tcpNoDelay() to subscribe to source odometry. Uses more CPU! -->
+  <!-- This is necessary to avoid message drops at high rate. It is not necessary when using nodelets or low rate odometry. -->
+  <arg name="tcp_no_delay" default="true" />
+
+  <node name="odometry_transformer" pkg="odometry_transformer" type="odometry_transformer_node" output="screen">
+
+    <remap from="source_odometry" to="$(arg source_odometry)"/>
+    <remap from="target_odometry" to="$(arg target_odometry)"/>
+
+    <remap from="source_pose" to="$(arg source_pose)"/>
+    <remap from="target_pose" to="$(arg target_pose)"/>
+
+    <param name="source_frame" value="$(arg source_frame)"/>
+    <param name="target_frame" value="$(arg target_frame)"/>
+
+    <rosparam param="q_TS" subst_value="True">$(arg q_TS)</rosparam>
+    <rosparam param="T_r_TS" subst_value="True">$(arg T_r_TS)</rosparam>
+
+    <param name="lookup_tf" value="$(arg lookup_tf)"/>
+    <param name="queue_size" value="$(arg queue_size)"/>
+    <param name="tcp_no_delay" value="$(arg tcp_no_delay)"/>
+
+    <param name="pose_transform" value="$(arg pose_transform)"/>
+
+  </node>
+</launch>