Add nav2_msgs/ParticleCloud display plugin

rviz_default_plugins/CMakeLists.txt

@@ -63,6 +63,7 @@ find_package(interactive_markers REQUIRED)
 find_package(laser_geometry REQUIRED)
 find_package(map_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(resource_retriever REQUIRED)
@@ -120,6 +121,7 @@ set(rviz_default_plugins_headers_to_moc
   include/rviz_default_plugins/displays/tf/frame_info.hpp
   include/rviz_default_plugins/displays/tf/tf_display.hpp
   include/rviz_default_plugins/displays/wrench/wrench_display.hpp
+  include/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp
   include/rviz_default_plugins/robot/robot.hpp
   include/rviz_default_plugins/robot/robot_joint.hpp
   include/rviz_default_plugins/robot/robot_link.hpp
@@ -200,6 +202,8 @@ set(rviz_default_plugins_source_files
   src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp
   src/rviz_default_plugins/displays/tf/tf_display.cpp
   src/rviz_default_plugins/displays/wrench/wrench_display.cpp
+  src/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.cpp
+  src/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.cpp
   src/rviz_default_plugins/robot/robot.cpp
   src/rviz_default_plugins/robot/robot_joint.cpp
   src/rviz_default_plugins/robot/robot_link.cpp
@@ -250,6 +254,7 @@ ament_target_dependencies(rviz_default_plugins
   interactive_markers
   laser_geometry
   nav_msgs
+  nav2_msgs
   map_msgs
   rclcpp
   resource_retriever
@@ -272,6 +277,7 @@ ament_export_dependencies(
   laser_geometry
   map_msgs
   nav_msgs
+  nav2_msgs
   rclcpp
   tf2
   tf2_geometry_msgs
@@ -335,6 +341,7 @@ if(BUILD_TESTING)
   set(TEST_TARGET_DEPENDENCIES
     map_msgs
     nav_msgs
+    nav2_msgs
     rclcpp
     sensor_msgs
     urdf
@@ -572,6 +579,17 @@ if(BUILD_TESTING)
     ament_target_dependencies(pose_array_display_test ${TEST_TARGET_DEPENDENCIES})
   endif()
 
+  ament_add_gmock(particle_cloud_display_test
+    test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_test.cpp
+    test/rviz_default_plugins/displays/display_test_fixture.cpp
+    test/rviz_default_plugins/scene_graph_introspection_helper.cpp
+    ${SKIP_DISPLAY_TESTS})
+  if(TARGET particle_cloud_display_test)
+    target_include_directories(particle_cloud_display_test PUBLIC ${TEST_INCLUDE_DIRS})
+    target_link_libraries(particle_cloud_display_test ${TEST_LINK_LIBRARIES})
+    ament_target_dependencies(particle_cloud_display_test ${TEST_TARGET_DEPENDENCIES})
+  endif()
+
   ament_add_gmock(pose_tool_test
     test/rviz_default_plugins/tools/pose/pose_tool_test.cpp
     test/rviz_default_plugins/displays/display_test_fixture.cpp
@@ -924,6 +942,21 @@ if(BUILD_TESTING)
       rviz_visual_testing_framework::rviz_visual_testing_framework)
   endif()
 
+  ament_add_gtest(particle_cloud_display_visual_test
+    test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_visual_test.cpp
+    test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.cpp
+    test/rviz_default_plugins/publishers/particle_cloud_publisher.hpp
+    ${SKIP_VISUAL_TESTS}
+    TIMEOUT 180)
+  if(TARGET particle_cloud_display_visual_test)
+    target_include_directories(particle_cloud_display_visual_test PUBLIC
+      ${TEST_INCLUDE_DIRS}
+      ${rviz_visual_testing_framework_INCLUDE_DIRS})
+    target_link_libraries(particle_cloud_display_visual_test
+      ${TEST_LINK_LIBRARIES}
+      rviz_visual_testing_framework::rviz_visual_testing_framework)
+  endif()
+
   ament_add_gtest(pose_display_visual_test
     test/rviz_default_plugins/displays/pose/pose_display_visual_test.cpp
     test/rviz_default_plugins/page_objects/pose_display_page_object.cpp

rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.hpp

@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2018, Bosch Software Innovations GmbH.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
+#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
+
+#include <vector>
+
+#include <OgreManualObject.h>
+#include <OgreMaterialManager.h>
+#include <OgreSceneNode.h>
+#include <OgreVector3.h>
+#include <OgreQuaternion.h>
+
+#include "rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp"
+#include "rviz_default_plugins/visibility_control.hpp"
+
+namespace rviz_default_plugins
+{
+namespace displays
+{
+struct OgrePoseWithWeight;
+
+class RVIZ_DEFAULT_PLUGINS_PUBLIC FlatWeightedArrowsArray
+{
+public:
+  explicit FlatWeightedArrowsArray(Ogre::SceneManager * scene_manager_);
+  ~FlatWeightedArrowsArray();
+
+  void createAndAttachManualObject(Ogre::SceneNode * scene_node);
+  void updateManualObject(
+    Ogre::ColourValue color,
+    float alpha,
+    float min_length,
+    float max_length,
+    const std::vector<rviz_default_plugins::displays::OgrePoseWithWeight> & poses);
+  void clear();
+
+private:
+  void setManualObjectMaterial();
+  void setManualObjectVertices(
+    const Ogre::ColourValue & color,
+    float min_length,
+    float max_length,
+    const std::vector<rviz_default_plugins::displays::OgrePoseWithWeight> & poses);
+
+  Ogre::SceneManager * scene_manager_;
+  Ogre::ManualObject * manual_object_;
+  Ogre::MaterialPtr material_;
+};
+
+}  // namespace displays
+}  // namespace rviz_default_plugins
+
+#endif  // RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_

rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp

@@ -0,0 +1,149 @@
+/*
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * Copyright (c) 2018, Bosch Software Innovations GmbH.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__PARTICLE_CLOUD_DISPLAY_HPP_
+#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__PARTICLE_CLOUD_DISPLAY_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "nav2_msgs/msg/particle_cloud.hpp"
+
+#include "rviz_rendering/objects/shape.hpp"
+#include "rviz_common/message_filter_display.hpp"
+
+#include "rviz_default_plugins/visibility_control.hpp"
+
+namespace Ogre
+{
+class ManualObject;
+}  // namespace Ogre
+
+namespace rviz_common
+{
+namespace properties
+{
+class EnumProperty;
+class ColorProperty;
+class FloatProperty;
+}  // namespace properties
+}  // namespace rviz_common
+
+namespace rviz_rendering
+{
+class Arrow;
+class Axes;
+}  // namespace rviz_rendering
+
+namespace rviz_default_plugins
+{
+namespace displays
+{
+class FlatWeightedArrowsArray;
+struct OgrePoseWithWeight
+{
+  Ogre::Vector3 position;
+  Ogre::Quaternion orientation;
+  float weight;
+};
+
+/** @brief Displays a nav2_msgs/ParticleCloud message as a bunch of line-drawn weighted arrows. */
+class RVIZ_DEFAULT_PLUGINS_PUBLIC ParticleCloudDisplay : public
+  rviz_common::MessageFilterDisplay<nav2_msgs::msg::ParticleCloud>
+{
+  Q_OBJECT
+
+public:
+  // TODO(botteroa-si): Constructor for testing, remove once ros_nodes can be mocked and call
+  // initialize instead
+  ParticleCloudDisplay(
+    rviz_common::DisplayContext * display_context,
+    Ogre::SceneNode * scene_node);
+  ParticleCloudDisplay();
+  ~ParticleCloudDisplay() override;
+
+  void processMessage(nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg) override;
+  void setShape(QString shape);  // for testing
+
+protected:
+  void onInitialize() override;
+  void reset() override;
+
+private Q_SLOTS:
+  /// Update the interface and visible shapes based on the selected shape type.
+  void updateShapeChoice();
+
+  /// Update the arrow color.
+  void updateArrowColor();
+
+  /// Update arrow geometry
+  void updateGeometry();
+
+private:
+  void initializeProperties();
+  bool validateFloats(const nav2_msgs::msg::ParticleCloud & msg);
+  bool setTransform(std_msgs::msg::Header const & header);
+  void updateDisplay();
+  void updateArrows2d();
+  void updateArrows3d();
+  void updateAxes();
+  void updateArrow3dGeometry();
+  void updateAxesGeometry();
+
+  std::unique_ptr<rviz_rendering::Axes> makeAxes();
+  std::unique_ptr<rviz_rendering::Arrow> makeArrow3d();
+
+  std::vector<OgrePoseWithWeight> poses_;
+  std::unique_ptr<FlatWeightedArrowsArray> arrows2d_;
+  std::vector<std::unique_ptr<rviz_rendering::Arrow>> arrows3d_;
+  std::vector<std::unique_ptr<rviz_rendering::Axes>> axes_;
+
+  Ogre::SceneNode * arrow_node_;
+  Ogre::SceneNode * axes_node_;
+
+  rviz_common::properties::EnumProperty * shape_property_;
+  rviz_common::properties::ColorProperty * arrow_color_property_;
+  rviz_common::properties::FloatProperty * arrow_alpha_property_;
+
+  rviz_common::properties::FloatProperty * arrow_min_length_property_;
+  rviz_common::properties::FloatProperty * arrow_max_length_property_;
+
+  float min_length_;
+  float max_length_;
+  float length_scale_;
+  float head_radius_scale_;
+  float head_length_scale_;
+  float shaft_radius_scale_;
+};
+
+}  // namespace displays
+}  // namespace rviz_default_plugins
+
+#endif  // RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__PARTICLE_CLOUD_DISPLAY_HPP_

rviz_default_plugins/package.xml

@@ -28,6 +28,7 @@
   <depend>interactive_markers</depend>
   <depend>laser_geometry</depend>
   <depend>nav_msgs</depend>
+  <depend>nav2_msgs</depend>
   <depend>map_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

rviz_default_plugins/plugins_description.xml

@@ -264,6 +264,17 @@
     <message_type>geometry_msgs/msg/PoseArray</message_type>
   </class>
 
+  <class
+    name="rviz_default_plugins/ParticleCloud"
+    type="rviz_default_plugins::displays::ParticleCloudDisplay"
+    base_class_type="rviz_common::Display"
+  >
+    <description>
+      The Particle Cloud display shows a nav2_msgs/ParticleCloud message, as a collection of arrows scaled according to weight.
+    </description>
+    <message_type>nav2_msgs/msg/ParticleCloud</message_type>
+  </class>
+
   <class
     name="rviz_default_plugins/Path"
     type="rviz_default_plugins::displays::PathDisplay"