Trajectory visualization (#360)

Based on code originally by @domeinzer.

cartographer_ros/CMakeLists.txt

@@ -32,6 +32,7 @@ set(PACKAGE_DEPENDENCIES
   tf2_eigen
   tf2_ros
   urdf
+  visualization_msgs
 )
 
 find_package(cartographer REQUIRED)

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -23,6 +23,8 @@
 
 namespace cartographer_ros {
 
+constexpr double kTrajectoryLineStripMarkerScale = 0.07;
+
 MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
                                    tf2_ros::Buffer* const tf_buffer)
     : node_options_(node_options),
@@ -192,6 +194,30 @@ MapBuilderBridge::GetTrajectoryStates() {
   return trajectory_states;
 }
 
+visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() {
+  visualization_msgs::MarkerArray trajectory_nodes_list;
+  const auto trajectory_nodes =
+      map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
+  int marker_id = 0;
+  for (const auto& single_trajectory : trajectory_nodes) {
+    visualization_msgs::Marker marker;
+    marker.id = marker_id++;
+    marker.type = visualization_msgs::Marker::LINE_STRIP;
+    marker.header.stamp = ::ros::Time::now();
+    marker.header.frame_id = node_options_.map_frame;
+    marker.color.b = 1.0;
+    marker.color.a = 1.0;
+    marker.scale.x = kTrajectoryLineStripMarkerScale;
+    marker.pose.orientation.w = 1.0;
+    for (const auto& node : single_trajectory) {
+      marker.points.push_back(ToGeometryMsgPoint(
+          (node.pose * node.constant_data->tracking_to_pose).translation()));
+    }
+    trajectory_nodes_list.markers.push_back(marker);
+  }
+  return trajectory_nodes_list;
+}
+
 SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
   return sensor_bridges_.at(trajectory_id).get();
 }

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -31,6 +31,7 @@
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "nav_msgs/OccupancyGrid.h"
+#include "visualization_msgs/MarkerArray.h"
 
 namespace cartographer_ros {
 
@@ -60,6 +61,7 @@ class MapBuilderBridge {
   cartographer_ros_msgs::SubmapList GetSubmapList();
   std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
   std::unordered_map<int, TrajectoryState> GetTrajectoryStates();
+  visualization_msgs::MarkerArray GetTrajectoryNodesList();
 
   SensorBridge* sensor_bridge(int trajectory_id);
 

cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -225,9 +225,7 @@ geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) {
 
 geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) {
   geometry_msgs::Pose pose;
-  pose.position.x = rigid3d.translation().x();
-  pose.position.y = rigid3d.translation().y();
-  pose.position.z = rigid3d.translation().z();
+  pose.position = ToGeometryMsgPoint(rigid3d.translation());
   pose.orientation.w = rigid3d.rotation().w();
   pose.orientation.x = rigid3d.rotation().x();
   pose.orientation.y = rigid3d.rotation().y();
@@ -235,4 +233,12 @@ geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) {
   return pose;
 }
 
+geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) {
+  geometry_msgs::Point point;
+  point.x = vector3d.x();
+  point.y = vector3d.y();
+  point.z = vector3d.z();
+  return point;
+}
+
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/msg_conversion.h

@@ -44,6 +44,8 @@ geometry_msgs::Transform ToGeometryMsgTransform(
 geometry_msgs::Pose ToGeometryMsgPose(
     const ::cartographer::transform::Rigid3d& rigid3d);
 
+geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d);
+
 ::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
     const sensor_msgs::LaserScan& msg);
 

cartographer_ros/cartographer_ros/node.cc

@@ -40,6 +40,7 @@
 #include "ros/serialization.h"
 #include "sensor_msgs/PointCloud2.h"
 #include "tf2_eigen/tf2_eigen.h"
+#include "visualization_msgs/MarkerArray.h"
 
 namespace cartographer_ros {
 
@@ -103,6 +104,9 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
   submap_list_publisher_ =
       node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
           kSubmapListTopic, kLatestOnlyPublisherQueueSize);
