From 7b9e2fe2e13acafb0b448ca4b0fee9be5e71942d Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Mon, 12 Feb 2018 09:55:24 +0100
Subject: [PATCH] RViz settings for landmarks. (#717)

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
---
 cartographer_ros/cartographer_ros/map_builder_bridge.cc | 8 ++++++--
 cartographer_ros/configuration_files/demo_2d.rviz       | 8 ++++++++
 cartographer_ros/configuration_files/demo_3d.rviz       | 8 ++++++++
 3 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
index d55f387..121e21a 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -30,6 +30,7 @@ namespace {
 using ::cartographer::transform::Rigid3d;
 
 constexpr double kTrajectoryLineStripMarkerScale = 0.07;
+constexpr double kLandmarkMarkerScale = 0.3;
 constexpr double kConstraintMarkerScale = 0.025;
 
 ::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) {
@@ -62,7 +63,7 @@ int GetLandmarkIndex(
   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;
+    landmark_id_to_index->emplace(landmark_id, new_index);
     return new_index;
   }
   return it->second;
@@ -74,9 +75,12 @@ visualization_msgs::Marker CreateLandmarkMarker(int landmark_index,
   visualization_msgs::Marker marker;
   marker.ns = "Landmarks";
   marker.id = landmark_index;
-  marker.type = visualization_msgs::Marker::SPHERE;
+  marker.type = visualization_msgs::Marker::CUBE;
   marker.header.stamp = ::ros::Time::now();
   marker.header.frame_id = frame_id;
+  marker.scale.x = kLandmarkMarkerScale;
+  marker.scale.y = kLandmarkMarkerScale;
+  marker.scale.z = kLandmarkMarkerScale;
   marker.color = ToMessage(cartographer::io::GetColor(landmark_index));
   marker.pose = ToGeometryMsgPose(landmark_pose);
   return marker;
diff --git a/cartographer_ros/configuration_files/demo_2d.rviz b/cartographer_ros/configuration_files/demo_2d.rviz
index f09a8d2..dd4d746 100644
--- a/cartographer_ros/configuration_files/demo_2d.rviz
+++ b/cartographer_ros/configuration_files/demo_2d.rviz
@@ -177,6 +177,14 @@ Visualization Manager:
         "": true
       Queue Size: 100
       Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /landmark_poses_list
+      Name: Landmark Poses
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 100; 100; 100
diff --git a/cartographer_ros/configuration_files/demo_3d.rviz b/cartographer_ros/configuration_files/demo_3d.rviz
index 89e4f42..6beebca 100644
--- a/cartographer_ros/configuration_files/demo_3d.rviz
+++ b/cartographer_ros/configuration_files/demo_3d.rviz
@@ -239,6 +239,14 @@ Visualization Manager:
         "": true
       Queue Size: 100
       Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /landmark_poses_list
+      Name: Landmark Poses
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 100; 100; 100