From 8d6cd2878bae389d932982e759acb760f3291f63 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 7 Jan 2021 01:24:18 +0530 Subject: [PATCH] Add particle cloud display plugin for RViz (#1688) * Add particle cloud display rviz plugin Signed-off-by: Sarthak Mittal * General refactoring * Fix header guards * Update bringup rviz files and default values --- .../bringup/rviz/nav2_default_view.rviz | 13 +- .../bringup/rviz/nav2_namespaced_view.rviz | 13 +- nav2_rviz_plugins/CMakeLists.txt | 4 + .../flat_weighted_arrows_array.hpp | 94 ++++ .../particle_cloud_display.hpp | 158 +++++++ nav2_rviz_plugins/plugins_description.xml | 6 + .../flat_weighted_arrows_array.cpp | 141 ++++++ .../particle_cloud_display.cpp | 423 ++++++++++++++++++ 8 files changed, 834 insertions(+), 18 deletions(-) create mode 100644 nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp create mode 100644 nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp create mode 100644 nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp create mode 100644 nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz index c07e3e0cba6..dfdda36ba12 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/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: diff --git a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz index 2a024f75b94..4897dda6e4f 100644 --- a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz +++ b/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: /particlecloud + Value: /particle_cloud Value: true - Class: rviz_common/Group Displays: diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index f344363296c..9231505b916 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/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} ) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp new file mode 100644 index 00000000000..7647aaf67fa --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include + +#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 & poses); + void clear(); + +private: + void setManualObjectMaterial(); + void setManualObjectVertices( + const Ogre::ColourValue & color, + float min_length, + float max_length, + const std::vector & 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_ diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp new file mode 100644 index 00000000000..f67a6e05382 --- /dev/null +++ b/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 +#include + +#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 +{ + 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 makeAxes(); + std::unique_ptr makeArrow3d(); + + std::vector poses_; + std::unique_ptr arrows2d_; + std::vector> arrows3d_; + std::vector> 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_ diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index 34d27f28d09..90bcf66d01a 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -12,4 +12,10 @@ The Navigation2 rviz panel. + + The Particle Cloud rviz display. + + diff --git a/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp b/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp new file mode 100644 index 00000000000..702a6e50dcf --- /dev/null +++ b/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp @@ -0,0 +1,141 @@ +/* + * 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. + +#include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp" + +#include +#include +#include + +#include +#include + +#include "rviz_rendering/material_manager.hpp" + +namespace nav2_rviz_plugins +{ + +FlatWeightedArrowsArray::FlatWeightedArrowsArray(Ogre::SceneManager * scene_manager) +: scene_manager_(scene_manager), manual_object_(nullptr) {} + +FlatWeightedArrowsArray::~FlatWeightedArrowsArray() +{ + if (manual_object_) { + scene_manager_->destroyManualObject(manual_object_); + } +} + +void FlatWeightedArrowsArray::createAndAttachManualObject(Ogre::SceneNode * scene_node) +{ + manual_object_ = scene_manager_->createManualObject(); + manual_object_->setDynamic(true); + scene_node->attachObject(manual_object_); +} + +void FlatWeightedArrowsArray::updateManualObject( + Ogre::ColourValue color, + float alpha, + float min_length, + float max_length, + const std::vector & poses) +{ + clear(); + + color.a = alpha; + setManualObjectMaterial(); + rviz_rendering::MaterialManager::enableAlphaBlending(material_, alpha); + + manual_object_->begin( + material_->getName(), Ogre::RenderOperation::OT_LINE_LIST, "rviz_rendering"); + setManualObjectVertices(color, min_length, max_length, poses); + manual_object_->end(); +} + +void FlatWeightedArrowsArray::clear() +{ + if (manual_object_) { + manual_object_->clear(); + } +} + +void FlatWeightedArrowsArray::setManualObjectMaterial() +{ + static int material_count = 0; + std::string material_name = "FlatWeightedArrowsMaterial" + std::to_string(material_count++); + material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting(material_name); +} + +void FlatWeightedArrowsArray::setManualObjectVertices( + const Ogre::ColourValue & color, + float min_length, + float max_length, + const std::vector & poses) +{ + manual_object_->estimateVertexCount(poses.size() * 6); + + float scale = max_length - min_length; + float length; + for (const auto & pose : poses) { + length = std::min(std::max(pose.weight * scale + min_length, min_length), max_length); + Ogre::Vector3 vertices[6]; + vertices[0] = pose.position; // back of arrow + vertices[1] = + pose.position + pose.orientation * Ogre::Vector3(length, 0, 0); // tip of arrow + vertices[2] = vertices[1]; + vertices[3] = pose.position + pose.orientation * Ogre::Vector3( + 0.75f * length, 0.2f * length, 0); + vertices[4] = vertices[1]; + vertices[5] = pose.position + pose.orientation * Ogre::Vector3( + 0.75f * length, -0.2f * length, + 0); + + for (const auto & vertex : vertices) { + manual_object_->position(vertex); + manual_object_->colour(color); + } + } +} + +} // namespace nav2_rviz_plugins diff --git a/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp b/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp new file mode 100644 index 00000000000..f04abad5a61 --- /dev/null +++ b/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp @@ -0,0 +1,423 @@ +/* + * 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. + +#include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp" + +#include +#include + +#include +#include +#include + +#include "rviz_common/logging.hpp" +#include "rviz_common/msg_conversions.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/color_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/validate_floats.hpp" + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/axes.hpp" + +#include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp" + +namespace nav2_rviz_plugins +{ +namespace +{ +struct ShapeType +{ + enum + { + Arrow2d, + Arrow3d, + Axes, + }; +}; + +} // namespace + +ParticleCloudDisplay::ParticleCloudDisplay( + rviz_common::DisplayContext * display_context, + Ogre::SceneNode * scene_node) +: ParticleCloudDisplay() +{ + context_ = display_context; + scene_node_ = scene_node; + scene_manager_ = context_->getSceneManager(); + + arrows2d_ = std::make_unique(scene_manager_); + arrows2d_->createAndAttachManualObject(scene_node); + arrow_node_ = scene_node_->createChildSceneNode(); + axes_node_ = scene_node_->createChildSceneNode(); + updateShapeChoice(); +} + +ParticleCloudDisplay::ParticleCloudDisplay() +: min_length_(0.02f), max_length_(0.3f) +{ + initializeProperties(); + + shape_property_->addOption("Arrow (Flat)", ShapeType::Arrow2d); + shape_property_->addOption("Arrow (3D)", ShapeType::Arrow3d); + shape_property_->addOption("Axes", ShapeType::Axes); + arrow_alpha_property_->setMin(0); + arrow_alpha_property_->setMax(1); + arrow_min_length_property_->setMax(max_length_); + arrow_max_length_property_->setMin(min_length_); +} + +void ParticleCloudDisplay::initializeProperties() +{ + shape_property_ = new rviz_common::properties::EnumProperty( + "Shape", "Arrow (Flat)", "Shape to display the pose as.", this, SLOT(updateShapeChoice())); + + arrow_color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(255, 25, 0), "Color to draw the arrows.", this, SLOT(updateArrowColor())); + + arrow_alpha_property_ = new rviz_common::properties::FloatProperty( + "Alpha", + 1.0f, + "Amount of transparency to apply to the displayed poses.", + this, + SLOT(updateArrowColor())); + + arrow_min_length_property_ = new rviz_common::properties::FloatProperty( + "Min Arrow Length", min_length_, "Minimum length of the arrows.", this, SLOT(updateGeometry())); + + arrow_max_length_property_ = new rviz_common::properties::FloatProperty( + "Max Arrow Length", max_length_, "Maximum length of the arrows.", this, SLOT(updateGeometry())); + + // Scales are set based on initial values + length_scale_ = max_length_ - min_length_; + shaft_radius_scale_ = 0.0435; + head_length_scale_ = 0.3043; + head_radius_scale_ = 0.1304; +} + +ParticleCloudDisplay::~ParticleCloudDisplay() +{ + // because of forward declaration of arrow and axes, destructor cannot be declared in .hpp as + // default +} + +void ParticleCloudDisplay::onInitialize() +{ + MFDClass::onInitialize(); + arrows2d_ = std::make_unique(scene_manager_); + arrows2d_->createAndAttachManualObject(scene_node_); + arrow_node_ = scene_node_->createChildSceneNode(); + axes_node_ = scene_node_->createChildSceneNode(); + updateShapeChoice(); +} + +void ParticleCloudDisplay::processMessage(const nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg) +{ + if (!validateFloats(*msg)) { + setStatus( + rviz_common::properties::StatusProperty::Error, + "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + if (!setTransform(msg->header)) { + return; + } + + poses_.resize(msg->particles.size()); + + for (std::size_t i = 0; i < msg->particles.size(); ++i) { + poses_[i].position = rviz_common::pointMsgToOgre(msg->particles[i].pose.position); + poses_[i].orientation = rviz_common::quaternionMsgToOgre(msg->particles[i].pose.orientation); + poses_[i].weight = static_cast(msg->particles[i].weight); + } + + updateDisplay(); + + context_->queueRender(); +} + +bool ParticleCloudDisplay::validateFloats(const nav2_msgs::msg::ParticleCloud & msg) +{ + for (auto & particle : msg.particles) { + if (!rviz_common::validateFloats(particle.pose) || + !rviz_common::validateFloats(particle.weight)) + { + return false; + } + } + return true; +} + +bool ParticleCloudDisplay::setTransform(std_msgs::msg::Header const & header) +{ + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(header, position, orientation)) { + setMissingTransformToFixedFrame(header.frame_id); + return false; + } + setTransformOk(); + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + return true; +} + +void ParticleCloudDisplay::updateDisplay() +{ + int shape = shape_property_->getOptionInt(); + switch (shape) { + case ShapeType::Arrow2d: + updateArrows2d(); + arrows3d_.clear(); + axes_.clear(); + break; + case ShapeType::Arrow3d: + updateArrows3d(); + arrows2d_->clear(); + axes_.clear(); + break; + case ShapeType::Axes: + updateAxes(); + arrows2d_->clear(); + arrows3d_.clear(); + break; + } +} + +void ParticleCloudDisplay::updateArrows2d() +{ + arrows2d_->updateManualObject( + arrow_color_property_->getOgreColor(), + arrow_alpha_property_->getFloat(), + min_length_, + max_length_, + poses_); +} + +void ParticleCloudDisplay::updateArrows3d() +{ + while (arrows3d_.size() < poses_.size()) { + arrows3d_.push_back(makeArrow3d()); + } + while (arrows3d_.size() > poses_.size()) { + arrows3d_.pop_back(); + } + + Ogre::Quaternion adjust_orientation(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y); + float shaft_length; + for (std::size_t i = 0; i < poses_.size(); ++i) { + shaft_length = std::min( + std::max( + poses_[i].weight * length_scale_ + min_length_, + min_length_), max_length_); + arrows3d_[i]->set( + shaft_length, + shaft_length * shaft_radius_scale_, + shaft_length * head_length_scale_, + shaft_length * head_radius_scale_ + ); + arrows3d_[i]->setPosition(poses_[i].position); + arrows3d_[i]->setOrientation(poses_[i].orientation * adjust_orientation); + } +} + +void ParticleCloudDisplay::updateAxes() +{ + while (axes_.size() < poses_.size()) { + axes_.push_back(makeAxes()); + } + while (axes_.size() > poses_.size()) { + axes_.pop_back(); + } + float shaft_length; + for (std::size_t i = 0; i < poses_.size(); ++i) { + shaft_length = std::min( + std::max( + poses_[i].weight * length_scale_ + min_length_, + min_length_), max_length_); + axes_[i]->set(shaft_length, shaft_length * shaft_radius_scale_); + axes_[i]->setPosition(poses_[i].position); + axes_[i]->setOrientation(poses_[i].orientation); + } +} + +std::unique_ptr ParticleCloudDisplay::makeArrow3d() +{ + Ogre::ColourValue color = arrow_color_property_->getOgreColor(); + color.a = arrow_alpha_property_->getFloat(); + + auto arrow = std::make_unique( + scene_manager_, + arrow_node_, + min_length_, + min_length_ * shaft_radius_scale_, + min_length_ * head_length_scale_, + min_length_ * head_radius_scale_ + ); + + arrow->setColor(color); + return arrow; +} + +std::unique_ptr ParticleCloudDisplay::makeAxes() +{ + return std::make_unique( + scene_manager_, + axes_node_, + min_length_, + min_length_ * shaft_radius_scale_ + ); +} + +void ParticleCloudDisplay::reset() +{ + MFDClass::reset(); + arrows2d_->clear(); + arrows3d_.clear(); + axes_.clear(); +} + +void ParticleCloudDisplay::updateShapeChoice() +{ + int shape = shape_property_->getOptionInt(); + bool use_axes = shape == ShapeType::Axes; + + arrow_color_property_->setHidden(use_axes); + arrow_alpha_property_->setHidden(use_axes); + + if (initialized()) { + updateDisplay(); + } +} + +void ParticleCloudDisplay::updateArrowColor() +{ + int shape = shape_property_->getOptionInt(); + Ogre::ColourValue color = arrow_color_property_->getOgreColor(); + color.a = arrow_alpha_property_->getFloat(); + + if (shape == ShapeType::Arrow2d) { + updateArrows2d(); + } else if (shape == ShapeType::Arrow3d) { + for (const auto & arrow : arrows3d_) { + arrow->setColor(color); + } + } + context_->queueRender(); +} + +void ParticleCloudDisplay::updateGeometry() +{ + min_length_ = arrow_min_length_property_->getFloat(); + max_length_ = arrow_max_length_property_->getFloat(); + length_scale_ = max_length_ - min_length_; + + arrow_min_length_property_->setMax(max_length_); + arrow_max_length_property_->setMin(min_length_); + + int shape = shape_property_->getOptionInt(); + switch (shape) { + case ShapeType::Arrow2d: + updateArrows2d(); + arrows3d_.clear(); + axes_.clear(); + break; + case ShapeType::Arrow3d: + updateArrow3dGeometry(); + arrows2d_->clear(); + axes_.clear(); + break; + case ShapeType::Axes: + updateAxesGeometry(); + arrows2d_->clear(); + arrows3d_.clear(); + break; + } + + context_->queueRender(); +} + +void ParticleCloudDisplay::updateArrow3dGeometry() +{ + float shaft_length; + for (std::size_t i = 0; i < poses_.size() && i < arrows3d_.size(); ++i) { + shaft_length = std::min( + std::max( + poses_[i].weight * length_scale_ + min_length_, + min_length_), max_length_); + arrows3d_[i]->set( + shaft_length, + shaft_length * shaft_radius_scale_, + shaft_length * head_length_scale_, + shaft_length * head_radius_scale_ + ); + } +} + +void ParticleCloudDisplay::updateAxesGeometry() +{ + float shaft_length; + for (std::size_t i = 0; i < poses_.size() && i < axes_.size(); ++i) { + shaft_length = std::min( + std::max( + poses_[i].weight * length_scale_ + min_length_, + min_length_), max_length_); + axes_[i]->set(shaft_length, shaft_length * shaft_radius_scale_); + } +} + +void ParticleCloudDisplay::setShape(QString shape) +{ + shape_property_->setValue(shape); +} + +} // namespace nav2_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::ParticleCloudDisplay, rviz_common::Display)