diff --git a/doc/jsk_rviz_plugins/plugins/bounding_box_array.rst b/doc/jsk_rviz_plugins/plugins/bounding_box_array.rst index 2f01a4b4..bf7e9870 100644 --- a/doc/jsk_rviz_plugins/plugins/bounding_box_array.rst +++ b/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. diff --git a/doc/jsk_rviz_plugins/plugins/images/bounding_box_array_rviz.gif b/doc/jsk_rviz_plugins/plugins/images/bounding_box_array_rviz.gif new file mode 100644 index 00000000..bfc8f1e9 Binary files /dev/null and b/doc/jsk_rviz_plugins/plugins/images/bounding_box_array_rviz.gif differ diff --git a/jsk_rqt_plugins/CMakeLists.txt b/jsk_rqt_plugins/CMakeLists.txt index 9f02b9ca..a1cec5b9 100644 --- a/jsk_rqt_plugins/CMakeLists.txt +++ b/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) diff --git a/jsk_rviz_plugins/CMakeLists.txt b/jsk_rviz_plugins/CMakeLists.txt index b9f14dc3..a9bb121e 100644 --- a/jsk_rviz_plugins/CMakeLists.txt +++ b/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} diff --git a/jsk_rviz_plugins/samples/bounding_box_sample.py b/jsk_rviz_plugins/samples/bounding_box_sample.py index 6f60477f..a912b279 100755 --- a/jsk_rviz_plugins/samples/bounding_box_sample.py +++ b/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 diff --git a/jsk_rviz_plugins/src/bounding_box_array_display.cpp b/jsk_rviz_plugins/src/bounding_box_array_display.cpp index 4fdcace2..15638748 100644 --- a/jsk_rviz_plugins/src/bounding_box_array_display.cpp +++ b/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 diff --git a/jsk_rviz_plugins/src/bounding_box_array_display.h b/jsk_rviz_plugins/src/bounding_box_array_display.h index 39d840dd..b3b5b83e 100644 --- a/jsk_rviz_plugins/src/bounding_box_array_display.h +++ b/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); diff --git a/jsk_rviz_plugins/src/bounding_box_display.cpp b/jsk_rviz_plugins/src/bounding_box_display.cpp index 67e9ee8a..39713519 100644 --- a/jsk_rviz_plugins/src/bounding_box_display.cpp +++ b/jsk_rviz_plugins/src/bounding_box_display.cpp @@ -49,6 +49,12 @@ namespace jsk_rviz_plugins coloring_property_->addOption("Label", 1); coloring_property_->addOption("Value", 2); + 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())); } BoundingBoxDisplay::~BoundingBoxDisplay() { 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 BoundingBoxDisplay::onInitialize() @@ -87,10 +109,14 @@ namespace jsk_rviz_plugins updateColor(); updateAlpha(); + updateAlphaMin(); + updateAlphaMax(); updateOnlyEdge(); updateColoring(); + updateAlphaMethod(); updateLineWidth(); updateShowCoords(); + updateValueThreshold(); } void BoundingBoxDisplay::updateLineWidth() @@ -117,6 +143,34 @@ namespace jsk_rviz_plugins } } + void BoundingBoxDisplay::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 BoundingBoxDisplay::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 BoundingBoxDisplay::updateOnlyEdge() { only_edge_ = only_edge_property_->getBool(); @@ -151,6 +205,26 @@ namespace jsk_rviz_plugins } } + void BoundingBoxDisplay::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 BoundingBoxDisplay::updateShowCoords() { show_coords_ = show_coords_property_->getBool(); @@ -201,6 +275,20 @@ namespace jsk_rviz_plugins } } + void BoundingBoxDisplay::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 diff --git a/jsk_rviz_plugins/src/bounding_box_display.h b/jsk_rviz_plugins/src/bounding_box_display.h index c45f9ba9..020b7f89 100644 --- a/jsk_rviz_plugins/src/bounding_box_display.h +++ b/jsk_rviz_plugins/src/bounding_box_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::BoundingBox::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::BoundingBox::ConstPtr& msg); diff --git a/jsk_rviz_plugins/src/bounding_box_display_common.h b/jsk_rviz_plugins/src/bounding_box_display_common.h index af6fdf67..b72b1ccc 100644 --- a/jsk_rviz_plugins/src/bounding_box_display_common.h +++ b/jsk_rviz_plugins/src/bounding_box_display_common.h @@ -74,7 +74,11 @@ namespace jsk_rviz_plugins QColor color_; std::string coloring_method_; double alpha_; + double alpha_min_; + double alpha_max_; + std::string alpha_method_; double line_width_; + double value_threshold_; std::vector > coords_objects_; std::vector coords_nodes_; @@ -116,6 +120,19 @@ namespace jsk_rviz_plugins return QColor(255.0, 255.0, 255.0, 255.0); } + double getAlpha(const jsk_recognition_msgs::BoundingBox& box) + { + if (alpha_method_ == "flat") { + return alpha_; + } + else if (alpha_method_ == "value") + { + return alpha_min_ + box.value * (alpha_max_ - alpha_min_); + } + ROS_WARN_THROTTLE(10, "unknown alpha method"); + return 1.0; + } + bool isValidBoundingBox( const jsk_recognition_msgs::BoundingBox box_msg) { @@ -196,11 +213,17 @@ namespace jsk_rviz_plugins edges_.clear(); float min_value = DBL_MAX; float max_value = -DBL_MAX; + // filter boxes before drawing + std::vector box_indices; std::vector boxes; for (size_t i = 0; i < msg->boxes.size(); i++) { jsk_recognition_msgs::BoundingBox box = msg->boxes[i]; if (isValidBoundingBox(box)) { + if (box.value < value_threshold_) { + continue; + } boxes.push_back(box); + box_indices.push_back(i); min_value = std::min(min_value, msg->boxes[i].value); max_value = std::max(max_value, msg->boxes[i].value); } @@ -210,6 +233,8 @@ namespace jsk_rviz_plugins box.dimensions.x, box.dimensions.y, box.dimensions.z); } } + + // draw filtered boxes allocateShapes(boxes.size()); for (size_t i = 0; i < boxes.size(); i++) { jsk_recognition_msgs::BoundingBox box = boxes[i]; @@ -241,11 +266,11 @@ namespace jsk_rviz_plugins dimensions[1] = box.dimensions.y; dimensions[2] = box.dimensions.z; shape->setScale(dimensions); - QColor color = getColor(i, box, min_value, max_value); + QColor color = getColor(box_indices[i], box, min_value, max_value); shape->setColor(color.red() / 255.0, color.green() / 255.0, color.blue() / 255.0, - alpha_); + getAlpha(box)); } } @@ -255,11 +280,17 @@ namespace jsk_rviz_plugins shapes_.clear(); float min_value = DBL_MAX; float max_value = -DBL_MAX; + // filter boxes before drawing + std::vector box_indices; std::vector boxes; for (size_t i = 0; i < msg->boxes.size(); i++) { jsk_recognition_msgs::BoundingBox box = msg->boxes[i]; if (isValidBoundingBox(box)) { + if (box.value < value_threshold_) { + continue; + } boxes.push_back(box); + box_indices.push_back(i); min_value = std::min(min_value, msg->boxes[i].value); max_value = std::max(max_value, msg->boxes[i].value); } @@ -270,6 +301,7 @@ namespace jsk_rviz_plugins } } + // draw filtered boxes allocateBillboardLines(boxes.size()); for (size_t i = 0; i < boxes.size(); i++) { jsk_recognition_msgs::BoundingBox box = boxes[i]; @@ -296,11 +328,11 @@ namespace jsk_rviz_plugins edge->setMaxPointsPerLine(2); edge->setNumLines(12); edge->setLineWidth(line_width_); - QColor color = getColor(i, box, min_value, max_value); + QColor color = getColor(box_indices[i], box, min_value, max_value); edge->setColor(color.red() / 255.0, color.green() / 255.0, color.blue() / 255.0, - alpha_); + getAlpha(box)); Ogre::Vector3 A, B, C, D, E, F, G, H; A[0] = dimensions.x / 2.0;