diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index fb4055491..f0afab452 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -63,6 +63,7 @@ find_package(interactive_markers REQUIRED) find_package(laser_geometry REQUIRED) find_package(map_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(resource_retriever REQUIRED) @@ -120,6 +121,7 @@ set(rviz_default_plugins_headers_to_moc include/rviz_default_plugins/displays/tf/frame_info.hpp include/rviz_default_plugins/displays/tf/tf_display.hpp include/rviz_default_plugins/displays/wrench/wrench_display.hpp + include/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp include/rviz_default_plugins/robot/robot.hpp include/rviz_default_plugins/robot/robot_joint.hpp include/rviz_default_plugins/robot/robot_link.hpp @@ -200,6 +202,8 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp src/rviz_default_plugins/displays/tf/tf_display.cpp src/rviz_default_plugins/displays/wrench/wrench_display.cpp + src/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.cpp + src/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.cpp src/rviz_default_plugins/robot/robot.cpp src/rviz_default_plugins/robot/robot_joint.cpp src/rviz_default_plugins/robot/robot_link.cpp @@ -250,6 +254,7 @@ ament_target_dependencies(rviz_default_plugins interactive_markers laser_geometry nav_msgs + nav2_msgs map_msgs rclcpp resource_retriever @@ -272,6 +277,7 @@ ament_export_dependencies( laser_geometry map_msgs nav_msgs + nav2_msgs rclcpp tf2 tf2_geometry_msgs @@ -335,6 +341,7 @@ if(BUILD_TESTING) set(TEST_TARGET_DEPENDENCIES map_msgs nav_msgs + nav2_msgs rclcpp sensor_msgs urdf @@ -572,6 +579,17 @@ if(BUILD_TESTING) ament_target_dependencies(pose_array_display_test ${TEST_TARGET_DEPENDENCIES}) endif() + ament_add_gmock(particle_cloud_display_test + test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_test.cpp + test/rviz_default_plugins/displays/display_test_fixture.cpp + test/rviz_default_plugins/scene_graph_introspection_helper.cpp + ${SKIP_DISPLAY_TESTS}) + if(TARGET particle_cloud_display_test) + target_include_directories(particle_cloud_display_test PUBLIC ${TEST_INCLUDE_DIRS}) + target_link_libraries(particle_cloud_display_test ${TEST_LINK_LIBRARIES}) + ament_target_dependencies(particle_cloud_display_test ${TEST_TARGET_DEPENDENCIES}) + endif() + ament_add_gmock(pose_tool_test test/rviz_default_plugins/tools/pose/pose_tool_test.cpp test/rviz_default_plugins/displays/display_test_fixture.cpp @@ -924,6 +942,21 @@ if(BUILD_TESTING) rviz_visual_testing_framework::rviz_visual_testing_framework) endif() + ament_add_gtest(particle_cloud_display_visual_test + test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_visual_test.cpp + test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.cpp + test/rviz_default_plugins/publishers/particle_cloud_publisher.hpp + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET particle_cloud_display_visual_test) + target_include_directories(particle_cloud_display_visual_test PUBLIC + ${TEST_INCLUDE_DIRS} + ${rviz_visual_testing_framework_INCLUDE_DIRS}) + target_link_libraries(particle_cloud_display_visual_test + ${TEST_LINK_LIBRARIES} + rviz_visual_testing_framework::rviz_visual_testing_framework) + endif() + ament_add_gtest(pose_display_visual_test test/rviz_default_plugins/displays/pose/pose_display_visual_test.cpp test/rviz_default_plugins/page_objects/pose_display_page_object.cpp diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.hpp new file mode 100644 index 000000000..6ba0ecee1 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.hpp @@ -0,0 +1,83 @@ +/* + * 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. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ +struct OgrePoseWithWeight; + +class RVIZ_DEFAULT_PLUGINS_PUBLIC FlatWeightedArrowsArray +{ +public: + explicit FlatWeightedArrowsArray(Ogre::SceneManager * scene_manager_); + ~FlatWeightedArrowsArray(); + + void createAndAttachManualObject(Ogre::SceneNode * scene_node); + void updateManualObject( + Ogre::ColourValue color, + float alpha, + float min_length, + float max_length, + const std::vector & 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 displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp new file mode 100644 index 000000000..f177472ad --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp @@ -0,0 +1,150 @@ +/* + * 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. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__PARTICLE_CLOUD_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__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" + +#include "rviz_default_plugins/visibility_control.hpp" + +namespace Ogre +{ +class ManualObject; +} // namespace Ogre + +namespace rviz_common +{ +namespace properties +{ +class EnumProperty; +class ColorProperty; +class FloatProperty; +} // namespace properties +} // namespace rviz_common + +namespace rviz_rendering +{ +class Arrow; +class Axes; +} // namespace rviz_rendering + +namespace rviz_default_plugins +{ +namespace displays +{ +class FlatWeightedArrowsArray; +struct OgrePoseWithWeight +{ + Ogre::Vector3 position; + Ogre::Quaternion orientation; + float weight; +}; + +/** @brief Displays a nav2_msgs/ParticleCloud message as a bunch of line-drawn weighted arrows. */ +class RVIZ_DEFAULT_PLUGINS_PUBLIC ParticleCloudDisplay : public + rviz_common::MessageFilterDisplay +{ + 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 displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__PARTICLE_CLOUD_DISPLAY_HPP_ diff --git a/rviz_default_plugins/package.xml b/rviz_default_plugins/package.xml index d8f0c55e4..67f988f5a 100644 --- a/rviz_default_plugins/package.xml +++ b/rviz_default_plugins/package.xml @@ -28,6 +28,7 @@ interactive_markers laser_geometry nav_msgs + nav2_msgs map_msgs pluginlib rclcpp diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index 5cf6a1e34..c2669dd06 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -264,6 +264,17 @@ geometry_msgs/msg/PoseArray + + + The Particle Cloud display shows a nav2_msgs/ParticleCloud message, as a collection of arrows scaled according to weight. + + nav2_msgs/msg/ParticleCloud + + +#include +#include + +#include +#include + +#include "rviz_rendering/material_manager.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +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 displays +} // namespace rviz_default_plugins diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.cpp new file mode 100644 index 000000000..8458852f3 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.cpp @@ -0,0 +1,412 @@ +/* + * 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. + */ + +#include "rviz_default_plugins/displays/particle_cloud/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 "rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ +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 displays +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::ParticleCloudDisplay, rviz_common::Display) diff --git a/rviz_default_plugins/test/reference_images/particle_cloud_display_with_arrow3d_ref.png b/rviz_default_plugins/test/reference_images/particle_cloud_display_with_arrow3d_ref.png new file mode 100644 index 000000000..2d5fdf992 Binary files /dev/null and b/rviz_default_plugins/test/reference_images/particle_cloud_display_with_arrow3d_ref.png differ diff --git a/rviz_default_plugins/test/reference_images/particle_cloud_display_with_axes_ref.png b/rviz_default_plugins/test/reference_images/particle_cloud_display_with_axes_ref.png new file mode 100644 index 000000000..a5ff4a1be Binary files /dev/null and b/rviz_default_plugins/test/reference_images/particle_cloud_display_with_axes_ref.png differ diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_test.cpp new file mode 100644 index 000000000..f644bfa15 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_test.cpp @@ -0,0 +1,222 @@ +/* + * 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 (subject to the limitations in the disclaimer + * below) 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. 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 HOLDER 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. + */ + +#include + +#include +#include + +#include +#include +#include + +#include "rviz_common/properties/float_property.hpp" + +#include "rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp" + +#include "test/rviz_rendering/scene_graph_introspection.hpp" +#include "../display_test_fixture.hpp" +#include "../../scene_graph_introspection_helper.hpp" + +using namespace ::testing; // NOLINT + +class ParticleCloudDisplayFixture : public DisplayTestFixture +{ +public: + ParticleCloudDisplayFixture() + { + display_ = std::make_unique( + context_.get(), + scene_manager_->getRootSceneNode()->createChildSceneNode()); + + for (int i = 2; i < 6; i++) { + common_arrow_properties_.push_back(display_->childAt(i)); + } + } + + std::unique_ptr display_; + std::vector common_arrow_properties_; +}; + +nav2_msgs::msg::ParticleCloud::SharedPtr createMessageWithOnePose() +{ + auto message = std::make_shared(); + message->header = std_msgs::msg::Header(); + message->header.frame_id = "particle_cloud_frame"; + message->header.stamp = rclcpp::Clock().now(); + + nav2_msgs::msg::Particle particle; + particle.pose.position.x = 1; + particle.pose.position.y = 2; + particle.pose.position.z = 3; + particle.pose.orientation.x = 1; + particle.pose.orientation.y = 0; + particle.pose.orientation.z = 1; + particle.pose.orientation.w = 0; + particle.weight = 1; + + message->particles.push_back(particle); + + return message; +} + +TEST_F(ParticleCloudDisplayFixture, constructor_set_all_the_properties_in_the_right_order) { + EXPECT_THAT(display_->childAt(2)->getNameStd(), Eq("Color")); + EXPECT_THAT(display_->childAt(3)->getNameStd(), Eq("Alpha")); + EXPECT_THAT(display_->childAt(4)->getNameStd(), Eq("Min Arrow Length")); + EXPECT_THAT(display_->childAt(5)->getNameStd(), Eq("Max Arrow Length")); +} + +TEST_F(ParticleCloudDisplayFixture, setTransform_with_invalid_message_returns_early) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + msg->particles[0].pose.position.x = nan("NaN"); + display_->processMessage(msg); + + auto arrows_3d = rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()); + auto axes = rviz_rendering::findAllAxes(scene_manager_->getRootSceneNode()); + auto manual_object = + rviz_rendering::findOneManualObject(scene_manager_->getRootSceneNode()); + + // the default position and orientation of the scene node are (0, 0, 0) and (1, 0, 0, 0) + EXPECT_THAT(display_->getSceneNode()->getPosition(), Vector3Eq(Ogre::Vector3(0, 0, 0))); + EXPECT_THAT( + display_->getSceneNode()->getOrientation(), QuaternionEq(Ogre::Quaternion(1, 0, 0, 0))); + EXPECT_THAT(manual_object->getBoundingRadius(), FloatEq(0)); + EXPECT_THAT(arrows_3d, SizeIs(0)); + EXPECT_THAT(axes, SizeIs(0)); +} + +TEST_F(ParticleCloudDisplayFixture, setTransform_with_invalid_transform_returns_early) { + EXPECT_CALL(*frame_manager_, getTransform(_, _, _, _)).WillOnce(Return(false)); + + auto msg = createMessageWithOnePose(); + display_->processMessage(msg); + + auto arrows_3d = rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()); + auto axes = rviz_rendering::findAllAxes(scene_manager_->getRootSceneNode()); + auto manual_object = rviz_rendering::findOneManualObject(scene_manager_->getRootSceneNode()); + + // the default position and orientation of the scene node are (0, 0, 0) and (1, 0, 0, 0) + EXPECT_THAT(display_->getSceneNode()->getPosition(), Vector3Eq(Ogre::Vector3(0, 0, 0))); + EXPECT_THAT( + display_->getSceneNode()->getOrientation(), QuaternionEq(Ogre::Quaternion(1, 0, 0, 0))); + EXPECT_THAT(manual_object->getBoundingRadius(), FloatEq(0)); + EXPECT_THAT(arrows_3d, SizeIs(0)); + EXPECT_THAT(axes, SizeIs(0)); +} + +TEST_F(ParticleCloudDisplayFixture, setTransform_sets_node_position_and_orientation_correctly) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + display_->processMessage(msg); + + EXPECT_THAT(display_->getSceneNode()->getPosition(), Vector3Eq(Ogre::Vector3(0, 1, 0))); + EXPECT_THAT( + display_->getSceneNode()->getOrientation(), QuaternionEq(Ogre::Quaternion(0, 0, 1, 0))); +} + +TEST_F(ParticleCloudDisplayFixture, processMessage_sets_manualObject_correctly) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + display_->processMessage(msg); + + auto manual_object = rviz_rendering::findOneManualObject(scene_manager_->getRootSceneNode()); + auto manual_objectbounding_radius = 4.17732; + EXPECT_THAT(manual_object->getBoundingRadius(), FloatEq(manual_objectbounding_radius)); + EXPECT_THAT( + manual_object->getBoundingBox().getCenter(), Vector3Eq(Ogre::Vector3(0.85f, 2, 3.3f))); +} + +TEST_F(ParticleCloudDisplayFixture, processMessage_sets_arrows3d_correctly) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + + display_->setShape("Arrow (3D)"); + display_->processMessage(msg); + + auto arrows = rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()); + + // The orientation is first manipulated by the displays and then in setOrientation() in arrow.cpp + auto expected_orientation = + Ogre::Quaternion(0, 1, 0, 1) * + Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y) * + Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_X); + expected_orientation.normalise(); + + EXPECT_TRUE(rviz_default_plugins::arrowIsVisible(scene_manager_->getRootSceneNode())); + EXPECT_THAT(arrows, SizeIs(1)); + rviz_default_plugins::assertArrowWithTransform( + scene_manager_, Ogre::Vector3(1, 2, 3), Ogre::Vector3(1, 1, 1), expected_orientation); +} + +TEST_F(ParticleCloudDisplayFixture, processMessage_sets_axes_correctly) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + + display_->setShape("Axes"); + display_->processMessage(msg); + + auto frames = rviz_rendering::findAllAxes(scene_manager_->getRootSceneNode()); + + auto expected_orientation = Ogre::Quaternion(0, 1, 0, 1); + expected_orientation.normalise(); + + EXPECT_TRUE(rviz_default_plugins::axesAreVisible(frames[0])); + EXPECT_THAT(frames, SizeIs(1)); + EXPECT_THAT(frames[0]->getPosition(), Vector3Eq(Ogre::Vector3(1, 2, 3))); + EXPECT_THAT(frames[0]->getOrientation(), QuaternionEq(expected_orientation)); +} + +TEST_F( + ParticleCloudDisplayFixture, + processMessage_updates_the_display_correctly_after_shape_change) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + display_->setShape("Arrow (3D)"); + display_->processMessage(msg); + + auto arrows = rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()); + auto frames = rviz_rendering::findAllAxes(scene_manager_->getRootSceneNode()); + auto manual_object = rviz_rendering::findOneManualObject(scene_manager_->getRootSceneNode()); + EXPECT_THAT(arrows, SizeIs(1)); + EXPECT_THAT(manual_object->getBoundingRadius(), FloatEq(0)); + EXPECT_THAT(frames, SizeIs(0)); + + display_->setShape("Axes"); + display_->processMessage(msg); + auto post_update_arrows = rviz_rendering::findAllArrows(scene_manager_->getRootSceneNode()); + auto post_update_frames = rviz_rendering::findAllAxes(scene_manager_->getRootSceneNode()); + EXPECT_THAT(post_update_frames, SizeIs(1)); + EXPECT_THAT(manual_object->getBoundingRadius(), FloatEq(0)); + EXPECT_THAT(post_update_arrows, SizeIs(0)); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_visual_test.cpp new file mode 100644 index 000000000..9857916b5 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_visual_test.cpp @@ -0,0 +1,65 @@ +/* + * 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 copyright holders 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. + */ + +#include +#include + +#include "rviz_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" + +#include "../../page_objects/particle_cloud_display_page_object.hpp" +#include "../../publishers/particle_cloud_publisher.hpp" + +TEST_F(VisualTestFixture, particle_cloud_visual_test) { + auto path_publisher = std::make_unique( + std::make_shared(), "particle_cloud_frame"); + + setCamPose(Ogre::Vector3(2.7f, 0, 8)); + + auto particle_cloud_display = addDisplay(); + particle_cloud_display->setTopic("/particle_cloud"); + particle_cloud_display->setShape("Arrow (3D)"); + particle_cloud_display->setColor(254, 182, 6); + + particle_cloud_display->setMinArrowLength(3); + particle_cloud_display->setMaxArrowLength(3); + captureMainWindow("particle_cloud_display_with_arrow3d"); + + particle_cloud_display->setAlpha(0); + captureMainWindow("empty_scene"); + + particle_cloud_display->setShape("Axes"); + updateCamWithDelay(Ogre::Vector3(7, 7, 7), Ogre::Vector3(0, 0, 0)); + particle_cloud_display->setMinArrowLength(6); + particle_cloud_display->setMaxArrowLength(6); + captureMainWindow("particle_cloud_display_with_axes"); + + assertScreenShotsIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.cpp new file mode 100644 index 000000000..37731fa5b --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.cpp @@ -0,0 +1,70 @@ +/* + * 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 copyright holder 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. + */ + +#include "particle_cloud_display_page_object.hpp" + +#include +#include + +ParticleCloudDisplayPageObject::ParticleCloudDisplayPageObject() +: BasePageObject(0, "ParticleCloud") +{ +} + +void ParticleCloudDisplayPageObject::setTopic(QString topic) +{ + setComboBox("Topic", topic); + waitForFirstMessage(); +} + +void ParticleCloudDisplayPageObject::setShape(QString shape) +{ + setComboBox("Shape", shape); +} + +void ParticleCloudDisplayPageObject::setColor(int red, int green, int blue) +{ + setColorCode("Color", red, green, blue); +} + +void ParticleCloudDisplayPageObject::setAlpha(float alpha) +{ + setFloat("Alpha", alpha); +} + +void ParticleCloudDisplayPageObject::setMaxArrowLength(float max_arrow_length) +{ + setFloat("Max Arrow Length", max_arrow_length); +} + +void ParticleCloudDisplayPageObject::setMinArrowLength(float min_arrow_length) +{ + setFloat("Min Arrow Length", min_arrow_length); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.hpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.hpp new file mode 100644 index 000000000..9a71f453b --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.hpp @@ -0,0 +1,49 @@ +/* + * 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 copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__PARTICLE_CLOUD_DISPLAY_PAGE_OBJECT_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__PARTICLE_CLOUD_DISPLAY_PAGE_OBJECT_HPP_ + +#include "rviz_visual_testing_framework/page_objects/base_page_object.hpp" + +class ParticleCloudDisplayPageObject : public BasePageObject +{ +public: + ParticleCloudDisplayPageObject(); + + void setTopic(QString topic); + void setShape(QString shape); + void setColor(int red, int green, int blue); + void setAlpha(float alpha); + void setMinArrowLength(float min_arrow_length); + void setMaxArrowLength(float max_arrow_length); +}; + +#endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__PARTICLE_CLOUD_DISPLAY_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/particle_cloud_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/particle_cloud_publisher.hpp new file mode 100644 index 000000000..479840dd2 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/particle_cloud_publisher.hpp @@ -0,0 +1,90 @@ +/* + * 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. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__PUBLISHERS__PARTICLE_CLOUD_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__PARTICLE_CLOUD_PUBLISHER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/clock.hpp" +#include "std_msgs/msg/header.hpp" +#include "nav2_msgs/msg/particle_cloud.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace nodes +{ +class ParticleCloudPublisher : public rclcpp::Node +{ +public: + ParticleCloudPublisher() + : Node("particle_cloud_publisher") + { + publisher = this->create_publisher("particle_cloud", 10); + timer = this->create_wall_timer( + 500ms, std::bind(&ParticleCloudPublisher::timer_callback, this)); + } + +private: + void timer_callback() + { + auto message = nav2_msgs::msg::ParticleCloud(); + message.header = std_msgs::msg::Header(); + message.header.frame_id = "particle_cloud_frame"; + message.header.stamp = rclcpp::Clock().now(); + + for (int i = 0; i < 3; ++i) { + nav2_msgs::msg::Particle particle; + + particle.pose.position.x = 0; + particle.pose.position.y = i - 1; + particle.pose.position.z = 0; + + particle.pose.orientation.x = 0; + particle.pose.orientation.y = 0; + particle.pose.orientation.z = 0; + particle.pose.orientation.w = 1; + + particle.weight = 1; + + message.particles.push_back(particle); + } + + publisher->publish(message); + } + + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr publisher; +}; + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__PARTICLE_CLOUD_PUBLISHER_HPP_