Publish Landmark markers for RViz. (#713)

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -25,9 +25,10 @@
 #include "cartographer_ros_msgs/StatusResponse.h"
 
 namespace cartographer_ros {
-
 namespace {
 
+using ::cartographer::transform::Rigid3d;
+
 constexpr double kTrajectoryLineStripMarkerScale = 0.07;
 constexpr double kConstraintMarkerScale = 0.025;
 
@@ -55,6 +56,32 @@ visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id,
   return marker;
 }
 
+int GetLandmarkIndex(
+    const std::string& landmark_id,
+    std::unordered_map<std::string, int>* landmark_id_to_index) {
+  auto it = landmark_id_to_index->find(landmark_id);
+  if (it == landmark_id_to_index->end()) {
+    const int new_index = landmark_id_to_index->size();
+    landmark_id_to_index->at(landmark_id) = new_index;
+    return new_index;
+  }
+  return it->second;
+}
+
+visualization_msgs::Marker CreateLandmarkMarker(int landmark_index,
+                                                const Rigid3d& landmark_pose,
+                                                const std::string& frame_id) {
+  visualization_msgs::Marker marker;
+  marker.ns = "Landmarks";
+  marker.id = landmark_index;
+  marker.type = visualization_msgs::Marker::SPHERE;
+  marker.header.stamp = ::ros::Time::now();
+  marker.header.frame_id = frame_id;
+  marker.color = ToMessage(cartographer::io::GetColor(landmark_index));
+  marker.pose = ToGeometryMsgPose(landmark_pose);
+  return marker;
+}
+
 void PushAndResetLineMarker(visualization_msgs::Marker* marker,
                             std::vector<visualization_msgs::Marker>* markers) {
   if (marker->points.size() > 1) {
@@ -233,6 +260,18 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
   return trajectory_node_list;
 }
 
+visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() {
+  visualization_msgs::MarkerArray landmark_poses_list;
+  const std::map<std::string, Rigid3d> landmark_poses =
+      map_builder_->pose_graph()->GetLandmarkPoses();
+  for (const auto& id_to_pose : landmark_poses) {
+    landmark_poses_list.markers.push_back(CreateLandmarkMarker(
+        GetLandmarkIndex(id_to_pose.first, &landmark_to_index_),
+        id_to_pose.second, node_options_.map_frame));
+  }
+  return landmark_poses_list;
+}
+
 visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
   visualization_msgs::MarkerArray constraint_list;
   int marker_id = 0;
@@ -336,8 +375,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
       continue;
     }
     const auto& trajectory_node_pose = node_it->data.global_pose;
-    const cartographer::transform::Rigid3d constraint_pose =
+    const Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij;
-        submap_pose * constraint.pose.zbar_ij;
 
     constraint_marker->points.push_back(
         ToGeometryMsgPoint(submap_pose.translation()));
@@ -365,7 +403,7 @@ SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
 
 void MapBuilderBridge::OnLocalSlamResult(
     const int trajectory_id, const ::cartographer::common::Time time,
-    const ::cartographer::transform::Rigid3d local_pose,
+    const Rigid3d local_pose,
     ::cartographer::sensor::RangeData range_data_in_local,
     const std::unique_ptr<const ::cartographer::mapping::
                               TrajectoryBuilderInterface::InsertionResult>) {

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -81,6 +81,7 @@ class MapBuilderBridge {
   std::unordered_map<int, TrajectoryState> GetTrajectoryStates()
       EXCLUDES(mutex_);
   visualization_msgs::MarkerArray GetTrajectoryNodeList();
+  visualization_msgs::MarkerArray GetLandmarkPosesList();
   visualization_msgs::MarkerArray GetConstraintList();
 
   SensorBridge* sensor_bridge(int trajectory_id);
@@ -101,6 +102,8 @@ class MapBuilderBridge {
   std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
   tf2_ros::Buffer* const tf_buffer_;
 
+  std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;
+
   // These are keyed with 'trajectory_id'.
   std::unordered_map<int, TrajectoryOptions> trajectory_options_;
   std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;

cartographer_ros/cartographer_ros/node.cc

@@ -94,6 +94,9 @@ Node::Node(
   trajectory_node_list_publisher_ =
       node_handle_.advertise<::visualization_msgs::MarkerArray>(
           kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
+  landmark_poses_list_publisher_ =
+      node_handle_.advertise<::visualization_msgs::MarkerArray>(
+          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
   constraint_list_publisher_ =
       node_handle_.advertise<::visualization_msgs::MarkerArray>(
           kConstraintListTopic, kLatestOnlyPublisherQueueSize);
@@ -119,6 +122,9 @@ Node::Node(
   wall_timers_.push_back(node_handle_.createWallTimer(
       ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
       &Node::PublishTrajectoryNodeList, this));
+  wall_timers_.push_back(node_handle_.createWallTimer(
+      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
+      &Node::PublishLandmarkPosesList, this));
   wall_timers_.push_back(node_handle_.createWallTimer(
       ::ros::WallDuration(kConstraintPublishPeriodSec),
       &Node::PublishConstraintList, this));
@@ -250,6 +256,15 @@ void Node::PublishTrajectoryNodeList(
   }
 }
 
+void Node::PublishLandmarkPosesList(
+    const ::ros::WallTimerEvent& unused_timer_event) {
+  carto::common::MutexLocker lock(&mutex_);
+  if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
+    landmark_poses_list_publisher_.publish(
+        map_builder_bridge_.GetLandmarkPosesList());
+  }
+}
+
 void Node::PublishConstraintList(
     const ::ros::WallTimerEvent& unused_timer_event) {
   carto::common::MutexLocker lock(&mutex_);

cartographer_ros/cartographer_ros/node.h

@@ -151,6 +151,7 @@ class Node {
   void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
   void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
   void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
+  void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
   void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
   void SpinOccupancyGridThreadForever();
   bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
@@ -169,6 +170,7 @@ class Node {
   ::ros::NodeHandle node_handle_;
   ::ros::Publisher submap_list_publisher_;
   ::ros::Publisher trajectory_node_list_publisher_;
+  ::ros::Publisher landmark_poses_list_publisher_;
   ::ros::Publisher constraint_list_publisher_;
   // These ros::ServiceServers need to live for the lifetime of the node.
   std::vector<::ros::ServiceServer> service_servers_;

cartographer_ros/cartographer_ros/node_constants.h

@@ -37,6 +37,7 @@ constexpr char kSubmapQueryServiceName[] = "submap_query";
 constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
 constexpr char kWriteStateServiceName[] = "write_state";
 constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
+constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
 constexpr char kConstraintListTopic[] = "constraint_list";
 constexpr double kConstraintPublishPeriodSec = 0.5;