From 54b1043c8f15d21cdc6e77d947e797ea5b5f576c Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 5 May 2020 17:15:56 +0530 Subject: [PATCH 1/4] Add particle cloud display rviz plugin Signed-off-by: Sarthak Mittal --- .../bringup/rviz/nav2_default_view.rviz | 67 ++- .../bringup/rviz/nav2_namespaced_view.rviz | 13 +- nav2_rviz_plugins/CMakeLists.txt | 4 + .../flat_weighted_arrows_array.hpp | 95 ++++ .../particle_cloud_display.hpp | 162 +++++++ nav2_rviz_plugins/plugins_description.xml | 6 + .../src/flat_weighted_arrows_array.cpp | 141 ++++++ .../src/particle_cloud_display.cpp | 424 ++++++++++++++++++ 8 files changed, 864 insertions(+), 48 deletions(-) create mode 100644 nav2_rviz_plugins/include/nav2_rviz_plugins/flat_weighted_arrows_array.hpp create mode 100644 nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display.hpp create mode 100644 nav2_rviz_plugins/src/flat_weighted_arrows_array.cpp create mode 100644 nav2_rviz_plugins/src/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 04e989eb170..fe0c9ca83b4 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz @@ -8,7 +8,7 @@ Panels: - /TF1/Frames1 - /TF1/Tree1 Splitter Ratio: 0.5833333134651184 - Tree Height: 606 + Tree Height: 592 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -94,8 +94,6 @@ Visualization Manager: Value: true imu_link: Value: true - map: - Value: true odom: Value: true wheel_left_link: @@ -108,29 +106,7 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - map: - odom: - base_footprint: - base_link: - base_scan: - {} - camera_link: - camera_depth_frame: - camera_depth_optical_frame: - {} - camera_rgb_frame: - camera_rgb_optical_frame: - {} - caster_back_left_link: - {} - caster_back_right_link: - {} - imu_link: - {} - wheel_left_link: - {} - wheel_right_link: - {} + {} Update Interval: 0 Value: true - Alpha: 1 @@ -211,31 +187,32 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates 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/ParticleCloudDisplay Color: 0; 180; 0 Enabled: true - Head Length: 0.07000000029802322 - Head Radius: 0.029999999329447746 + Max Arrow Length: 0.10000000149011612 + Min Arrow Length: 0.019999999552965164 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: - - Alpha: 0.3 + - Alpha: 0.30000001192092896 Class: rviz_default_plugins/Map Color Scheme: costmap Draw Behind: false @@ -247,6 +224,12 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -337,6 +320,12 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -546,7 +535,7 @@ Window Geometry: Hide Right Dock: true Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002de000000a90000008100fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002d0000000b7000000b700fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RealsenseCamera: collapsed: false Selection: @@ -556,5 +545,5 @@ Window Geometry: Views: collapsed: true Width: 1545 - X: 696 - Y: 229 + X: 375 + Y: 118 diff --git a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz index 2a024f75b94..69eeb065b3e 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/ParticleCloudDisplay Color: 0; 180; 0 Enabled: true - Head Length: 0.07000000029802322 - Head Radius: 0.029999999329447746 + Max Arrow Length: 0.10000000149011612 + Min Arrow Length: 0.019999999552965164 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 65c32d8e852..3d5a8f6ed95 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/flat_weighted_arrows_array.hpp + include/nav2_rviz_plugins/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/flat_weighted_arrows_array.cpp + src/particle_cloud_display.cpp ${nav2_rviz_plugins_headers_to_moc} ) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/flat_weighted_arrows_array.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/flat_weighted_arrows_array.hpp new file mode 100644 index 00000000000..ec5cb2059a7 --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/flat_weighted_arrows_array.hpp @@ -0,0 +1,95 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * Copyright (c) 2020, Sarthak Mittal + * 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__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ +#define NAV2_RVIZ_PLUGINS__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "nav2_rviz_plugins/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__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display.hpp new file mode 100644 index 00000000000..9264f0bdff5 --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display.hpp @@ -0,0 +1,162 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * Copyright (c) 2020, Sarthak Mittal + * 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_HPP_ +#define NAV2_RVIZ_PLUGINS__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" + +// TODO(botteroa): Originally the display extended the MessageFilterDisplay. Revisit when available. +// #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_HPP_ diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index 34d27f28d09..af1405ebb09 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/flat_weighted_arrows_array.cpp b/nav2_rviz_plugins/src/flat_weighted_arrows_array.cpp new file mode 100644 index 00000000000..4501549442b --- /dev/null +++ b/nav2_rviz_plugins/src/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/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.cpp b/nav2_rviz_plugins/src/particle_cloud_display.cpp new file mode 100644 index 00000000000..05e869da599 --- /dev/null +++ b/nav2_rviz_plugins/src/particle_cloud_display.cpp @@ -0,0 +1,424 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * Copyright (c) 2020, Sarthak Mittal + * 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.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/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.1f) +{ + 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) From 61f8638894c5dabda883f22314e313f984955eb7 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Wed, 6 May 2020 07:03:04 +0530 Subject: [PATCH 2/4] General refactoring --- .../bringup/rviz/nav2_default_view.rviz | 54 ++++++++++--------- nav2_rviz_plugins/CMakeLists.txt | 8 +-- .../flat_weighted_arrows_array.hpp | 3 +- .../particle_cloud_display.hpp | 4 -- .../flat_weighted_arrows_array.cpp | 2 +- .../particle_cloud_display.cpp | 5 +- 6 files changed, 38 insertions(+), 38 deletions(-) rename nav2_rviz_plugins/include/nav2_rviz_plugins/{ => particle_cloud_display}/flat_weighted_arrows_array.hpp (97%) rename nav2_rviz_plugins/include/nav2_rviz_plugins/{ => particle_cloud_display}/particle_cloud_display.hpp (96%) rename nav2_rviz_plugins/src/{ => particle_cloud_display}/flat_weighted_arrows_array.cpp (98%) rename nav2_rviz_plugins/src/{ => particle_cloud_display}/particle_cloud_display.cpp (98%) diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz index fe0c9ca83b4..e22b970ad05 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz @@ -8,7 +8,7 @@ Panels: - /TF1/Frames1 - /TF1/Tree1 Splitter Ratio: 0.5833333134651184 - Tree Height: 592 + Tree Height: 606 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -94,6 +94,8 @@ Visualization Manager: Value: true imu_link: Value: true + map: + Value: true odom: Value: true wheel_left_link: @@ -106,7 +108,29 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - {} + map: + odom: + base_footprint: + base_link: + base_scan: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + caster_back_left_link: + {} + caster_back_right_link: + {} + imu_link: + {} + wheel_left_link: + {} + wheel_right_link: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -187,12 +211,6 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map_updates Use Timestamp: false Value: true - Alpha: 1 @@ -212,7 +230,7 @@ Visualization Manager: Value: true - Class: rviz_common/Group Displays: - - Alpha: 0.30000001192092896 + - Alpha: 0.3 Class: rviz_default_plugins/Map Color Scheme: costmap Draw Behind: false @@ -224,12 +242,6 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /global_costmap/costmap - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -320,12 +332,6 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /local_costmap/costmap - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -535,7 +541,7 @@ Window Geometry: Hide Right Dock: true Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002d0000000b7000000b700fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002de000000a90000008100fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RealsenseCamera: collapsed: false Selection: @@ -545,5 +551,5 @@ Window Geometry: Views: collapsed: true Width: 1545 - X: 375 - Y: 118 + X: 696 + Y: 229 diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index 3d5a8f6ed95..9cb52f132ad 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -37,8 +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/flat_weighted_arrows_array.hpp - include/nav2_rviz_plugins/particle_cloud_display.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( @@ -50,8 +50,8 @@ set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED src/goal_tool.cpp src/nav2_panel.cpp - src/flat_weighted_arrows_array.cpp - src/particle_cloud_display.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/flat_weighted_arrows_array.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp similarity index 97% rename from nav2_rviz_plugins/include/nav2_rviz_plugins/flat_weighted_arrows_array.hpp rename to nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp index ec5cb2059a7..8e34369e011 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/flat_weighted_arrows_array.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp @@ -1,7 +1,6 @@ /* * Copyright (c) 2012, Willow Garage, Inc. * Copyright (c) 2018, Bosch Software Innovations GmbH. - * Copyright (c) 2020, Sarthak Mittal * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -55,7 +54,7 @@ #include #include -#include "nav2_rviz_plugins/particle_cloud_display.hpp" +#include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp" namespace nav2_rviz_plugins { diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp similarity index 96% rename from nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display.hpp rename to nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp index 9264f0bdff5..5b4e47e56fe 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp @@ -1,7 +1,6 @@ /* * Copyright (c) 2012, Willow Garage, Inc. * Copyright (c) 2018, Bosch Software Innovations GmbH. - * Copyright (c) 2020, Sarthak Mittal * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -55,9 +54,6 @@ #include "rviz_rendering/objects/shape.hpp" #include "rviz_common/message_filter_display.hpp" -// TODO(botteroa): Originally the display extended the MessageFilterDisplay. Revisit when available. -// #include "rviz_common/message_filter_display.hpp" - namespace Ogre { class ManualObject; diff --git a/nav2_rviz_plugins/src/flat_weighted_arrows_array.cpp b/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp similarity index 98% rename from nav2_rviz_plugins/src/flat_weighted_arrows_array.cpp rename to nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp index 4501549442b..702a6e50dcf 100644 --- a/nav2_rviz_plugins/src/flat_weighted_arrows_array.cpp +++ b/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp @@ -43,7 +43,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_rviz_plugins/flat_weighted_arrows_array.hpp" +#include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp" #include #include diff --git a/nav2_rviz_plugins/src/particle_cloud_display.cpp b/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp similarity index 98% rename from nav2_rviz_plugins/src/particle_cloud_display.cpp rename to nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp index 05e869da599..bd1421555b4 100644 --- a/nav2_rviz_plugins/src/particle_cloud_display.cpp +++ b/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp @@ -1,7 +1,6 @@ /* * Copyright (c) 2012, Willow Garage, Inc. * Copyright (c) 2018, Bosch Software Innovations GmbH. - * Copyright (c) 2020, Sarthak Mittal * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -44,7 +43,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_rviz_plugins/particle_cloud_display.hpp" +#include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp" #include #include @@ -63,7 +62,7 @@ #include "rviz_rendering/objects/arrow.hpp" #include "rviz_rendering/objects/axes.hpp" -#include "nav2_rviz_plugins/flat_weighted_arrows_array.hpp" +#include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp" namespace nav2_rviz_plugins { From fdfaec9b093e1ac0cd5d30286c95dc861c91717a Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Wed, 6 May 2020 07:42:14 +0530 Subject: [PATCH 3/4] Fix header guards --- .../particle_cloud_display/flat_weighted_arrows_array.hpp | 6 +++--- .../particle_cloud_display/particle_cloud_display.hpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) 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 index 8e34369e011..7647aaf67fa 100644 --- 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 @@ -43,8 +43,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RVIZ_PLUGINS__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ -#define NAV2_RVIZ_PLUGINS__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ +#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 @@ -91,4 +91,4 @@ class FlatWeightedArrowsArray } // namespace nav2_rviz_plugins -#endif // NAV2_RVIZ_PLUGINS__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ +#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 index 5b4e47e56fe..f67a6e05382 100644 --- 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 @@ -43,8 +43,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY_HPP_ -#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY_HPP_ +#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_ +#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_ #include #include @@ -155,4 +155,4 @@ private Q_SLOTS: } // namespace nav2_rviz_plugins -#endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY_HPP_ +#endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_ From 3a9d5c3dfdbe62c35601bf296eb18dc1e93f586f Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 7 May 2020 09:34:02 +0530 Subject: [PATCH 4/4] Update bringup rviz files and default values --- nav2_bringup/bringup/rviz/nav2_default_view.rviz | 6 +++--- nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz | 6 +++--- nav2_rviz_plugins/plugins_description.xml | 2 +- .../src/particle_cloud_display/particle_cloud_display.cpp | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz index e22b970ad05..5b2700bc31d 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz @@ -214,11 +214,11 @@ Visualization Manager: Use Timestamp: false Value: true - Alpha: 1 - Class: nav2_rviz_plugins/ParticleCloudDisplay + Class: nav2_rviz_plugins/ParticleCloud Color: 0; 180; 0 Enabled: true - Max Arrow Length: 0.10000000149011612 - Min Arrow Length: 0.019999999552965164 + Max Arrow Length: 0.3 + Min Arrow Length: 0.02 Name: Amcl Particle Swarm Shape: Arrow (Flat) Topic: diff --git a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz index 69eeb065b3e..4897dda6e4f 100644 --- a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz @@ -164,11 +164,11 @@ Visualization Manager: Use Timestamp: false Value: true - Alpha: 1 - Class: nav2_rviz_plugins/ParticleCloudDisplay + Class: nav2_rviz_plugins/ParticleCloud Color: 0; 180; 0 Enabled: true - Max Arrow Length: 0.10000000149011612 - Min Arrow Length: 0.019999999552965164 + Max Arrow Length: 0.3 + Min Arrow Length: 0.02 Name: Amcl Particle Swarm Shape: Arrow (Flat) Topic: diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index af1405ebb09..90bcf66d01a 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -12,7 +12,7 @@ The Navigation2 rviz panel. - The Particle Cloud rviz display. 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 index bd1421555b4..f04abad5a61 100644 --- a/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp +++ b/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp @@ -97,7 +97,7 @@ ParticleCloudDisplay::ParticleCloudDisplay( } ParticleCloudDisplay::ParticleCloudDisplay() -: min_length_(0.02f), max_length_(0.1f) +: min_length_(0.02f), max_length_(0.3f) { initializeProperties();