Merge pull request #20 from marip8/fix/visualization-speed

Fixed speed of loading interactive markers

include/reach_ros/display/ros_display.h

@@ -41,6 +41,8 @@ class ROSDisplay : public reach::Display
   const std::string kinematic_base_frame_;
   const double marker_scale_;
   const bool use_full_color_range_;
+
+  mutable reach::ReachResult db_;
   visualization_msgs::Marker collision_marker_;
 
   // ROS comoponents

src/display/ros_display.cpp

@@ -62,10 +62,17 @@ void ROSDisplay::showResults(const reach::ReachResult& db) const
 {
   server_.clear();
 
+  // Copy the reach result once into a mutable member variable in order to use it within the lambda
+  // without having to copy it into every instance of the lambda (which gets expensive for > 300 points)
+  db_ = db;
+
   // Create a callback for when a marker is clicked on
-  auto show_goal_cb = [this, db](const visualization_msgs::InteractiveMarkerFeedbackConstPtr& fb) {
-    std::size_t idx = std::strtoul(fb->marker_name.c_str(), nullptr, 10);
-    updateRobotPose(db.at(idx).goal_state);
+  auto show_goal_cb = [this](const visualization_msgs::InteractiveMarkerFeedbackConstPtr& fb) {
+    if (fb->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP)
+    {
+      std::size_t idx = std::strtoul(fb->marker_name.c_str(), nullptr, 10);
+      updateRobotPose(db_.at(idx).goal_state);
+    }
   };
 
   Eigen::MatrixX3f heatmap_colors = reach::computeHeatMapColors(db, use_full_color_range_);