pose publisher trajectory marker and/or pose array

include/publisher_pose.h

@@ -8,12 +8,25 @@
 
 #include "publisher.h"
 
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <geometry_msgs/PoseArray.h>
+#include <visualization_msgs/Marker.h>
+
 namespace wolf
 {
 
 class PublisherPose: public Publisher
 {
-        std::string map_frame_id_;
+        bool pose_array_, marker_;
+
+        geometry_msgs::PoseArray pose_array_msg_;
+        visualization_msgs::Marker marker_msg_;
+        std_msgs::ColorRGBA marker_color_;
+
+        ros::Publisher pub_pose_array_, pub_marker_;
 
     public:
         PublisherPose(const std::string& _unique_name,
@@ -26,6 +39,8 @@ class PublisherPose: public Publisher
         void initialize(ros::NodeHandle &nh, const std::string& topic) override;
 
         void publishDerived() override;
+
+        void publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp);
 };
 
 WOLF_REGISTER_PUBLISHER(PublisherPose)

src/publisher_pose.cpp

@@ -5,7 +5,6 @@
  **************************/
 #include <ros/ros.h>
 #include "tf/transform_datatypes.h"
-#include <nav_msgs/Odometry.h>
 
 namespace wolf
 {
@@ -15,12 +14,42 @@ PublisherPose::PublisherPose(const std::string& _unique_name,
                              const ProblemPtr _problem) :
         Publisher(_unique_name, _server, _problem)
 {
+    pose_array_ = _server.getParam<bool>(prefix_ + "/pose_array_msg");
+    marker_     = _server.getParam<bool>(prefix_ + "/marker_msg");
+    if (marker_)
+    {
+        Eigen::Vector4d col = _server.getParam<Eigen::Vector4d>(prefix_ + "/marker_color");
+        marker_color_.r = col(0);
+        marker_color_.g = col(1);
+        marker_color_.b = col(2);
+        marker_color_.a = col(3);
+    }
 }
 
 void PublisherPose::initialize(ros::NodeHandle& nh, const std::string& topic)
 {
-    publisher_ = nh.advertise<nav_msgs::Odometry>(topic, 1);
-    nh.param<std::string>("map_frame_id", map_frame_id_, "map");
+    std::string map_frame_id;
+    nh.param<std::string>("map_frame_id", map_frame_id, "map");
+
+    // initialize msg and publisher
+    if (pose_array_)
+    {
+        pose_array_msg_.header.frame_id = map_frame_id;
+
+        pub_pose_array_ = nh.advertise<geometry_msgs::PoseArray>(topic + "_pose_array", 1);
+    }
+    if (marker_)
+    {
+        marker_msg_.header.frame_id = map_frame_id;
+        marker_msg_.type = visualization_msgs::Marker::LINE_STRIP;
+        marker_msg_.action = visualization_msgs::Marker::ADD;
+        marker_msg_.ns = "trajectory";
+        marker_msg_.scale.x = 0.1;
+        marker_msg_.color = marker_color_;
+        marker_msg_.pose.orientation = tf::createQuaternionMsgFromYaw(0);
+
+        pub_marker_ = nh.advertise<visualization_msgs::Marker>(topic + "_marker", 1);
+    }
 }
 
 void PublisherPose::publishDerived()
@@ -36,34 +65,51 @@ void PublisherPose::publishDerived()
         return;
     }
 
-    // Fill PoseStamped msg
-    nav_msgs::Odometry msg;
-    msg.header.frame_id = map_frame_id_;
-    msg.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds());
+    // Fill Pose msg
+    geometry_msgs::Pose pose_msg;
 
     // 2D
     if (problem_->getDim() == 2)
     {
-        msg.pose.pose.position.x = current_state['P'](0);
-        msg.pose.pose.position.y = current_state['P'](1);
-        msg.pose.pose.position.z = 0;
+        pose_msg.position.x = current_state['P'](0);
+        pose_msg.position.y = current_state['P'](1);
+        pose_msg.position.z = 0;
 
-        msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(current_state['O'](0));
+        pose_msg.orientation = tf::createQuaternionMsgFromYaw(current_state['O'](0));
     }
     // 3D
     else
     {
-        msg.pose.pose.position.x = current_state['P'](0);
-        msg.pose.pose.position.y = current_state['P'](1);
-        msg.pose.pose.position.z = current_state['P'](2);
-
-        msg.pose.pose.orientation.x = current_state['O'](0);
-        msg.pose.pose.orientation.y = current_state['O'](1);
-        msg.pose.pose.orientation.z = current_state['O'](2);
-        msg.pose.pose.orientation.w = current_state['O'](3);
+        pose_msg.position.x = current_state['P'](0);
+        pose_msg.position.y = current_state['P'](1);
+        pose_msg.position.z = current_state['P'](2);
+
+        pose_msg.orientation.x = current_state['O'](0);
+        pose_msg.orientation.y = current_state['O'](1);
+        pose_msg.orientation.z = current_state['O'](2);
+        pose_msg.orientation.w = current_state['O'](3);
     }
 
-    publisher_.publish(msg);
+    publishPose(pose_msg, ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds()));
+}
+
+void PublisherPose::publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp)
+{
+    // fill msgs and publish
+    if (pose_array_)
+    {
+        pose_array_msg_.header.stamp = stamp;
+        pose_array_msg_.poses.push_back(pose);
+
+        pub_pose_array_.publish(pose_array_msg_);
+    }
+    if (marker_)
+    {
+        marker_msg_.header.stamp = stamp;
+        marker_msg_.points.push_back(pose.position);
+
+        pub_marker_.publish(marker_msg_);
+    }
 }
 
 }