Merge pull request #894 from k-okada/joachim-p/master

add more visualization options for BoundingBox and BoundingBoxArray #844

doc/jsk_rviz_plugins/plugins/bounding_box_array.rst

@@ -7,3 +7,37 @@ What is this?
 Visualize ``jsk_recognition_msgs/BoundingBoxArray.msg``.
 
 .. image:: images/bounding_box_array.png
+
+Properties
+**********
+
+.. image:: images/bounding_box_array_rviz.gif
+
+
+- `Topic`
+
+  Name of topic of `jsk_recognition_msgs/BoundingBoxArray`
+
+- `coloring`
+
+  `Flat Color` applies the same color to all boxes. `Label` uses the
+  label value of each box for coloring, and `Value` applies the alpha
+  value from the box message.
+
+- `alpha method`
+
+  `flat` applies the same alpha value to all boxes. You can set this
+  value in the `alpha` field. `value` uses the box message data, and the
+  alpha can be scaled by setting the `alpha min` and `alpha max` fields.
+
+- `only edge`
+
+  If it is true, only the edges of the boxes will be displayed.
+
+- `show coords`
+
+  If it is true, the coordinates of the boxes will be displayed.
+
+- `value threshold`
+
+  Display only boxes with alpha value avobe this threshold.

jsk_rqt_plugins/CMakeLists.txt

@@ -18,17 +18,34 @@ catkin_package()
 # recursively into ${CATKIN_PACKAGE_SHARE_DESTINATION}.
 # Be careful that 'launch' and 'launch/' are different: the former is directory
 # and the latter is each content.