+  trajectory_nodes_list_publisher_ =
+      node_handle_.advertise<::visualization_msgs::MarkerArray>(
+          kTrajectoryNodesListTopic, kLatestOnlyPublisherQueueSize);
   service_servers_.push_back(node_handle_.advertiseService(
       kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
   service_servers_.push_back(node_handle_.advertiseService(
@@ -131,6 +135,9 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
   wall_timers_.push_back(node_handle_.createWallTimer(
       ::ros::WallDuration(node_options_.pose_publish_period_sec),
       &Node::PublishTrajectoryStates, this));
+  wall_timers_.push_back(node_handle_.createWallTimer(
+      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
+      &Node::PublishTrajectoryNodesList, this));
 }
 
 Node::~Node() {
@@ -220,6 +227,15 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
   }
 }
 
+void Node::PublishTrajectoryNodesList(
+    const ::ros::WallTimerEvent& unused_timer_event) {
+  carto::common::MutexLocker lock(&mutex_);
+  if (trajectory_nodes_list_publisher_.getNumSubscribers() > 0) {
+    trajectory_nodes_list_publisher_.publish(
+        map_builder_bridge_.GetTrajectoryNodesList());
+  }
+}
+
 void Node::SpinOccupancyGridThreadForever() {
   for (;;) {
     std::this_thread::sleep_for(std::chrono::milliseconds(1000));

cartographer_ros/cartographer_ros/node.h

@@ -51,6 +51,7 @@ constexpr char kSubmapListTopic[] = "submap_list";
 constexpr char kSubmapQueryServiceName[] = "submap_query";
 constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
 constexpr char kWriteAssetsServiceName[] = "write_assets";
+constexpr char kTrajectoryNodesListTopic[] = "trajectory_nodes_list";
 
 // Wires up ROS topics to SLAM.
 class Node {
@@ -90,6 +91,7 @@ class Node {
                          int trajectory_id);
   void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
   void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
+  void PublishTrajectoryNodesList(const ::ros::WallTimerEvent& timer_event);
   void SpinOccupancyGridThreadForever();
   bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
   bool ValidateTopicName(const ::cartographer_ros_msgs::SensorTopics& topics,
@@ -104,6 +106,7 @@ class Node {
 
   ::ros::NodeHandle node_handle_;
   ::ros::Publisher submap_list_publisher_;
+  ::ros::Publisher trajectory_nodes_list_publisher_;
   // These ros::ServiceServers need to live for the lifetime of the node.
   std::vector<::ros::ServiceServer> service_servers_;
   ::ros::Publisher scan_matched_point_cloud_publisher_;

cartographer_ros/cartographer_ros/node_options.cc

@@ -34,6 +34,8 @@ NodeOptions CreateNodeOptions(
       lua_parameter_dictionary->GetDouble("submap_publish_period_sec");
   options.pose_publish_period_sec =
       lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
+  options.trajectory_publish_period_sec =
+      lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec");
 
   return options;
 }

cartographer_ros/cartographer_ros/node_options.h

@@ -32,6 +32,7 @@ struct NodeOptions {
   double lookup_transform_timeout_sec;
   double submap_publish_period_sec;
   double pose_publish_period_sec;
+  double trajectory_publish_period_sec;
 };
 
 NodeOptions CreateNodeOptions(

cartographer_ros/configuration_files/backpack_2d.lua

@@ -30,6 +30,7 @@ options = {
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
 }
 
 MAP_BUILDER.use_trajectory_builder_2d = true

cartographer_ros/configuration_files/backpack_3d.lua

@@ -30,6 +30,7 @@ options = {
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
 }
 
 TRAJECTORY_BUILDER_3D.scans_per_accumulation = 160

cartographer_ros/configuration_files/demo_2d.rviz

@@ -170,6 +170,14 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /trajectory_nodes_list
+      Name: MarkerArray
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 100; 100; 100

cartographer_ros/configuration_files/demo_3d.rviz

@@ -232,6 +232,14 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /trajectory_nodes_list
+      Name: MarkerArray
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 100; 100; 100

cartographer_ros/configuration_files/pr2.lua

@@ -30,6 +30,7 @@ options = {
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
 }
 
 MAP_BUILDER.use_trajectory_builder_2d = true

cartographer_ros/configuration_files/revo_lds.lua

@@ -30,6 +30,7 @@ options = {
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
 }
 
 MAP_BUILDER.use_trajectory_builder_2d = true

cartographer_ros/configuration_files/taurob_tracker.lua

@@ -30,6 +30,7 @@ options = {
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
 }
 
 TRAJECTORY_BUILDER_3D.scans_per_accumulation = 180

cartographer_ros/package.xml

@@ -58,6 +58,7 @@
   <depend>tf2_eigen</depend>
   <depend>tf2_ros</depend>
   <depend>urdf</depend>
+  <depend>visualization_msgs</depend>
   <depend>yaml-cpp</depend>
 
   <test_depend>rosunit</test_depend>

docs/source/configuration.rst

@@ -81,6 +81,10 @@ pose_publish_period_sec
   Interval in seconds at which to publish poses, e.g. 5e-3 for a frequency of
   200 Hz.
 
+trajectory_publish_period_sec
+  Interval in seconds at which to publish the trajectory markers, e.g. 30e-3
+  for 30 milliseconds.
+
 .. _REP 105: http://www.ros.org/reps/rep-0105.html
 .. _ROS Names: http://wiki.ros.org/Names
 .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html