added loc coverage display tool for rviz

tools/localization_rviz_plugins/CMakeLists.txt

@@ -18,7 +18,7 @@
 cmake_minimum_required(VERSION 3.0)
 project(localization_rviz_plugins)
 
-if (FALSE AND BUILD_LOC_RVIZ_PLUGINS)
+if (BUILD_LOC_RVIZ_PLUGINS)
 
   ## Compile as C++14, supported in ROS Kinetic and newer
   add_compile_options(-std=c++14)
@@ -61,13 +61,14 @@ if (FALSE AND BUILD_LOC_RVIZ_PLUGINS)
 
   # Declare C++ libraries
   add_library(${PROJECT_NAME}
-    src/depth_odometry_display.cc
-    src/localization_graph_display.cc
-    src/localization_graph_panel.cc
-    src/pose_display.cc
-    src/utilities.cc
-    src/imu_augmentor_display.cc
-    src/slider_property.cc
+    #src/depth_odometry_display.cc
+    src/localization_coverage_display.cc
+    #src/localization_graph_display.cc
+    #src/localization_graph_panel.cc
+    #src/pose_display.cc
+    #src/utilities.cc
+    #src/imu_augmentor_display.cc
+    #src/slider_property.cc
     src/sparse_mapping_display.cc
   )
   add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})

tools/localization_rviz_plugins/nodelet_plugins.xml

@@ -6,6 +6,13 @@
       Depth Odometry Display
     </description>
   </class>
+  <class name="localization_rviz_plugins/LocalizationCoverageDisplay"
+         type="localization_rviz_plugins::LocalizationCoverageDisplay"
+         base_class_type="rviz::Display">
+    <description>
+      Localization Coverage Display
+    </description>
+  </class>
   <class name="localization_rviz_plugins/Graph"
          type="localization_rviz_plugins::LocalizationGraphDisplay"
          base_class_type="rviz::Display">

tools/localization_rviz_plugins/readme.md

@@ -16,6 +16,9 @@ The depth odometry display publishes source and target point clouds and correspo
 ## Imu Augmentor Display
 The imu augmentor display draws imu augmentor poses.  This is useful when comparing with graph localizer poses and sparse mapping poses, as ideally these are all alligned.
 
+## Localization Coverage Display 
+The localization coverage display draws map pose positions from a bagfile. To use this, use: export COVERAGE_BAG=/path/to/bagfile before launching rviz to set the bagfile to use for plotting. The bag file should have poses stored using the /sparse_mapping/pose topic, which is the output for the localization_analysis map matcher and offline replay tools for map matched poses. Subscribe to the output topic called /localization_coverage/cloud using the rviz PointCloud2 visualization plugin to view the pose positions.
+
 ## Localization Graph Display 
 The localization graph display draws the full history of poses in the latest graph localization message.  It also draws imu estimates between poses as arrows, and publishes optical flow feature track images using the feature tracks in the localizer.
 

tools/localization_rviz_plugins/src/localization_coverage_display.cc