-install(DIRECTORY bin/
+file(GLOB PYTHON_SCRIPTS bin/*)
+catkin_install_python(
+  PROGRAMS ${PYTHON_SCRIPTS}
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  USE_SOURCE_PERMISSIONS
 )
 install(FILES plugin.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
-install(DIRECTORY launch resource sample sample_scripts test
+install(DIRECTORY launch resource sample sample_scripts
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS
 )
+file(GLOB PYTHON_SAMPLES sample_scripts/*.py)
+catkin_install_python(
+  PROGRAMS ${PYTHON_SAMPLES}
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/sample_scripts
+)
+
+install(DIRECTORY test
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+  PATTERN "*.launch"
+)
+file(GLOB PYTHON_TEST test/*.py)
+catkin_install_python(
+  PROGRAMS ${PYTHON_TEST}
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+)
 
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)

jsk_rviz_plugins/CMakeLists.txt

@@ -224,18 +224,24 @@ install(TARGETS jsk_rviz_plugins
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
 
-install(DIRECTORY scripts/
+file(GLOB PYTHON_SCRIPTS scripts/*.py)
+catkin_install_python(
+  PROGRAMS ${PYTHON_SCRIPTS}
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  USE_SOURCE_PERMISSIONS
 )
 
 install(FILES plugin_description.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
-install(DIRECTORY cfg config icons launch samples test
+install(DIRECTORY cfg config icons launch test
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS
 )
+file(GLOB PYTHON_SAMPLES samples/*.py)
+catkin_install_python(
+  PROGRAMS ${PYTHON_SAMPLES}
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/samples
+)
 
 install(DIRECTORY src/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

jsk_rviz_plugins/samples/bounding_box_sample.py

@@ -12,15 +12,19 @@
 while not rospy.is_shutdown():
   box_a = BoundingBox()
   box_b = BoundingBox()
+  box_c = BoundingBox()
   box_a.label = 2
   box_b.label = 5
+  box_b.label = 10
   box_arr = BoundingBoxArray()
   now = rospy.Time.now()
   box_a.header.stamp = now
   box_b.header.stamp = now
+  box_c.header.stamp = now
   box_arr.header.stamp = now
   box_a.header.frame_id = "map"
   box_b.header.frame_id = "map"
+  box_c.header.frame_id = "map"
   box_arr.header.frame_id = "map"
   q = quaternion_about_axis((counter % 100) * math.pi * 2 / 100.0, [0, 0, 1])
   box_a.pose.orientation.x = q[0]
@@ -29,16 +33,23 @@
   box_a.pose.orientation.w = q[3]
   box_b.pose.orientation.w = 1
   box_b.pose.position.y = 2
+  box_c.pose.position.y = 1
+  box_c.pose.position.z = -0.52
   box_b.dimensions.x = (counter % 10 + 1) * 0.1
   box_b.dimensions.y = ((counter + 1) % 10 + 1) * 0.1
   box_b.dimensions.z = ((counter + 2) % 10 + 1) * 0.1
   box_a.dimensions.x = 1
   box_a.dimensions.y = 1
   box_a.dimensions.z = 1
+  box_c.dimensions.x = 3
+  box_c.dimensions.y = 3
+  box_c.dimensions.z = 0.02
   box_a.value = (counter % 100) / 100.0
   box_b.value = 1 - (counter % 100) / 100.0
+  box_c.value = (counter % 300) / 100.0
   box_arr.boxes.append(box_a)
   box_arr.boxes.append(box_b)
+  box_arr.boxes.append(box_c)
   pub.publish(box_arr)
   r.sleep()
   counter = counter + 1

jsk_rviz_plugins/src/bounding_box_array_display.cpp

@@ -49,6 +49,12 @@ namespace jsk_rviz_plugins
     coloring_property_->addOption("Label", 2);
     coloring_property_->addOption("Value", 3);
 
+    alpha_method_property_ = new rviz::EnumProperty(
+      "alpha method", "flat", "alpha method",
+      this, SLOT(updateAlphaMethod()));
+    alpha_method_property_->addOption("flat", 0);
+    alpha_method_property_->addOption("value", 1);
+
     color_property_ = new rviz::ColorProperty(
       "color", QColor(25, 255, 0),
       "color to draw the bounding boxes",
@@ -57,6 +63,14 @@ namespace jsk_rviz_plugins
       "alpha", 0.8,
       "alpha value to draw the bounding boxes",
       this, SLOT(updateAlpha()));
+    alpha_min_property_ = new rviz::FloatProperty(
+      "alpha min", 0.0,
+      "alpha value corresponding to value = 0",
+      this, SLOT(updateAlphaMin()));
+    alpha_max_property_ = new rviz::FloatProperty(
+      "alpha max", 1.0,
+      "alpha value corresponding to value = 1",
+      this, SLOT(updateAlphaMax()));
     only_edge_property_ = new rviz::BoolProperty(
       "only edge", false,
       "show only the edges of the boxes",
@@ -69,15 +83,23 @@ namespace jsk_rviz_plugins
       "show coords", false,
       "show coordinate of bounding box",
       this, SLOT(updateShowCoords()));
+    value_threshold_property_ = new rviz::FloatProperty(
+      "value threshold", 0.0,
+      "filter all boxes with value < threshold",
+      this, SLOT(updateValueThreshold()));
   }
 
   BoundingBoxArrayDisplay::~BoundingBoxArrayDisplay()
   {
     delete color_property_;
     delete alpha_property_;
+    delete alpha_min_property_;
+    delete alpha_max_property_;
     delete only_edge_property_;
     delete coloring_property_;
+    delete alpha_method_property_;
     delete show_coords_property_;
+    delete value_threshold_property_;
   }
 
   void BoundingBoxArrayDisplay::onInitialize()
@@ -87,10 +109,14 @@ namespace jsk_rviz_plugins
 
     updateColor();
     updateAlpha();
+    updateAlphaMin();
+    updateAlphaMax();
     updateOnlyEdge();
     updateColoring();
+    updateAlphaMethod();
     updateLineWidth();
     updateShowCoords();
+    updateValueThreshold();
   }
 
   void BoundingBoxArrayDisplay::updateLineWidth()
@@ -117,6 +143,34 @@ namespace jsk_rviz_plugins
     }
   }
 
+  void BoundingBoxArrayDisplay::updateAlphaMin()
+  {
+    if (alpha_min_property_->getFloat() > alpha_max_)
+    {
+      ROS_WARN("alpha_min must be <= alpha_max");
+      alpha_min_property_->setFloat(alpha_min_);
+      return;
+    }
+    alpha_min_ = alpha_min_property_->getFloat();
+    if (latest_msg_) {
+      processMessage(latest_msg_);
+    }
+  }
+
+  void BoundingBoxArrayDisplay::updateAlphaMax()
+  {
+    if (alpha_max_property_->getFloat() < alpha_min_)
+    {
+      ROS_WARN("alpha_min must be <= alpha_max");
+      alpha_max_property_->setFloat(alpha_max_);
+      return;
+    }
+    alpha_max_ = alpha_max_property_->getFloat();
+    if (latest_msg_) {
+      processMessage(latest_msg_);
+    }
+  }
+
   void BoundingBoxArrayDisplay::updateOnlyEdge()
   {
     only_edge_ = only_edge_property_->getBool();
@@ -161,6 +215,26 @@ namespace jsk_rviz_plugins
     }
   }
 
+  void BoundingBoxArrayDisplay::updateAlphaMethod()
+  {
+    if (alpha_method_property_->getOptionInt() == 0) {
+      alpha_method_ = "flat";
+      alpha_property_->show();
+      alpha_min_property_->hide();
+      alpha_max_property_->hide();
+    }
+    else if (alpha_method_property_->getOptionInt() == 1) {
+      alpha_method_ = "value";
+      alpha_property_->hide();
+      alpha_min_property_->show();
+      alpha_max_property_->show();
+    }
+
+    if (latest_msg_) {
+      processMessage(latest_msg_);
+    }
+  }
+
   void BoundingBoxArrayDisplay::updateShowCoords()
   {
     show_coords_ = show_coords_property_->getBool();
@@ -204,6 +278,20 @@ namespace jsk_rviz_plugins
     }
   }
 
+  void BoundingBoxArrayDisplay::updateValueThreshold()
+  {
+    if (value_threshold_property_->getFloat() < 0.0 || value_threshold_property_->getFloat() > 1.0)
+    {
+      ROS_WARN("value threshold must be in [0,1]");
+      value_threshold_property_->setFloat(value_threshold_);
+      return;
+    }
+    value_threshold_ = value_threshold_property_->getFloat();
+    if (latest_msg_) {
+      processMessage(latest_msg_);
+    }
+  }
+
 }  // namespace jsk_rviz_plugins
 
 #include <pluginlib/class_list_macros.h>

jsk_rviz_plugins/src/bounding_box_array_display.h

@@ -68,19 +68,27 @@ namespace jsk_rviz_plugins
     // Properties
     rviz::EnumProperty* coloring_property_;
     rviz::ColorProperty* color_property_;
+    rviz::EnumProperty* alpha_method_property_;
     rviz::FloatProperty* alpha_property_;
+    rviz::FloatProperty* alpha_min_property_;
+    rviz::FloatProperty* alpha_max_property_;
     rviz::BoolProperty* only_edge_property_;
     rviz::FloatProperty* line_width_property_;
     rviz::BoolProperty* show_coords_property_;
+    rviz::FloatProperty* value_threshold_property_;
 
     jsk_recognition_msgs::BoundingBoxArray::ConstPtr latest_msg_;
   protected Q_SLOTS:
     void updateColor();
     void updateAlpha();
+    void updateAlphaMin();
+    void updateAlphaMax();
     void updateOnlyEdge();
     void updateColoring();
+    void updateAlphaMethod();
     void updateLineWidth();
     void updateShowCoords();
+    void updateValueThreshold();
   private:
     void processMessage(
       const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg);