Add particle cloud display plugin for RViz (ros-navigation#1688)

* Add particle cloud display rviz plugin

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* General refactoring

* Fix header guards

* Update bringup rviz files and default values

nav2_bringup/bringup/rviz/nav2_default_view.rviz

@@ -214,24 +214,19 @@ Visualization Manager:
       Use Timestamp: false
       Value: true
     - Alpha: 1
-      Arrow Length: 0.019999999552965164
-      Axes Length: 0.30000001192092896
-      Axes Radius: 0.009999999776482582
-      Class: rviz_default_plugins/PoseArray
+      Class: nav2_rviz_plugins/ParticleCloud
       Color: 0; 180; 0
       Enabled: true
-      Head Length: 0.07000000029802322
-      Head Radius: 0.029999999329447746
+      Max Arrow Length: 0.3
+      Min Arrow Length: 0.02
       Name: Amcl Particle Swarm
-      Shaft Length: 0.23000000417232513
-      Shaft Radius: 0.009999999776482582
       Shape: Arrow (Flat)
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Best Effort
-        Value: /particlecloud
+        Value: /particle_cloud
       Value: true
     - Class: rviz_common/Group
       Displays:

nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz

@@ -164,24 +164,19 @@ Visualization Manager:
       Use Timestamp: false
       Value: true
     - Alpha: 1
-      Arrow Length: 0.019999999552965164
-      Axes Length: 0.30000001192092896
-      Axes Radius: 0.009999999776482582
-      Class: rviz_default_plugins/PoseArray
+      Class: nav2_rviz_plugins/ParticleCloud
       Color: 0; 180; 0
       Enabled: true
-      Head Length: 0.07000000029802322
-      Head Radius: 0.029999999329447746
+      Max Arrow Length: 0.3
+      Min Arrow Length: 0.02
       Name: Amcl Particle Swarm
-      Shaft Length: 0.23000000417232513
-      Shaft Radius: 0.009999999776482582
       Shape: Arrow (Flat)
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Best Effort
-        Value: <robot_namespace>/particlecloud
+        Value: <robot_namespace>/particle_cloud
       Value: true
     - Class: rviz_common/Group
       Displays:

nav2_rviz_plugins/CMakeLists.txt

@@ -37,6 +37,8 @@ set(nav2_rviz_plugins_headers_to_moc
   include/nav2_rviz_plugins/goal_common
   include/nav2_rviz_plugins/goal_tool.hpp
   include/nav2_rviz_plugins/nav2_panel.hpp
+  include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp
+  include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp
 )
 
 include_directories(
@@ -48,6 +50,8 @@ set(library_name ${PROJECT_NAME})
 add_library(${library_name} SHARED
   src/goal_tool.cpp
   src/nav2_panel.cpp
+  src/particle_cloud_display/flat_weighted_arrows_array.cpp
+  src/particle_cloud_display/particle_cloud_display.cpp
   ${nav2_rviz_plugins_headers_to_moc}
 )
 

nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp

@@ -0,0 +1,94 @@
+/*
+ * 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.
+ */
+
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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.
+
+#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
+#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
+
+#include <vector>
+
+#include <OgreManualObject.h>
+#include <OgreMaterialManager.h>
+#include <OgreSceneNode.h>
+#include <OgreVector3.h>
+#include <OgreQuaternion.h>
+
+#include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp"
+
+namespace nav2_rviz_plugins
+{
+
+struct OgrePoseWithWeight;
+
+class 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<nav2_rviz_plugins::OgrePoseWithWeight> & poses);
+  void clear();
+
+private:
+  void setManualObjectMaterial();
+  void setManualObjectVertices(
+    const Ogre::ColourValue & color,
+    float min_length,
+    float max_length,
+    const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses);
+
+  Ogre::SceneManager * scene_manager_;
+  Ogre::ManualObject * manual_object_;
+  Ogre::MaterialPtr material_;
+};
+
+}  // namespace nav2_rviz_plugins
+
+#endif  // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_

nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp

@@ -0,0 +1,158 @@
+/*
+ * 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.
+ */
+
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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.
+
+#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_
+#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__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"
+
+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 nav2_rviz_plugins
+{
+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 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 nav2_rviz_plugins
+
+#endif  // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_

nav2_rviz_plugins/plugins_description.xml

@@ -12,4 +12,10 @@
     <description>The Navigation2 rviz panel.</description>
   </class>
 
+  <class name="nav2_rviz_plugins/ParticleCloud"
+         type="nav2_rviz_plugins::ParticleCloudDisplay"
+         base_class_type="rviz_common::Display">
+    <description>The Particle Cloud rviz display.</description>
+  </class>
+
 </library>