@@ -0,0 +1,96 @@
+/* Copyright (c) 2017, United States Government, as represented by the
+ * Administrator of the National Aeronautics and Space Administration.
+ *
+ * All rights reserved.
+ *
+ * The Astrobee platform is licensed under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <localization_common/logger.h>
+#include <localization_common/utilities.h>
+
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+
+#include <rviz/frame_manager.h>
+#include <rviz/visualization_manager.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <string>
+
+#include "localization_coverage_display.h"  // NOLINT
+
+namespace localization_rviz_plugins {
+LocalizationCoverageDisplay::LocalizationCoverageDisplay() {
+  int ff_argc = 1;
+  char argv[] = "localization_coverage_display";
+  char* argv_ptr = &argv[0];
+  char** argv_ptr_ptr = &argv_ptr;
+  cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("localization_coverage/cloud", 1);
+  // Load positions from bagfile
+  const std::string coverage_bag = std::getenv("COVERAGE_BAG");
+  rosbag::Bag bag(coverage_bag, rosbag::bagmode::Read);
+  rosbag::View view(bag, rosbag::TopicQuery("/sparse_mapping/pose"));
+  for (const rosbag::MessageInstance msg : view) {
+    geometry_msgs::PoseStampedPtr pose_msg = msg.instantiate<geometry_msgs::PoseStamped>();
+    if (pose_msg) {
+      positions_.emplace_back(localization_common::PoseFromMsg(*pose_msg).translation().cast<float>());
+    }
+  }
+}
+
+void LocalizationCoverageDisplay::onInitialize() { drawMap(); }
+
+void LocalizationCoverageDisplay::drawMap() {
+  // TODO(rsoussan): Use pcl point cloud when pcl_ros dependency added
+  sensor_msgs::PointCloud2 cloud;
+  cloud.header = std_msgs::Header();
+  cloud.header.frame_id = "world";
+  cloud.height = 1;
+  cloud.width = positions_.size();
+  cloud.fields.resize(3);
+  cloud.fields[0].name = "x";
+  cloud.fields[0].offset = 0;
+  cloud.fields[0].datatype = 7;
+  cloud.fields[0].count = 1;
+  cloud.fields[1].name = "y";
+  cloud.fields[1].offset = 4;
+  cloud.fields[1].datatype = 7;
+  cloud.fields[1].count = 1;
+  cloud.fields[2].name = "z";
+  cloud.fields[2].offset = 8;
+  cloud.fields[2].datatype = 7;
+  cloud.fields[2].count = 1;
+  cloud.is_bigendian = false;
+  cloud.point_step = 12;
+  cloud.row_step = cloud.point_step * cloud.width;
+  cloud.is_dense = true;
+  cloud.data.resize(cloud.row_step);
+
+  for (int i = 0; i < static_cast<int>(cloud.width); ++i) {
+    const Eigen::Vector3f& point = positions_[i];
+    memcpy(&cloud.data[cloud.point_step * i + 0], &point.x(), 4);
+    memcpy(&cloud.data[cloud.point_step * i + 4], &point.y(), 4);
+    memcpy(&cloud.data[cloud.point_step * i + 8], &point.z(), 4);
+  }
+
+  cloud.header.stamp = ros::Time::now();
+  // TODO(rsoussan): Use ros point_cloud_common instead of publishing when rviz_default_plugin linker issue fixed
+  cloud_publisher_.publish(cloud);
+}
+
+void LocalizationCoverageDisplay::reset() {}
+}  // namespace localization_rviz_plugins
+
+#include <pluginlib/class_list_macros.h>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(localization_rviz_plugins::LocalizationCoverageDisplay, rviz::Display)

tools/localization_rviz_plugins/src/localization_coverage_display.h

@@ -0,0 +1,61 @@
+/* Copyright (c) 2017, United States Government, as represented by the
+ * Administrator of the National Aeronautics and Space Administration.
+ *
+ * All rights reserved.
+ *
+ * The Astrobee platform is licensed under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ */
+
+// Header file must go in src directory for Qt/Rviz plugin
+#ifndef LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_COVERAGE_DISPLAY_H_  // NOLINT
+#define LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_COVERAGE_DISPLAY_H_  // NOLINT
+
+// Required for Qt
+#ifndef Q_MOC_RUN
+#include <ros/node_handle.h>
+#include <ros/publisher.h>
+#include <rviz/display.h>
+#endif
+
+#include <rosbag/view.h>
+#include <Eigen/Geometry>
+#include <vector>
+
+// Forward declarations for ogre and rviz
+namespace Ogre {
+class SceneNode;
+}
+
+namespace localization_rviz_plugins {
+
+class LocalizationCoverageDisplay : public rviz::Display {
+  Q_OBJECT    // NOLINT
+    public :  // NOLINT
+              LocalizationCoverageDisplay();
+  ~LocalizationCoverageDisplay() = default;
+
+ protected:
+  void onInitialize() final;
+  void reset() final;
+
+ private:
+  void drawMap();
+
+  // TODO(rosussan): Remove publishing and use point_cloud_common from rviz/default_plugins
+  // when linking error is fixed
+  std::vector<Eigen::Vector3f> positions_;
+  ros::Publisher cloud_publisher_;
+  ros::NodeHandle nh_;
+};
+}  // namespace localization_rviz_plugins
+#endif  // LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_COVERAGE_DISPLAY_H_ NOLINT