methods of publish output

aicp_core/include/aicp_utils/visualizer.hpp

@@ -39,6 +39,20 @@ public:
                               std::string name,
                               int64_t utime = -1) = 0;
 
+    virtual void publishOdomPoses(Eigen::Isometry3d pose_,
+                              int param,
+                              std::string name,
+                              int64_t utime = -1) = 0;
+
+    virtual void publishPriorPoses(Eigen::Isometry3d pose_,
+                              int param,
+                              std::string name,
+                              int64_t utime = -1) = 0;
+
+    virtual void publishOdomToMapPose(Eigen::Isometry3d pose_,
+                              int64_t utime = -1) = 0;
+
+
     // Gets
     virtual const PathPoses& getPath() = 0;
 

aicp_ros/include/aicp_ros/visualizer_ros.hpp

@@ -51,6 +51,19 @@ public:
     void publishPoses(PathPoses poses,
                       int param, std::string name, int64_t utime);
 
+    void publishOdomPoses(Eigen::Isometry3d pose,
+                      int param, std::string name, int64_t utime);
+    void publishOdomPoses(PathPoses poses,
+                      int param, std::string name, int64_t utime);
+
+    void publishPriorPoses(Eigen::Isometry3d pose,
+                      int param, std::string name, int64_t utime);
+    void publishPriorPoses(PathPoses poses,
+                      int param, std::string name, int64_t utime);
+
+    void publishOdomToMapPose(Eigen::Isometry3d pose, int64_t utime);
+
+
     // Publish tf from fixed_frame to odom
     void publishFixedFrameToOdomTF(const Eigen::Isometry3d& fixed_frame_to_base_eigen,
                                    ros::Time msg_time);
@@ -68,11 +81,18 @@ private:
     ros::Publisher prior_map_pub_;
     ros::Publisher aligned_map_pub_;
     ros::Publisher pose_pub_;
+    ros::Publisher odom_pose_pub_;
+    ros::Publisher prior_pose_pub_;
+
     ros::Publisher fixed_to_odom_pub_;
+    ros::Publisher odom_to_map_pub_;
+    
     // Duplicates the list in collections renderer. assumed to be 3xN colors
     std::vector<double> colors_;
     // Path (vector of poses)
     PathPoses path_;
+    PathPoses odom_path_;
+    PathPoses prior_path_;
 
     std::string fixed_frame_; // map or map_test
     std::string odom_frame_;

aicp_ros/src/visualizer_ros.cpp

@@ -18,7 +18,13 @@ ROSVisualizer::ROSVisualizer(ros::NodeHandle& nh, string fixed_frame) : nh_(nh),
     prior_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/prior_map", 10);
     aligned_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/aicp/aligned_map", 10);
     pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/poses",100);
+    odom_pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/odom_poses",100);
+    prior_pose_pub_ = nh_.advertise<nav_msgs::Path>("/aicp/prior_poses",100);
+
     fixed_to_odom_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(fixed_to_odom_prefix_ + fixed_frame_ + "_to_odom", 10);
+
+    odom_to_map_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/icp_tools/map_pose", 10);
+
     colors_ = {
          51/255.0, 160/255.0, 44/255.0,  //0
          166/255.0, 206/255.0, 227/255.0,
@@ -153,6 +159,72 @@ void ROSVisualizer::publishPoses(PathPoses poses, int param, string name, int64_
     pose_pub_.publish(path_msg);
 }
 
+
+void ROSVisualizer::publishOdomPoses(Eigen::Isometry3d pose, int param, std::string name, int64_t utime)
+{
+    odom_path_.push_back(pose);
+    publishOdomPoses(odom_path_, param, name, utime);
+}
+
+void ROSVisualizer::publishOdomPoses(PathPoses poses, int param, string name, int64_t utime){
+
+    nav_msgs::Path path_msg;
+    int secs = utime*1E-6;
+    int nsecs = (utime - (secs*1E6))*1E3;
+    path_msg.header.stamp = ros::Time(secs, nsecs);
+    path_msg.header.frame_id = fixed_frame_;
+
+    for (size_t i = 0; i < poses.size(); ++i){
+        geometry_msgs::PoseStamped m;
+        m.header.stamp = ros::Time(secs, nsecs);
+        m.header.frame_id = fixed_frame_;
+        tf::poseEigenToMsg(poses[i], m.pose);
+        path_msg.poses.push_back(m);
+    }
+
+    odom_pose_pub_.publish(path_msg);
+}
+
+
+void ROSVisualizer::publishPriorPoses(Eigen::Isometry3d pose, int param, std::string name, int64_t utime)
+{
+    prior_path_.push_back(pose);
+    publishPriorPoses(odom_path_, param, name, utime);
+}
+
+void ROSVisualizer::publishPriorPoses(PathPoses poses, int param, string name, int64_t utime){
+
+    nav_msgs::Path path_msg;
+    int secs = utime*1E-6;
+    int nsecs = (utime - (secs*1E6))*1E3;
+    path_msg.header.stamp = ros::Time(secs, nsecs);
+    path_msg.header.frame_id = fixed_frame_;
+
+    for (size_t i = 0; i < poses.size(); ++i){
+        geometry_msgs::PoseStamped m;
+        m.header.stamp = ros::Time(secs, nsecs);
+        m.header.frame_id = fixed_frame_;
+        tf::poseEigenToMsg(poses[i], m.pose);
+        path_msg.poses.push_back(m);
+    }
+
+    prior_pose_pub_.publish(path_msg);
+}
+
+
+void ROSVisualizer::publishOdomToMapPose(Eigen::Isometry3d pose, int64_t utime)
+{
+    int secs = utime*1E-6;
+    int nsecs = (utime - (secs*1E6))*1E3;
+
+    geometry_msgs::PoseWithCovarianceStamped msg;
+    msg.header.stamp = ros::Time(secs, nsecs);
+    msg.header.frame_id = "odom";
+    tf::poseEigenToMsg(pose, msg.pose.pose);
+    odom_to_map_pub_.publish(msg);
+}
+
+
 void ROSVisualizer::publishFixedFrameToOdomPose(const Eigen::Isometry3d &fixed_frame_to_base_eigen,
                                                 ros::Time msg_time)
 {