From 77b94a5deaca8b7a31e630b7fc7f2cf6714ccf4e Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 7 May 2020 00:01:47 +0530 Subject: [PATCH 1/2] Add nav2_msgs/ParticleCloud display plugin --- rviz_default_plugins/CMakeLists.txt | 33 ++ .../flat_weighted_arrows_array.hpp | 82 ++++ .../particle_cloud/particle_cloud_display.hpp | 149 +++++++ rviz_default_plugins/package.xml | 1 + rviz_default_plugins/plugins_description.xml | 11 + .../flat_weighted_arrows_array.cpp | 129 ++++++ .../particle_cloud/particle_cloud_display.cpp | 411 ++++++++++++++++++ ...article_cloud_display_with_arrow3d_ref.png | Bin 0 -> 23138 bytes .../particle_cloud_display_with_axes_ref.png | Bin 0 -> 46511 bytes .../particle_cloud_display_test.cpp | 221 ++++++++++ .../particle_cloud_display_visual_test.cpp | 64 +++ .../particle_cloud_display_page_object.cpp | 69 +++ .../particle_cloud_display_page_object.hpp | 48 ++ .../publishers/particle_cloud_publisher.hpp | 89 ++++ 14 files changed, 1307 insertions(+) create mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.hpp create mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.cpp create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.cpp create mode 100644 rviz_default_plugins/test/reference_images/particle_cloud_display_with_arrow3d_ref.png create mode 100644 rviz_default_plugins/test/reference_images/particle_cloud_display_with_axes_ref.png create mode 100644 rviz_default_plugins/test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_test.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_visual_test.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.hpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/publishers/particle_cloud_publisher.hpp 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..e636eab8e --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.hpp @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ + +#include + +#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..29476fc8f --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.hpp @@ -0,0 +1,149 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__PARTICLE_CLOUD_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__PARTICLE_CLOUD__PARTICLE_CLOUD_DISPLAY_HPP_ + +#include +#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..3f830d493 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/particle_cloud_display.cpp @@ -0,0 +1,411 @@ +/* + * 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. + */ + +#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 0000000000000000000000000000000000000000..2d5fdf992171da346ffb35c3c55ab80a4fb54860 GIT binary patch literal 23138 zcmeHPXH=6}w@!f&AwUqMgc@`Z3lbp$0+J9gU_%{eEZDG71O<=*=L_!p1seT zxXjB#U6rDW!C=%Ec)I_L!QfY6Ft`>X4m`PAd+$#S#(UZVch_GcHf*8OXg>PNd@7BC z`{85qOr?|TEeGgIY&V}DK9u4F{xd@8Z_>d3#N>Z`V9Jr_TK|0X_lN|tzOkCol8zsy z0beJ&`4moa$n5eMHxcrfFn)f(4x5;qs+@YEUr5$h=YIE*NTt=Q8Z8+dKM(R4KmIWV zH+ixWyAtQI;_LUTL0GRK>+b;bjC?l*oDq5a{me2(GPH2~EnNPz$RDHzzHTNa-@*ar~Mr^v|;&#%?zQRAEyTbPZC$SQsjSk{5;5G%8z>nxiO&9 z9CaNs>&C0W6ja~dj-LlnH}(IhZpo;+b&lU}g}R|ys8BbBx+$!i!n!G(DR6F+70wjK zbP8vxh}{&i8_XaSse&R^P-LbsVO69Gic~?7DkxF~MXI1k6;SL_fxG>`g?VT4h2Owr z<~{PI!TZ&fR=Wq4UyWFk1~-PDj;L~>)XQ;GPZk8{b!4b4XC+nK*Z$nJRDbcy#>)#z z@%25Qr1O ztiGNZ<#$H-B6%(lAg7xkWsKyvgJJ8GSsPW5FvG!W zP{zzdS$rld6ma6Gn4k~4LTO{eYS&x3OQmGO$`Q_Vu6E^iQuh@)v-cV!^_g+ng=z%T z$JV3Ra%>6;x?`@gL?eUUdR=4TwSGQv*_8rfL1#CE5dE38{9v5$&O71na}k7``2zYw zi+ovOAUB+dF^rvZQw{Rf0r|2*`O1?QFhp*c8|#Pq>|g$RvaiZNw6aP~)ngDa(gqo6 zL1RsC`Dj`k#%V^wuep1-Iv=U3N-47(nJQ3@<&S`6_e1itQ9(_W3rdN_8CtMa9L?&F ztmMZFlP~l_y49%TPLeaagLpvtQ1FCwJv#POrmohDSuvX0I4ibOvTs;d1YL%)|FaxX zOH(S5j7p6Y(t|j+h--33VlTgDx(Di&P0ipQchl87S7VIV)7enO*ysDj|0_WJGYnxV z;ibHk5T?#;65YT@NhO*EUz}7~n@e7J)L04LUNwi=mdgDkS;g*lGO4)<<~jiP zi?-`jcdyX1^Rd4d1=c0HsDhXMPwe?5$4ZhRfUz&3{C?!4`K<8)T#S`i!Q zVqdpEk@N+TwZ>vn{q__)tjD4>z?K$OV3&J-W;Vf`*3UIuv6{H!=Le3fFIkS6BygO( zIH8ryP!Z4rKq2%Cpl}sZU>-PDl=e(HO;-ns0B?#D*qQ&eWcrX{!pYApxA%i!1u7qU z=j`w8G{lEA%}$T}JfZT@p-+6FGc4bngt~+&=<9SU|Myb<@Mc*2agV`>&5N!v^o!=k zo%CCsJud_tGg0QrDw|4sfZw_8)o-`PipXNO|vG4AGu(91!+#S8@lt{b5RBP=CO zaPPS^c%CG!66Z4O(_!b+$q`{aBL9_jXYVAIa*1KKy)$nlweB~=Wv?R!>EO=iF|-c! zWCw;d_4pq*ROh-*M05k2kf6CypR)ze-zzUuaj2NntcMhhoD#{!eM7Z(A1(;VNxIt) z2z`Cp)G{qUWB-Taq>zPQ%Xr8kyaN0%UK^4k4%+7!>~tAsWVWIqWD3OYe)s{SMurW8vvQEphFtRcPCdUiax9#$iM8UlzVLjJs zH@1w5mFqhr2Mso>C8!mz?jCj`lY=0SydxLU!+-SB*BHozH8>CQW{as zEh~~#ZKl22BeWdaOlAxM%3)ZNXMw3{rXHZIHy5e)xf2t_>gK^ezy7_vTicVTSy#`y zRNU9X5a$N?WTlhc!s|HBF~Z{|PsRYgH<&GJ#uv8p`No#2qPnsPdIP{|>5C zuj4tV9G7n}x@2K|uyT`equXD&fPu)72WC@dsyb`1f8JX7#y;~RV8JpLS&Gl*kM@65 zU2VVS(f0(dFkcgE``aUrYqhsLnDbHst=@8CCV2BSU5ew|^doL)T0XP3oGz%TdHi^; zW4(Q51|XCG4YrTD#%llRmX^*DVjp@tTnwq3T-0-tn-xgDihpFAb7+panc4{FNHJJV zh+%wH2dByZg0VWc8P&i;3$R!$9?uhAp~EAS>?Z1bagjvA-x}bF4$ShcD%TspW<{2c z(Grr9a9NT3zGD8VFuGD)AX+^efb4@4J4`m)?V`8phGtQ|rloSg0M-B*x&{%CC+h#? zWo6PSGUxUHXBNSvRAtI8D^dCTpIT!h1-_=s3_=7;7b+DNYgU=8Z??}g0M^SL%8UL$ zUi0)JVQtz%XW!>Km#ZC(wI3OpYrsA+m?E!C|D&o3&M4=k8Mb`E+dOWQLt$(r`Jq7Q zV#eDa#^{z+ewkoP!7q|zby%!pEaum^!PF3S;I1tJ9aaLDNH5@}Gng z2#bPqHm;~ss<@l4+0r1~+bE22f(K_4DyUkyOwW-8U`SU9GNi&04tWUOn8ihz8`G~M z3vL1`7FC=kWKs&)Ss^)hKDs%7$_UlYQ3|p8mhz}ZwvO*A+(em4 z9#>n(#vuzc|BuCWEws2^3yI4^47d(OaUE)wqL)L_%b{o;f;L;xIs`k_6s<%5(@r() zabW@)Ekb9!p6H%=K#6itI;S!*{u93$%}ppc-gKyD6LxS4!Z)_p6^%5xnr?Opn=fjL zBBi&P^?YU(vd_kdv~v>$8D&C{^9O`O-VdWo&XNB%kVvzgO|#t;tcRPQ7(h8^r`mLX zY)Iph*45<>j^WOkS71y)L~)P|d8mPj$NYSc+%fTRJgLA=^(x&2pZ2@KTQIx`4i}@* z&tCa(5T0I-CuJDM-c?yQ-&sENd8(L3A*{mJ&W)|ynVh|5 zUZxfZXe!WnStyH_F+1EuBi8A|SE|hp$DV3CyJR$-D%LSH!#NTo_=(X&M}ip!C1XCG zTYMt=y*t`o2m!9r_Yj2hP+>Gy8+}|jYE-ic?q0c*(#v531Ot(@N9Eej4%Xt{Qd^_n z%w^s?Zk|nb6Xk8q#dG3cnaxpcGWesoJV*z@mwaD?4e+H3{C#|Al*|2^+{Ghl-q)A1 z-Ozr`*YSd@Q>CX#C#(7_W(0}SV`o{-7rDkU!&@AmlItv$raOdsv`O^h5XNi~Y=H+G z8c%v|-Lg&ej#A?)Q%#Pw(!ugB%CKaRvCg>uq`B`REh?+!lWnVx+d4mk>ojTnV$=N; z-oE^FcJqieD|}-}ik+&*iylA`@Q0uk6>qkuXeg!XQUFDB7ANJ=`I)|pGKMbu`6!Xs zT}%j^e(KCI*YtSTpF7TE(mSfR7qXi>8hZQ(z)7cqR#zmOw+KEXoW$k7hgmB|!ot-3 zTw$9_3{&knq*Ku4(%gobqq@z#G4Szy{Ih8)t1*MdwheOV&uFU2^|VKWaGO!I0pp%j zSev%AiG2gjfQq2vj_lK+J)LRf)5vCKcVfK@73Yyj0@n6QLPI0r-<3@`dI`wfhm8+jYF0 z1dL1#7#kB%3l$J7vrxbyI8vFddvtXz2-|wXKhz2KPYm4?K>6unx)`xsFsC;6J zsP1K8KRWcFR6KWtXWP=oMi`mZXw=OjI!E+uObDizPCiEom{%>;+&Cs3dMIvtAvG;6 zJt(P>bV&TOA;-CBmZ{zFcLIRyGlnQk~8uJfyM89IM$BI$;IVvD??0fTXiEEwl0piHfuhJS1g3pTX;Z+$? zQhg<*O*Jm!HE~z?3s7K&i6Ti4qT1(FR;EeWE5;xlPa;;U8(;6|@kRiAsm|eY}n>zX{{WpOO5&)>BWM*J9)+%o4 zOPpW8g5OlUdEL*{mhm)qcN^uuQE5<~X*tDw+Ebe2bEnJ-AU|_{SN*{11DT$-XY}{HSY^SspTH$n1$(jf-f_#p2Q2y&Jr|o(X-M{Kr6QdDaBk zQf{KFPiKIS4?w^>e%;OARt4y`lOy^F4y|dCFLOkDyuIVq#_UXM_f&(VKBVxyedZRx zx&t&#W?MoFosF%Q00o;8BjB%+3N;p=y>V@V-H=(A>&^k}iNOU)$FaunJ)ly{M>{iY57)!^!zse+n$h;aTBSg=eT3t*v+ulJtksxPTZUB7OOYd^wy zIx?g@pIYOR+albXD0BwfFxeJ*;%QMbJ&$7($&4zMK~3))nh^_cR3Vx5KA2~6Q1faf zv-bcd=Kzq2%dNIOYqrD1TxD2HA1U%^-|L}0geV9m6?I@KaFak{zKNJHVWQpXWLK9f zH6l`{+DGAvg?_m#s2hIw-iKg~J?;`Pr?p^bt-pe*_Q*W(yDpo#II5 zZ1+t%5t0mUVkbb;%w~v#-wNIn!7M^aG5_)&gc_$8b&2P`akFm!sB~;bUA0Hx0Co-@ z@IBa8 ztK4_PuL@KFXw!Fi42(NvABr0yiW?%xMJUCEB*ld!#DSy9lHx*=%ooX{Z20wmw=n|L z9z2hv@ta}c0E93+Tcy?P$-dtLbvDwVuyog$$BqZd%s!ad%bOZRvVei64iehUIrDNRlxHn1d)^d^q)96pp>GtR+pR2z`q3T%icNCRmD~csHO5%fn8r=d zm~JaFcCLR)*?YeskI7r)VT>-Uom8NJKdhaMSdaa?gX7ITH=0ZiERk#-`iifJxviE# ziOubSAb<`UXXKy}M4{Z_2NFXHY(u!K2(0?plPFw(j{M$ljk;l?_#p^Fp$R>F?JV?m z<13llC!GV4w+?Ro0A

O+H4r1q@+5&0u#5^CX2!(al$O>tDglQKbBwYc4Scx6J_ zo@AeAW=NI(!drwV>_SwG2j#;-Z^)OAt5*7orZYQA(o3aGU03fIQ1(~ZUFC1wlIhy^ z-UvYu4nwhfs-ZO~G!9osD$B9=q{jVc+iZEkft%wR-&7E9PkQYbW3R%!>UKCY&Yh07 zONimzpHOdYAm35}X!_8znVi?>PBJUSX z!KlZh(>9N;mxVVSEa5eTw-1g4ijxdhz-Z08!Uez zN8#=$%}wDp3j@z)Exp4Au+4BFsEtBds92N)-ONt*JqGnQgNzHRICi}>W0oF*QS7c- zRu}Sn{_O=J0P+}SnCf-->MS1%e>7c={k#D;jtQZNOjXSG6#sTGwe9cD6`B$uzd3;( zlCXPVj?)aDnP%16!>n93HGPr~_Oz34kz`%lgGI1MhYng#cvi@6{0y79mwa5pohW zm`a$y*kGg*T8y@~=&(44<0h+pz}VgbZ{(cz^miEO5Q6| zgA2wY(scACaP^FOf+}74>Z!duKEFRPz!^`<;`^v)ksyq{xgWlj2cE zK+a45t>{rk*OdOc%h^xVKFY{^f?6;Z@xYKxg3ZV)QwQFtn#tvEhx_7A!-8GJRTD_* z;W@5+4)e_IY_+J-$9-*1Dl`7wvBsAF{sG8dnNZLcZz8`sW;UR-+&I{!H%R)9Mc%#} zOWp;pnb>5*-=~y2=Zjfe>8Mz$7Ln6ec}>bVpf@bdX)b2~)WG2No3mDomA8aPVP;RG zp+|GC>}Z1I6_XcnZ)8-8xmE4|@+B{M7tRAv!)NUaIV9RqVsC1&`n9`B$rjN&J?xWc z!stoYf35adfkGc5SgplWlqcRZ3OAJ+)__bANnEK)?nC-FGnU1n6#e@JjoVV)_b~y)wVFXD0UNlTqR=DG6x4O zgFigjf(^{lv{VZkz%BsO7t!01#4`wZyK)Pt&)Ua6`Oq>mP?O7?k?y#m;-=&{>qW+i ze%D9CW`wcfrbe+?8eId4SJpz zujPJyF;M_ILJ(#jz-`7(fTpHQ-su3kJho<^;H}lE`-HD|bQIZV4#CL?sFWVd(N55+ zk`~=b4V05I!R&&hH|W~l1dAmuB(Ge zQmM8khf<*fp_)Tdk(JcSVZZ0Lx_V!~_wDxmy>Fk-@B2C2KA(TwX1DG2d_JC!)BSOO zJf6!6TENiMFxG$|NOP{g?_vl-EQ26;HwF&gx!!c+DFj{FHrJQFByn|w3x&cYph_($ z#&Gawz+1u{L!lRzUyu<9OPDVs)-okNpWZD*^UeJ`M{&4gVV!$$I0wFeHT zEh9}%GoHY=5V{!u`SU5tNB#IirXc-Ra!MOgT#$Y~%zyktP`sb@^DX{dOy-2{iQNl* zQF9oNYoamPd|@;OcOOqM#S#PC{`t|mKFkVzx1tY>zdsc$qIetq|5{A-uOB8m7nOVR zGuW|m+rR!~Sy0+Fh=E`Kc^$>uh<|?D7)~Z5{p#S1;O{^7s@$;ZPYs?B2i6(==XDfs zD-P_}%0FHe=)g-OIHk6`{_%)ZyAH=>SHc;O$Nza9#oOSN{$XXsp@DzvF`VX$!sh*+ z|ELP35M)QCKHmPbJ{0PR`KgY-9baUcus=i+Jik-Dl7RH z%z^5|9-ZR)8@2YqgV{yCKh+VUQb*W7tHU41XGNHx7kk*j{iS+ z@IRQ)|3%ejm*HhOt(T86CW(J~zCal^t-t;>g;+CtX~lnBw9rp?*0*QVVxjS<`M%QbTVCqN6LR4>(2!Ezj#CyiT<0z)Bh;*(ggm;+U&ymX+CW#{1mSYA-GrgL{H;u zKaI{9?xr$3x1XQ><)#1S%ua~mPGs(&wxAEEi4vS8|x|Ln~>94p3 zC8?n2&!SC&gDs*rH#ezDLrqSXD)Ls${_)6Pr}FPFM*hRJ{f}PpFQVQM%vf#Lt4*c0 zVL!FKRC!RrV}BjpUr_5`ObLGz_rFcW__tmEXJqHUVXMEDCj70K&@K6jp|p^!1r(=Kb8zc~xyysZ3$~I1Ecpj1%Wr|J<<9NC|Xm9RG*8 z+duL^e-El`yo_pm|5w=QSCyEPe!Rohfe-nKBY!w4KUL+V|90H=H*^1ACl&txP((ru zOtP`&p`RY`P^G|EN(G8%Aw6?zhyIGG&=-{h|HUZ$(#OB(z}epHVYN5W$+=3juJ|L- zQ1WRNKYhy0hrMrCg{gZHRhiI%6DlaWNniik#Qz5NU+(sQvF!S35K0yQEwKL$X#G7L z{H0Yz0sh~IY82uV-HkPV9_yHovSR&RRk4By=kkAua6fBO@a2CuPpV@8Pi4uzzK@>w zllUvpS0)@iRl$AMM=}^a_RDpc+Dhu_-p{I)2z-}I3^e}fy9#uv{u5n7qpsUD#%muw z^epoHslh6hmg4@+wfX-fBl&3_|8v_DzoPwr5*74c+1wJ-+5MXqU7Brq&w5r7@u!Co zGL*#9Clx#V%tD$Ce>K}E(By|P36-reX+QrWQ20lL{XexG_`m)J8r#rL3~Te| zJoJN~MP!c>gZ`+=x`^;jFUHD*Qd?D?b3vA8K$p|fYNFeIHfW?+&M`0ftz=v2z#j%; z11x&&B6Q9TTr6|VB_~Sxo$AqpoG$I`@f=PUws3qO{SBITR}=m4usPD*?G}rbZT(kq zc)&s!&16!Hul#h3Bi>Nk#Li(q>Bxo{G`gNf@x=T~MwNUDe<})-jlHu^utiQ^mI9xc! z9B#rL=)^5B4bmxF%B+t=?|F)wyaB)7+&Q}v#n@b@JR6pl5)OP<#TqROTGhDPCo2yG z!W_X0Pi#h^-zVimRr-ZrkBD|n;q_XgrO~9MXypUyRSy6L2MCyU)dSisXe&mwpI;DG z3g(V~=HdDd*?8a8C&N zM#s^YqtPU8qfUS$#ea4p<@)wWmr>OdU#N z3nMBaRIiF6i}aR=GuCLE**H>!3*FOAVZ3g{ou1T?*^RlDvlPPVtAw*q3inoJRG2g< zr#;onvD#Dd32oIURK2G&D)ECd6ymq(P%IN)8zRJ~j^Q@FbP^EBnvPH_(h-q(QPQ6z zYNk3XqZ-v)IY+Tq8JW>Bn5K&HIw6~&hFop>RgA7xI8BU(6BWX=whGO$4)J5zPE3 zCZ3^z2<35URXutXsT zG_Ini-zm{RzaAX%VgivgsSVfyWfaSM9B}$rf$>HI52KoC7b{Sl;Mz`th40-;$yQ4- zqk~&|EncBXcCjikZ&I3_i#BXUrJTKtU0o}MzE5bQB@%P-R{p>m+{Zkl5F>(&Kv~Jb zeKjI@I){XY;fF*m26}Y{{x0jWw+!@e-hc353W-wW@&gmFPpTfxR6QC8GrlVK3?sra zOUK{H_gH);^V+B6FZ);tr)!uwhYYO6d(mQ6;uG8(v}2F&*^o={w3nrGx~_8X1TuGS zt*~<|qAMR9Z2)i7)zvK({p{2((S|Wchh7J8GPHlYwQFA_lNrH;isdDkms>=;*BeTc z^(9fbsKrExDBr3568_2XH$z~%JN*Z}B?kF7aO+ETLWuSf7{oRg|}0BAdjOkm!NuxDo@XXt;cn~dxgWjG9nO+2u)OU zXL23fKy7bokm)^5EsS8%n*16LtPH1h#+A8uSgdwZTM_n)$7Q02A`wO72a032TTreu z5?umw6!mS8J2KNm9sxbN|7maZR|i(l&=kIXlTMRAS70YBDcR)f+jc9!$qPU3TdZw9 z%(|QqZ@!LM^-03fdWl%e?n9_qr1DI-g$HwmaSRhfWMwJ`6Bim}5Uax)!t}D}d^PP} z_UIzNIuzsbTcs6gs!aT>ayO?9;DkXhL4zVK5DcK!*Ui=Abq}@H=j}SxxOs>)>5_)V zn_DTnM!#R7ysRXZ>&r}{OwU)6`j8(Lu~|hEr_8udCT!=~^Dm|RZ*eq2( z^_t3xJ(N~#nE}~#q|)lDMQ(DGP38_Ht{)T48qGU6u_*J3olLL6bjt>8u9?g1B978n z|6x>)4N_*$7jwsDkTWfpZJ@ul;6R^gn}6La17DBmqHR6g#oA_yYo?+~D|$gT2#er&Exm!En=*CKpzaR0DxnO&mD!DL1~>N{OOn#EIP~y}amKc8_H+Zi@6s*}9koD2oJGVR4F1C=v%7!{J2BkH^7at6;cJ)Y&r)Gtz41tP>U$yp!KVYEzup+J-e(wj;y! z`j)tOG-@^8&$Mt_BU~9Pu{SzOHhZ8NWJIdY#9w(PGz&^%dn#fpkf8;ZuoCY{Z)BI+ z$ut{GtL(|>siIhH`081Apc^BEddTd&&0Qho`f8TTRi#v<4CUF(24~|Bc$P2bU-YC| z_tbe$0vM!cJw|$#m+Wlz^yVot^$u5M;`=qu2&Qe201e4>np~)GDLHoXHoUKgzJn=8 z)~Cil#acyETjr+Ff?TPEeAVQgI`{{aT4fb-^oC9?$qBL|%T3;-$N>;Y?O?#72BbXl zG-T!mxg(qLsJ`l0PW?1VJr$l*I7h$S&MinSQ0$Wc(pm))B+ddA?)eO;BeE5elFP15 znlLwQPtEh4^9uLJGzpr5Y8n*8G~OgjN4Tw`3G){oPAl|)S}>}av`b(nt;-yaXr4gu z-zh+VKL6Sq$ndd$*!_z;S)kmD&*jRk5(DIATYNwft`0st?dMWR$|7J)Uwce@l}~ z)7;2Vn%a{Y;L9tMiApj9=jf z!HLLo^7ZDwJ3-E8!VEs1CBTre7$;}S*jgrOXJ~$x2(bW_epqOQ0>yxp3W+zVqU$?T zP0n#(TZ6ZC3b)bM-LtVF{zxSVL~rBmQ{`+tL$hEL)7}t)=o662`!fzsgmiH6`-;g% zHrPt7gp)Wai_Y{3(u)2BysiSNc-{}Y0Lj_M0f`PA)dntZ=;1LOJ+EXTMhJW?7TdjI zdgi)Gj-3wx{9a`FNFmtcFF=-7m39Y#jPsPSl4A1`3^^Cexs`Im)3Kj1{jdcypUjRn z?mpy&!=(n@oT1RQInY<4%A3-GzM3$KPp8O2Nwslfd)}ENlz})WN$+&s-3d zmrrs`82;DUx!Rp;IEirzELIPM_%*H#xF5IP;u9c*>77@^ z8C@0-;WyAJ}U8BLu~+v1_V3=-GM+xWZ6#7kHI!8JDd zmS@zs-RU%Y_JiFOn@6S~5XocjJQ3n`<=j2Elp98g=P9%WRJ2A;8kUmgnJn2QrKxQj zB~o|z^L9=xJd=*?uny0>bd32NrzNAvvh8JMV&X3oyzGj-q5qvZYBmKVBv+Rp`(0m?;gPkFe z(Kgr{u40y-SIgq9k#btb2a=gyd}sTCNHo({>0 z0BPrk=>a84u*x#_53uwf=}_g;OdNto;G?!g%Pq`d_0)kLLWdqDsy+hgQEnH#*lV1` z+hIL9jUD**0s*lkI!^Syq@=U6lU!z3KQS_`<9%y12y&n$7Q0e*l441JQoj%&@kceK~q?Zu^em4Ku~dr|`8J zo5%@V!s+;$;^_*gW~UeyC8*%CQ63*!WWfeR%zF$$a`Q~D=KZegaHN~wR>afa$)McG ztg~GM&(wJ!Xez$LdXXLG^o+bsr2@eMVP#Whe+@M+f#hi6~4098nw*QNHmlfN;OPk6Xosou(L3y`6 zl!O>y$25K$X}-&?DMKoVkyyxqjBd}JN%vC7{EFcN{TSH*u!O13p+B22Sq|!%9^Rc` z=;Dc(vxXmT-dpGP;ZFIa!6uK;?aZJ{A^Au5?K^sOmu2Kk=F0r*YJtu{m3a!Tr6SW@ z6Gl~S$qZ~s7rrG%u9J$A0yqmJ_q=^PbYaaN$qfuaFTyhLR>XoTpG_V2*Cn29D4!H< z)^~h+74Wn7C^<9I`0S|$=(klFXNZeV%}31EJVZh}o!c?I zKStjCR>MuA!!9aPlwf4by_p?w?`Ay;0`V#C{u(mjgB#@tAL;PyaUR1EyLj&x|JkAW zFsg0dhB}X8#9FrT#bO7{kqI^XKjv)QllM?3?|=;K{s)KxW{Kln9zP`z+LMGcy-PB* z0w9q^R&skcmVapv&feVaLrTKK$ozDaO97zGUIQVFFspp1B?}CYeDs+(x8& zE3mCkQ)#vp@yiAE{VxleTT%h;SKb=(%@DGIomzOcT!3I`nC1Zp9(Z>a3g;E~W}46Q zUFc&Qa4p9O!a(;u`*7WdaP^xH?!St19HNP-%UQlNeCykM8x^eWIRT)OYE{P{P6o;T z`gq2;2414mGcPwfniw>_xCg~aF+T$GI$hMfn^mJ~Du2wY!V@9~y_mARDzbo+ za&lCc#&TP?CXywuCyi!t@?e+s>siM*UCz9_`sg=i$&+fT`ldB_ESF+s{Y3@a?m52D zGDOxoecW85fDG?gNY!(n=Yi@9@PYmMBtgwgd;oOYNoEeaBA@Tc358F~> zik9bmo<^gMe13F$)621>@uJ;^FlRP@h~RnLIy~D}lIelda-jF5PGzR2t>EAcjR>oq zLPDb`RgI)ZTEfbF*`IGnngs>b8#uuB_q{Nv&fU-UYd_>!`kMr!4U1A&&uaDsRwH&p zvIlP;bU>MTF8TH)a4!dT>9s+XY<>X;R@J>n|7nkq?4L#;BpMOw-43CuOuJ^wx|gG~ z4I0R3{wbUw4udxWCcS;moMZL(H)Av|T`Z^Mr1h%hrXSNb*GL(SY7`0)U!0zWK)KE8 zx~3CCEB26R%55P}Y7+h2d~(HwX*SfLAlX;%*Y=3=BRpRRL|dZl+k;dig5;EZ7n3PN z&D5xhmQEAzTZT(5!@W#kNq5PF+6L%a$4QMN_-U!ml;m|xW+aogU@mRObSAwb0(DIk zqh&LSvc30Bl%<kc({1FyGz3Rq0X2IGO6ihtZ1!xZHGC<_Y1Lc6K_zL)k|{z@196 zo3b~$PhJj4keSE4s~3LxmWpeuEg?ls&hw}H`gQ9x(T+V{t4?Y%+aJvIowG^&^?7J{ zm5?c3PUpa!w#R&vz$P9?h6HtR^KGVVx=0Ed!Vn8eKF}ZIa5&kU%-ihC z_A5SSROjuCti2PRweUu(=xw@%6kx|S}ImH?q(rh7_Q!ShQ%Glk4 zk6r*v78S|;9?Hy58&7=udQbnQw)qnyLKzcr(Me*vwD+R(;!&z=;>&S{k0;r7)+EbocoqCP%agj>g<7{z(#V zadx?g!3=@b37f#WJnoy-)lBobJzR4uOHvP4h(q8O%ka}9^}bAdN1IM*vN1A5=DL^W z+z-!5Z;lr=&r0}NnQ33eL%Gx!RTc{Y2i6yAX-SB$l29rj41`qWr;TBrxnMh8YUZZ+=?@9IZ ze(I6jLe$nG-8OtpY$oX{AiMXiz~O|IlJVQAa`~uhLLWd;W13V(BWz=P=@rxt%Dn0G z+PT8577FKI!LkH>^lWMLcXLk*sCWl*rbqt4>u^RfDBJxT#vEdnL5K@%lSRwewDwfr zSXdaeu<4WTQCT_Kbz;kAIVNi{gPjov1+m*AT7pL*3GS@^pWm&J`4a5F*Kxl-}2+Ns8aMO8Q0a zsBD9gC5LqUoG0zz5~`~6Osk(liN^R(#Y?G^YKU$#b(}eOQ$^EF04TtzCkBaI*Lri+ zTCKwc1Dismm%{hMg*$efo%9)>9`ZFFae_h)-vX$(7w(`NrMRDCrTr97N*c@K)HE=9 zA<+yub_s7AiWn4Wxf%xlJTumzQU`&keyw@NrFriGn^+FXajuqgZ}XQ>-bwa}6Yg1# zdki8t^7zd`d{E+rs%+1xvs*FSKp#VDiGNk)!M70tV9O0V>>RA-!I zmSn+p9K?KvKO0HKRm8}1z;c1kzQ^wRyqhCDM%lKyhl+}#RKPhKp_xH#w^L?-kbDxP z9r0^2LhOHwPtbir+v|BS7qYw4K3Q#d9w;Z?KkNddq0V6l_S32%7IC!b-s zqxhVz^C$bJEne7h{BAa-D35*lz#5s1?U9+9Dudj+)25zk5DYR=d~128*Q>%}fOyz- zR~cV*@|NxcQC@M6dEZySc`QE9Cdc%uKOar(OJ(G-{g8U`5f{rg)=V_g`X{@g(!avt zv5{rVRy@L#x~p&I7OKTuB9;_l>s+j!1=a|=Y#PbA=A~L1x7IRcv+e*aWRdkA@Nas3 zQKn-S+68wvH@O5UnUh`aV=9gQru_v^8rN{Mzf|qpuFn#JB+P(zXT@h9Aw!(w8rLu% zo;&-L1q(R5;C4a|aqj|HO_Hp8RoRBG>;UM(z5DX6t}rWyixc|t(ZKH}h%DFotQ7p~ z(MJq^j!^W+DK6J_M=P^X#89PDQL0-%raR6=h*P|%*V&A zCojysNnh~!HGUeX;T#RY60S_ho#2di|HuC4>@F9qJsc& zPr;HuJ1PeE$N`A5-9Z7^VRTc_T@C51Tdfy3d#1l;Mx+34V6GH)v;HB`H&_^b+%@PN zEz`LP94)tW3bw_5NAOqBjZsiNn(mz{E~Qp&8K&vaakV4q$vF?+w6)4%b#gxdp@h=o z4|Hjb!ny>F>`aVFF;6ge-j)7yG(l8YA*>!NvKJv8oG8_ig5>G==LM0+tcTT-=fBp* zX;0c*gbR9HT5vT4-~XQ8W{`}|6%`nn;!2r#5b&-Gn|s|uyxE^eQiY>+g>$#hPR3GB zF73S%jeo9_gmh^d&hJnBGXM7`UE1D<-L=Ltp{-)ZL)v4m77+4K>k zwW~;o7Pr1NWy9ZB-^qqK-Ev~xer>&J6=`W5w5%Mq9WQ<~yEduY@!92E&YljIok30K zjosTc9fvYQK`=@Eq*XMb<+;K5eRnWZk2$;FM7LGA+3Aq*Ygj5hXD4$DZE5{2*nRI3 zUeDbEq?!=WA+a-6!29t4z`X&LlB0wk4BVkLiMa(tbKabM?Iff|b}rT_ zn@h*TrmE;SmVeIBDaoq-5bgcn&;jWo7fy=wv9!Pv{|3WrfL60iF{NJD z(oK}IgXyqD*lYg{Mb6n=k)D#s|LjJwpkUfl3zxB>jM4?1R(Vd<7jueEQ>;3vPd1I! zH%^}ZoMX>LIKX;|iu=<~)PS2K`KXmdm$h4`lzX zi6IBRHpgm=ETuuA=mb>#svSGD-ktY1(QVjM8vqrE&E1AmCf!2jSMQK#=lPLiwhdv z6ln(2#KBnEW683hw;irKqz@u#jI+nD5Na^FX8V`(Bo?n@ne?TO|H}LVUe609$yu zPUzK7JMf^9a)gXFvl-s3DCl>ej8_dT5zcwD0Y!mGWRRujmwYmg z6nc+p%3@c!+!nUVk6)Gh&Pjz1LVI^P3^oP22>q`mq<G+U7uokGSTu@tI z6MOVNyA`y6?dvHT(cnT4019vSe7B(88*?ZduM0R&_T<&cDO>Ya!?sJna|y+m48&jNIshN zV5I(XKK`2_4rgf5lSUx#Vh7_(m9q==)W@!CA>u6G2Nh! zRM(BtMLR+D$822X)*k@PnKSgqnQ?P!$3}jC2#M;5P&-}h?+-0Bs7WnqTu9u5y11Gh zz`966RF$HMdepDosL}cxUkwt9$gEI&ReV~#7{#&hy9xqS54UuN>lGEivMVr$w;d>Q zl$gmQ)LdZ=HzwmA$;`h1p3|hEKbhDdnLC%fL!I<2gzzqc!&(?WKGyJY{D#%QJ9mNG zvv}JT(+#NT#x@NN@(!@FqC4ns1m-Kw(KEoMPsMNPge7=GF=Go&Vvg!QdvOvcS?ljD zDL^?@LOn)u@ikNkz3XZrp^hEPFm3ysX1BV(0-3+jkdv=-$7k1c{D;@Crcd`Rw&uOx zA0s*I=v*0CgEYi?FF1t$uxu?eGBPa8XwhqHBhW$;t35hFraf1N-ysDf41V3F%s&Cc zK3HT6X6{IDk(KiD^YYU7vR9e2;l{c;3y-CwjvKpm*k@D}=sB+w7^Op!6x0^e$EJ;F0fNb*fd(DRa9#{kuT#8_}=lO z0fDS^I8HD#GwWm6bB(mbBc81}(4hCrQYaP!-##tX^vtB1 z`Z#(D>m+xIcfH8FyG_?>B8QwRd~jjTO)>=hs$W);CVlCDqap1twev)yu;!N+0qd@sMq_+US z?`k}?HLEzd=}2z~jLd}S2u$ZYGC_m)HWGj74S#n468e6+JfSMLUcz;TOsk zV}Z{)pQr?{NOMM(KX4Ae8OMcH%X!+#lk|6V%^lMx(S{k{b=c*0!X<0q7~D#UEhSpb zu_R7(%Tv#FjdqVwSj8BBCLQN~U{V}60Z?gAvAdY%hzDigySS;WY%F{b{whr@B_3Yr z_sB4kfGSH<&blXwbm48eMdDcxYiQ3W)}W;)1`F-9GxwFRh9x9fn!_+SL*|ptYh>x* zVf1efa6SXsH%GY8h(H>=V^~u1=Jjhi?%Mrj?3}&~`E5r+GU~+C#UHf3?f7mo#iOq2 zx);KUo}BUf@1q^ADXhj_Cu_Ho3%|Y$U*HR0XfXD@`|iPl?UzWDRU&P}7V#sZ9Np^_FX3 za2%>+)4G%Q>?NX|I+%~{25Xz1$jy1rI=auk!?Lh=2419Bf}Lke2lGZC;S`nSxD@NbR!D|J$yfp|*$F|)-a5C^>m zma(&?WGH?*47osU^Kt7?r-y5a)~AHbC}tV$aau#k`E^GVj*2!rEzx*;wlaSg&R`0` z_Ze+%(?dVl`Aj`soiOm@4=d)>(pI@TwYL+N#EI%8OLm&t4l3q2->3L11>Q0b3_oc0 z^`Clf3%WnTR)QJh82s6jv6_buslAuf`~+bi*1@k98m@`}J+^OJof?fhA4+cRUMRHB7MGi{^UBQ7d_ns$Js+;y$)dsV2u4U3N~I5 zRnJ0Q+F%e>TY|+|k380PDqJ^t{#|OZVO$U^aUmX|oeH0Pw(dgD^=|lkPOaFw>GW?t zxIY3I`5oLfLIy2Idf>bNMx$JCQ89F2U)zMwxfye@c20`1uUMaWz1>r7Vvh{LXRj>vi;-ym=g!M>34r1ZseC^i6oSX>VnySu-Ab$oVHvw6;@LJ;f3X`dcP zviW#WhaLIt4S)~|vYn3Hgbmly2(N`(o@@C4pM^&6AGtvLpg^R(iiRF$_i*!hDEON%dR}P4h$8vv~$25l8#Of@5;CuZmb{$>0&wFoS-h5 z(#A=ZmU$8va9~ONs`y|@*)>e@d@azZ(#r<(!q=QTsM@N0|ZhF#dd~wu^t)AO&#im^; z`|z-~ooF5vEL=i>BiHQ=3JDCW+E`05darL>7$*w5y&-bTMG5pwP1O^{D^tnRw;QEb z0aM$CLbmxOM4cMar7>@;bvpt_%%{|(n-r{U*O_ANy@HdE%zAD>jNx&GEK(G=(BM=< zw!J}`*JGlEr8^FBC{1W|@quTv58i!j*6DMAb4S_z3TksRMJ&f2MEL65c zR_MPa(a!z}&B?59>_25V!#Q;Ox8$Xs!Dp5Jv>_ zz~t}0L1S5C8q2xpB}j3E8viJ5R|`Rt-q`cpZvg4Y>i=Eapz^bOAUfKlH!9$Df z+nu-Gw+hX`hFhxr|^1IjgM zk{#8kd-*N(MLtqM7dmwVZd&ag1sABecozO~W@bmKFrPRZjxIV9U5Ug;Pncu}3o;gh z_xzvC$y?5T6(u!~b* z%9>bdAriX zm+zOcd^XsNuLkh=(1{aXVSULrwavGfQQ7l%?RB-uyb?G}7^!?_v5RU}Zc*|@!U4_O zT?-5%34V0Oxg!2RU=2-;)x8ZVil;X|J%SgPgElQ7hj69}DdF+yrQ;b_;^?b~;qG|Q z2amK?FS*Kw9-R8FN9iZ|0Vi*1IfC=S3%R)~)L@gfxG**Jr;xYUzy}$LGw(Hp1+fUk zEbm67Bc=Ca>}2bq>pFr%AW-skZqt7^0Ed~s6J{FJqww!~F7R(b?sfWOFf^&#r%!4k z3mAD&&9h7xzpY=FR^ntBYnW-&(r+8pq?3v#wd+x+FW^IoJMBqQ4#Bl629%{%2z440 zAGX@ndo0FD_~Kci>j-_mwHqC`37f0s2F7KK{9t#Lj$f7)4Xk-df~(y@C>(@EB~@K; zvhS2dk_K+^`$H#h&`uDFY$f4l;y^4IQRr?w7a^>!1Kl`Qvmm>LE;kGx^sq_8nQH%qFE?>zJ2Kvq!T=EFANPGoswzfXNV1-c0gj6fG+#( zFh0En^ePZ*KbqT!DYF=jlra%-eAZpZs`X2}uajZ@)KD5O3oE;)1xFl65F?xoWI2(*Z5oFmp>P$6ye1EaRxY?GLF)ezXGZju-6D+m8`~q z`=Ek@NQoLaR_gm}$2&AmiR|$uTHNE*m@Xqdr165^OC#BRxRJ{rZH|%9Xn48_0|@9u zx`JBkxYzS}57$fNH!eJSu-v{MIhC;z<%nx>7fThqA17~NiQxhIk$GJ*(7tB%*15en z2x;B6Wio&B{(Tjx&@O5}j~TrsqIft%yStRpfWJq}H*kJ>!34G! z1VW>pfk@i@c@x$XJ>~Fw|EzVNsZx4NArWXU(ihha$ixu8Jjl09Q)+2lK5ob01w&ca zFv!G@HHVLTAy4lH0|cG%qV)aanO>OuF&@+A(O?-s6gSy~;qy8Iv6=RT*Be3nRP!;- zJ7s61x$T&n)-v5{w$|yqY=CDgNn!~?AyC#nFc>Yy!@L*=p~VKlZReZC)qaOG)I^!P zJ?|zNxY+3$7gv(>muYB?%q|a^l9}bDAibU;75vk-V*ZDT4O369^!J4{pNJ~skxuz{;syt(@fFwYtc+yiGL&@H5puT zFg1z;Q6dRMiLy0%nje${rZ%>m0|5|#;!*vpk#HQNdm9JR-wkdG>J%UxJ)EDoWB8vM z^VS97->;g8Bw8W1SVp4sJ*G_Y@bC~fAzg|{=!fSJ0q5Tk!nn`&qVMH_9E)rr#bR$o zFcY)%cwkWV?UB0xgnY)ZSSM^`->ZRx;PZVl zo=Kf#$hI7WAAd}z?4Pn2crs0V(aehH-mHF>>^L-Few5_5$5dmXfnw~<`Fa+Ccv1>b z)fQ@W5&(v6f7CQDC7lb8^si71neu(TT5u<=7juP4I^f{Q-r*%i69G{C+2fWw4KWmU zbMAt&I)u!o|HJrb)2US*@fG_1|)Sfv*TI!WzL?I6ztDNH0vnpY_&@bw4)tNDyh#s>|flCrf}EIm*D?vXn_QwQM3 z-vFUbg)4Gbbn-2irUId(og~agk))GaF$Er(%RyWB;vC%ENj1@5Lm&ie3zuMaIzRpJ z60hhYPA0f3)4@fAYzB42nMty?_Eg%wiwkVwcWpc6B3if1JZKzD7u)Crf&f(&zAj`H z68_oJbUOzJV&49P*m}e&5Hlp&3z;Db#x1*IR!$kH9i~l}J5hoXM!;Rn{6cW;?gf>V zBhv=;y2A7qE97zs?|(TVPI`!`F)SRuT{`IXS#}pp1O0LQtu9SgG&3`T|78wI&@%Ss z)%#XXaZB0DRix+}w0v#u4n^7utA~UBv!$t(^T75T3VYM-IWOrrJ&Tb@dg#^V_|!|1 ze9zpB-Pe$E(21S{X65hDd{;`{wKip+H!NFZ>(;Je>BUql8PC`O)*lCpmvqN(Q75Ip z#%ONi2;zn-zd(iaw+vYI z5vD$Gg(2*2-xo@C2Ecf5+a8};XDL3qAVvf^_Qe{XU;a?%(uTCX@BIk=)kzxx=$XjQ zyKQHayV5xa+#*?#x%IItm_5tq5W!(u+C2dX1LvcO;{ziBeid|d-ta1tPY;Qp@z_(E z9CLr@Rm_(CxKAjYIN~v)oiLRmoqLo7An4p~Zqx^^+-*ZBqcR1T5mo z?hfEw7cJ(7k7`FxFEH%wNq~x{u{kUJE_cM)HF3&Ay zf(E)#G#Z>;8qa_+s{KBTfPaiWIqnMDja2*i>1#^g*s^^lfLuMi+#57Di1eA&>(;G{ zA{asPMNWmMO7J^ijua{X^+j%l2jC~dWfv^aQ-OGy2W-3UyBeK-0mgn^N5L{Ni)H~s z|Cs&`bY^Xy)mMZ94obVi!;WO((z$?h{EaqIbTi#UBuUBw!1Jcc2rY5!}hqYo5GeE<+ z`j(OI0YIfXCtV^Xv>;BdCLNprvt7*w#23EVf69#GP$e0-SiKrd4B0YBbFeR!!;dgs z&2_`Q*P*m>#8XO#Z`V7zUPtD0jG>j3#R-O(sCGY3Oq72y>H!VGuGHjSXEfmMuOfrS zfiPOsd3N`Pymt#b1DY~pvG7M@y`buk3!x<-D6q?^*jy%_paFy+3ohy{0rvuhW+#a) zHM8{ryuEo+J8oNw+jLmX%)Ps35VF?zxwmL@k5?}}0o=6A9{DgTTMULFJN&1nZ|6ulP)-WqR~`6ztcxe(spXHCN5>gU=2iU8X%(`cY1vz%^dhFg^Z!w7D%7AY9yBGPQzFS&*s?bhZX_P`lxZ43Jd}b*7 zkA{_(<~{+II1W6@NaH5f_je8*exS;x6Xi4u=|3L2d!|k1OfGlpdqhE<27KjvaI<&y zog-B02XLYvksl4w{6~a>?2)OLo7b+Z??M_GT8;o0P?tFfB1lgR($4;wm}|EN3`0!L zxR<#B#jW$&pFh=$8Qru+u676zfE!_c_p5zDpZwhsU6>ZBoNP0snC1u-JJ^j%V3F4+Z7$ijw~~~AMYq*l^P2upj5vR;S2l4J1WDEl z2`+nX=(I;o{AEVM(YpY!*2kEqRFM$2pxL0Wj4wF9plSGL*-gG0EeiZ7Q3Es2^=_#A z!aWi&yKu^7#x?%;hT11DQSkX5(WR$K)kIB}UKZLKa67__8QzrrU2$3d(Fe%Yt&5>20akx$vVHLm3F&ML#?#nGQM>AupgKr z!W$bV=Zc-9|Do2!XpT2D}@3U}697 zNkwVfI6<#Tg%>n5O4reyn_x{FLHRmlEFvaP9&s_+d%hT9ji^|A@R*_|>(#6Wfuzvj zV6J4hL*}$hujr( zw}S(++~@UaCoZ%)V~9s=&%qt;!-9MLg>Qa`@+6!icX>v~OHVa4O)lt(!D8Z(mMC2rpjM+ZzmO6wBi^1Of0mDTu`a5R2>k=@j0@h3uI&{&zv4bqkblwO5ef zN|XPaN(pV%Ebaj;>6Tf_<{ELCK4zm2=2nVb&X#>h%DwVpOSPTETN+C8|G_@_t4n%JUfbQwX#ku^nrucxzr<~galgxYQ!9kadNpr)CEphY%~c1)uSLBEJjn zw>>9cXWwV9Ky-h-g0-yCo2ZnqLuYXfw$na1moH?$&)_F{q+qVX0ZL-@0hBxF)p*1U z3_f$tD;yQtHK$XL5@hilWaA{pBd*y%oL|ug<8PSG`)DjX%-NY(EWl0J!&Gd8sT zKka>aJe2zz_Utoe#u&1UeJqs{4Ux6XAhau;Qc*(^SsE=!$_yHmz0FZcaiUH{Nh;M0 zDvCl*M9Wx`lcf=rRJ`{y)vt3p@89oV@A;!od`|N`^Ih)cy081XNfa#BxZIFs%UP0_ zjNW#3PUAzfVnfh5gS(<>M|JL3;u2Dc9(zvys}j+f zmFku$={w1`y<3T6eSgEra01w|oFLPQp;f+QJMQK*KaEWpHs4NnQD{R&elVc8=#%j`F?YdRg?}>stS#88mK=J5z zJ~U|FX-zkNug*%#6h|@0PwJNX67UHPA%P-7LVFysD!y3rbQj<D3dLLY?_4$<_2h85-_?J;DP_QOdA zq&I)w{W8+ z=l!!qpTXIG3{vRu;0Ca8iR%2?(^l~2C0n)h0cJIgFql}vx$qmaH+~vV;_c?_$M8D7 z_{d{F2Cr-;&vIIc5JUt%_Gy`|%c*&+nlEiuv|nuB8OoR&BIdM`wQL7(yNV1RL%f{z z+bfg_8fWVl2`Tp|!&~NW<_z4!sUx>T?Ox4tx<|i-dafOGXeg<#h0@mHjkZ-R%w0Tj zg)tA*3q(dPc4}%iot-x@p`#?mg|&x)xfFJ%Rf<^Q^}!x*R=vc>M$_vTD`pV$D4N2s zJ@g_@T=_G3Yoo-csbWi0^XBlB#v$M+L{;y~u1BhAB|W5P9QzZh8&_H#>)b|J3C)WW zT4t`!&cC2W;d9;6xmunr1Z^iS`DdzO7 zFh{l=+tivOAv+OxPV_a0V#n^pR`>4IdDAVB--}a zMskhWPUpuXll;!K*JKIA05-drGr*!PRaI;rWUPgoS^XM1fNThxPGDEX?mC#o#cES{HYt13=IHO>i&~a3QcQo&Q;*(mj^KxF>N^j zZEBv35b&>FO{1aBlQEGbrUF@M_u+cow)$!7SgC~^4uJoB-z`&2e_ zxdsm(cwUT)OFf;6I3}{20{RQoP7Z@>Z<(JASuijoPcZ#OH$CZ(rusw5b1w7r40zUs z1BHfL*G5uMnyDvALePFu%=NQ3x!uBuQ%DyHT+sUWG&b6j2Lr>TX7tC)F)IzRjYdob>-x-^GQ3o{%(ruewn z#I0d9bB1dI8)>+_$0DIq>lS$}-Th8K1F$N#v85#hzxR~b?E@I~c_g8aa-%H{LX@^! zyPb+g{(F#>$Zv=Z-&uGRf~SA|#~ z2EK)c?%{B_!zM{P&)Q&z0vnpV4bmg|@jBZn*jAriW$R~=f@2ge?|s^`v>P}$>!2xo z=b%ERsZL=SXOZl%xLqGXt;y=VhG7(L9wpPaZzm=dutyYc`pFM5yfh;kyMo5Nz~0;3 z{ATalhw@xW?)^sUx$9lqOJk|dvsH1SV&TA@yU9(bxOx{-lHl~Oszdp0)Atwc(J`zE zi9Dk)6ghqsUk7&}1MF8Y@QbIV+Yv(LB)s`}VILst1MU?RTx8HISDx1G#+GR7D8$(V zv8?fv(eGEG1LaOpb+2!-cn-%t>b^yem-(6Y*q$;BvGA+w8x>yT2in>T0 z(v?5%!R4P`8W)FBFEJ4?=8^o%?pU$(*!|uH0k3nYm#0zufjNqvTDAeI^}7+m-_5e%yu{9XkG|fZ<~(pnriJA;ppqM zon)p|Im>4U-J$_Nu#*LG3>OuZy)*RRaS>}V&+Ro`7)yc)8w-u(0-%L;07R|M`KG<9M?rN z5?J`P3}wR!jBoDMgs-Ks=|$&iI}~|?On)CP_s&r||IY?KXPkJvC8$KQgLL?u}A|vkXJb>z>gSF2r9iw42ci+itr9amD zQxZsNe%-8?t9z4LBtEy#`~KmJxyzC>Z>muq?Nt z2Z`t4+r9V}oEoslXYkAH@ctDL8Ko7ijHENM<=#xlNm;<>*2RcM{xG>6qZT7YHK&S$ zYq-z*`9X9_f3BqDr6neL^8=y+X&HTvWZtp(@}Mg6C7R9l~(5aC2;NNEh-KJ+{<&$ ze^uh^g5|ew?=z_qv3Ywe7e@4Oiwm4>NG^zjb&)_3(_IcN`P6)=#LT3Nh%X)5E{fW4 zSNZ~0FNe8$kCe>_e5lAswlDm{#?Y*B+~W79L-^q>w%MUtr=8Z*TgY0uzy^L{Ea6{5 z818uFR(eqyH_J)fARc3kw$U&+vd1k74&ZW;{98y=)86+!WW zK5(stwoqse+3A(uqGgrU5t}B?xt7{9RYJ1;Vju3ZVujuHx06$Eew{Tm4c zuY1bfWKTlyL41No+sb+Ssn^Px(l(w8L8zS17wwp!xT`FDyuKp#SPQ_fkQJhru39%W zDqAT)<7t5?*K#eB=44l)xzn$kcb$amTX+Uy7s3Tn(^QK826l4H!2>TePGWRIKFc2u zr@l)io`0)>rsZR=Qg6rOpGeow01YDL{P%(8S;Pp?_0Up|wi^iF;xX@-d{H^GQpGI+ z*sq;iY6WTU$#kd#hzy`oZsYnV&P$I!SmK+z?gGS5T{SP)9SqhhEkMxNT zGOfO3>N%ejn^UPLCN?GyVbmK2s%Inj0x9t9>h9?)R_r;l|_UoL6OLpKv*a}gSjABcw`uX=mL+p69K4iuWINFCsSuU?pKBnMoi*g zR{D;p$ivBl$F0ijHfk^u5g_$*>qVD~8y|nNP%kUNZ%=rYW&GDjr(_!ivtIC+7r6)O z*o6;Xp}jGQHu&hRSw*deo5@*Y-72mLDA)CS^7KHMDJcG(GrQTBWB)y!S)3YwPYs&{ zCtdRVBR0%#e|M}W^!h+B4reb_`x%4>vNd>d_Ni$)7&_?d3m}P{kT;_xyfNuYPm2p_djH@{@UYQRn( zeY)ZH6EQ;>mn{rBGQCFsycav+scQUH#OlS})Vg~U=dOPIah+!49eChgIVR8h9At?+ z+ZqO=vh^ZXbBhv{y0ZiS?zdH==gLM4n&viwa#;_Z9ZFGx_)qW*@D&Z$2s##vexzjv z&DS<0nPTIr*qsR;w zumBsJ)BX-56(W(ZLl0;I%U8vRRjlQrdhHE%1C6anWcvqcpmtwP_$A;7@v>tIqJ-K~ z>}IASIT#(et4yO&nJz&dF$D$ou;Yi87* ztSgglPx0oE!3G3v2HhD&j7XyiT+Yh0%um=H=0sd48zN%r?LFJ|!nzaAD}$L* z7xi8%<98BcGy6yns6er6{(!E4(jBqzj1P#onS*#sONuEvf1$fVjVV3X^?!>~M9&?` zm9Rq3u^ltffsskckNtSwU6`aH2q{|{#&A#od6Lv!t~7aOdG36B*~f)E;5{o~KaPPk zxBx*Xb1#bE{uc1foU&SOu%)v2+`nuXRPQY0&}AG>*`1KXx>MuAho1AaFP*7|HR+qa zuT&!K$Xj}_WqBQlrc}yIRF5uNzAQM)dj{uVUw$_rhIh#|R@KO3PPLuo3#tyk#fZJM z50V}sC+9e*cd#GRncEOWn^uy`1-b(#`L;ag6V9wLnj5zVDk^5`2~{ZuY-3g{<;)d~ zoDMoM{LrkLr?Z*&gdnT6bjd7QpOQ#1j9kB5*g#&@8WyeTKiPQpb zSV@DkdJi7@W%#P}*TssG7N`;ktFsZV7+`&S@V9$y$ywMijWT!)lw`yp7rDd$|%fHwv%S1uRShl-%7RU>oADhdM7ocUx~&+(FJlM~BUw-L!lAC^M_+ z$s!XXj!oz6y5B5BK?U^fw)Shsa)9%U|6r^1LC?vqLi8m-3^q z&J5h0lTiNk2~RSaAz0ENi{X;zrh+gRD;@=<*{4JW=8kM-U|@~XdJF#|J%#ta4`s#| z^2AkMv#jeMhUTx>cN#FXKUJ)w@FW#|N(bS$+)_{10*0GbZmgX`>#F%jr}9ApRQOUf z2uIw9E6WkS!g9wM;c7LNwSZp}r0j?}r0X@EqnsX=O~oE5&WStGEt6$^DNR86^;_|d zOqY_VJ}aSHA{Y17G4Q@-5M2D*AL5oN(>w;Rtyat@_DiZEi^0wR?)N^B%yl5 z5hY@?zjqUJZMGxmzw3U+X~aA@@u7Q0&~Y{4)r*ke!C>x7m3|Iiv~QMX(+WGpigx%z zY{es`TS4pQp4QsMQlVWBU0p9Ch1Iw@meC3z%%H*a6M;>tI8Xz)`$ht;Y>`+YTUds8 zuvNj6iaT7)8g0X+JUh+5RS&W7_Fs;oV#)odVjy8S>nSJQZ!DGO-3R}DIaek|4D%-p>HH$lcYK12~08?TD zu1pnFD{5Fnw!BwU+>w*)OLo@sIK(b$@y3uW=p&U%a9G!3+`3HZnL^aJBp4LeK;_#L zpS|zvq9SmYdQQC9NQM>e+^*-OE5%6vgwbX8 z^NRieDJ>m6l>H)?*JAAak}UyYhwBr!=CRwS1W+E9K>NybRV{ewyy%20CZs@ost($1 zU6w+u2T-jWy5r7l^90~4Sw$LXCknNB7aEieA*^WvLmQ(Nf&%eGq-)>Xpe1E#nG~mC zDqqgls@Ax$nALe6+-|eOvC~)JO0$EwC#TQ6Zw^<)EQ50o0AwI~URNBP`MPsAvK-F3 zxLWqOH*@TJ){htsm}6HPqUQBklVxQEUQ(Q`BdZ)w5>qW)fdg<#lb} z6#bmkxa2}!QN_a{VFh{;YHbZWf0_cdeQ@HBf}KmQq2~@Q;&4f+L(H@Hp-|sa5k(6i@kpwkYiYNGybE*IKxN)KZYLcI z(5hbU8Olrm&f-$KC*0^tA|)P@)zbkx~gPG&#yUpJ+&ie!HkCj`CD6 zR8Bg<6nYI?UH7ENgzARieHCbbE9PtwOQ&XOw(Q=~} z4ah4aN}2Dx>>JZN=V>qB5DmOSPNxuB1bgwD(|N*ynFa>+VJ9((aCUfR)t`N?1*w#G zYE*P+?5>u7Vs~tz{1$s42^sY72SUn^WCDVgA(SNc1U~0M|M<-}+6y zCu?eUpod8iK%-t*`^sr<+3$+p1!PGLJ2;`|)T5oD4ER(DnjfZ`UQOuNdolVPAIn&; zqH=R8#8L=2G$b1(&IIX9BKTQu-&{`e(_Z1&W}pPtTWg;#Vz()m%nU1FW2;m0{|Kz@ zT=GR(Dy<~p7^=vrd2&m(gifFj<=0B$wReGlM?LGNLdv=jj@Rq=?3Efhpz(y1*fQ2S zI{G@REElQPJvio|J74+B2l|*Qk$0+{d$NZ5ye$QqSo}HEE2s$-gq?iRd zpt@f$Tc9R>4sZPD*}Dy5GwNGY?6L}n=&_Lwt6c=bg8ZdAo=%!=ExoY2=JXQ6Y~G#* z52?AY#55i0V~&jUbPak5HV#^u=uA7@tsOjj73H2yez+&4tBK(>8dJl+iAZfuQ&QSy zW4#Y#J*e~cm3-uPPZbs23h<#EM$a63C=w+Wg%W1oRjH*))T6m6sg{LP(;YPnl^E@y zWV1wfK1el!B9g4J^3!Hq3O;im*`f&Wc#rmyn}7ybtsBCKwR{%L_X$JFbcnft&<o7Hrq+PhU%7WMao#`wlq zY{|i?_z~?VjY6EIT9Y2%21im~zn47MXD8{KULi=<=;QbXo>!+D?hE5du?c-VD%n^2 zN|l2VN$b`4{vNN|Tqe)4$Jmp;+nqNi?}3g}PUOeMrh$qim&;cm-@>QXpQcq{u-#UX zM~VGFpi^bkYDx5}9oo8d%qI@0wotj6M>6i>J7q#ejH7uao+Edpx)iO$i%|)KGgu;L zm&@Kv_GLLOTwX2@oKX>A_%cIASV~H!k(>2r7Di@M8+SYKtivGo^%qfJuV!j5N7vY& z10|T+gd?a0{F^sikr=GslnYD#X@6O!(wtz9tWrIveVOdwfWCiv#9?ZVyu7ETkwM6E z<8_qd=x8p3m$*!8eP!3Ru=tr%YS?xHXr#Lr%zj9xGm3u?ihM}QgZ5O%_QNQ9ygRJRo zs@tS2^v4A}gFfI_7^R!zslg!!(^oFq^c;`z>iw)3T)PIp|6rbn2sB4zp`!$fh{Tht&6Jx(e$& zC6e`!g;XM0NZL_wFbc8iMNdJf5rR_BZ^xyICr^IIefBr8OnZA9ZaOW(J*<=|O|}XR z4ZY=4>dC9UiuoIw)>)f1gj#3|*p zZ)b4}iTCx(xlg?*bst4b}Q{gbtFG3jH&t=K-Nd6hR-EADLS%Vjff;_KZ_& zR_&=dIlpk%C1#`0wYqEjvD44VI0aWUapx<-Ul6vKasvY&`m9}YsCfcQK%HwHN`Z80 z-8E24hGgdx^r$N5uYmCpt0Bzw(YT5aJ2?$I+EViD(!5pc1toiCSR~ydG1{!Ury2*Q zLi{FQj(&#MpE^O$gj-v19KAVnRY`{TNy{7@yd>Le~u{YcVS+!dk#L= zw%c~4BMx|u`Lp-xMBQ+5fBOf!BZL21pBvtMP(f*#z=vC5?FEZ{WRb%pX7${R1Kp-D;^f5t^OvIj*Og{o*1ay1Pxe1 z!Qr4p!GVNl(7p1QjO5YvK{J_wk$wm(dQNb=cdP)t`_mWdFt$dP47zAcTtBcJP(t`hw_x_A3_$!db+Xa)qik(p@d8DZZ6{RHujB1(>s z>AtmawyGAUh^@70Ma43}nZ0eCT>j?tD6xe;F=Q&776b|VzH7;#eu(Va;X@WiiGPaa z!!)1TB?{UT`h3Nl=Va%t2Ltr~(1`+2{)z80^eK(IQ1YI)Otj8l z5AA1BW{m6L5}#D!x1`A*1*>@Mf-6p=9>dLa)aII|Kf<*CC%T$ z1u4eUCF3b$YCl8!m*w zB~yRPtO1lER%|UT5^*l$==+vvvl@kyz_*2}S+Mgr$!@+vL>b8*4%AVC|T2|_rOX78tpaUUY4D(q^*7`En3o1ac^#)D~ zgsVZHN<1!05^#+IiqE~fM75uT5inHiIo^ET6j1R+qDa&_61BD%G;90-P3{Cq=Y$Gx zgEFw8jE8YMHtZ8Y-H|KDWUX|0wt!gzy0T&@1+DWt{~g5IpoxT2P?~?|#2?@QKiu2E zIewg*#U$A7a6dWA8t_|jDUMfj5_(E^mmK|wK)eU3Ou zR2UOQ@k5k#qO_-u92ImPDKrB95P$aCJnvYsr4gxYh1T(+ie-a>!aJ9fBBirc^F55| z(C)f9*k{?BB*R*N2_#Z~G3OfbEm6=G0l+5zMgC_Q`tM?VC^L`&WlnV4Vd?B3IxD`3h40x4RO1^`UmJ}xkVscM3`gPi07ns zliBR)O(5oy@0 zUnVGg;Tj8m(=K_@#kn+Lg{_8}E-uNoDv?U&M9(h@=ht5o|AlHypIzAXMRLANeo6W< z0Xp(P-Cqj2Z8p~YfQq};tjOD8yh{JguO>dE==Lt;GO^3koG4pz4U2Pyebp)w`dTkB zEqItKtW?EfA15e)YGv*&$>XiFc#Y6#`~et~9-2d6TMxg)V=Qt+92V$81_jdccmI&BXRhCmD%hW7p7A-6udHD z`UaAx2EalQt^>O1d*%5^`}Zu2>PMosKxnd}8d{L7)ER!t%EIAdMQ#gi3~F*9)r&J5 zRHdxECOgX&Kp;#4oBg=qaly-tw+SEE7Av-OR!7k#eGn>GKK=2Tf6;TtbD7xGO-o&R zbD7VJHGM>Tv#HPFbBa{l#%DA_(QIFe$!APQOs$$2eT0gX5^`iW`#8WYvQL50GTg-o zGqJPvB2vg^tpc`A=R!(<#ytAkOUn=WFd7fx&oz>@v;e9TBN?nT`_{+m&zH=f2^5~; ztM;#$hB-*tu#&v!!j>VKhwAulG(@>1^d%rVUgpVL*SyoJ-cg|uJ8>gkcETv0^ z^|u$k{{3;EGTVMJLYlm&@EN+a6_yr= zFa#Ja2q5%t&EF3TSn)-q^m#^OM0@so;(DIa?I36ORjYtj+4fe$XL9_nzV|_ETT0Hn z#+kvgMP4xAou^fJrjL2sIbXquJ<>tra&#>~7#3!R$)$h|o&cqd9^WlnbY9?5sWT$a zL*#>+Z>19=RI@3(ec-@+ZHP09e7;|QZoqz&2?6gKk&^od+6xq{3N@!mt_J`_z5gIy z4j3`ON#deXYk&dsLI?3Y!?FNH_6H0A!ga)6kaQ%J>V(Dy0cOb&M^LG=Gqh}h5V%{{ zzR`!v)={gmG)kN<5(p9|-`Ef|PRvTYtdiI=(dfK1#xo(8zsSfQuhFkc&n;~`hB}Z~ zBL7y8?Ky@$6tBLuz!wQ*U1X`-x5R%gj{kb`pV5=WBNB8^bYN|1X;5*`=)er zOw8!4zNYa_-yNIFYFWk3DS8ux4($U%YavJzf+2v`1JK>nZZs-lSFUFpsxvA|@cQRk zS1^wYuGm1~Q<(u^g$J2DPz_CjxsQll;DLVQ=l#|BW1peT1^OO=^joQ<@~XG@Zg3*? za;`2F0k03&?wO>^LJMB}GBmb3c?gbdxc=;l%HngM0^7D-Q7`KMv12gmDusYAeYhgE zo|33{j-vFCT4SP$OD!$ss|ENj{oO%)#=>?j&@tju9L|}u+;Xq;RTl$wDP+-p&x6+} zk3Xfmjg~noJJKQUTe^_UU5E7|MQ7vx!yAi0cF< zZ(d2L7CN=e%$()i{7Rqe1*R)h^EBA~z0100*`FJHi`)#$2R~L!h`J?1+D?sffCBKP zpbnbu-)jNeDl%K4L?sJD!FY|Nu|G9=a0^c?3y7vOtNYeA=HVxQ{(CbSCO*=jDJhfi zfnoH_a{D0x$A#mARNq~QaP84Kh98L$xGB5k&7@D{4NCOPB~z0xFbH6q9jVkxDz)eN zBeXrfcOEh}WRoCBq9brk?KGdwV3=;*Ix$7rZpi7TEBDiV$=0{>;QPu^IbQ#S zw(d5)KN*Kj1PyO{P>^N>d|_S0IB7;w$*lO@?(@=&>-Eliio~R3MK5{Ig-(T*gQ|G; z6ac|%c=3=H3eYd2KJIHfg~3w&^5al{mw#iKe}k{S;TB6s>EM=-4v^UgDjZeXELm^jODYo1 z^5a%^Ny5al-Bw?UQ@DeAF?X0SjijX(uRu0unO`a?p>|tigd?tUNmnJqYa{_$`K9;^ zcTUA&4-l(g-xlYcr&^!gJe*u>&X3r~6*|-q_LY=Ac8Agr)MxXp7&|iP-Db-L>NDV| zesyS+VGh8R{2uJXdekJK#2k*KR(^V_!H}EnB0^uj;=T*aUhNtY>+~H^S8NOCVLNiA ztC{-;9h4x70KH+9(}@~yrpSn{S9+p~8h)5?wXn}bG2aK8uz)yZq4 z18uc!KrTCs2H^fGCJ;2K`>tewL{&3~pIvw_Y}34h8OhVekb2O9@t;>9#ECmh}Uet5J5>pIUJW}@;J=aH< zFW_kh#J)E=1i8hk6U+(hi&Fyd16*UC!L^0{JIRd2wVhcQ!uk9zx+I8tFyo$=fGB>Z zDW_TNO8NXV>bt&Q#D8h2Z#46ML&Lm*G!>kS(Gxk}+Tv5a~vwvP6TV>#Lv6(Euun}1T_yxO1H}He|&pML8rDVxcXTQzhHqm(2Vlk z&cK9|mFoqD8*x^SkaIfmTQA07FHRctGTD$Zp{Grka9FxN=e8oJQRC-n%-v0*NCLLRWgvg0)#-E~+zVt>=@1a)tY(7WM44Qlza5FHKFCIQV6SrX zv~i?sdFpeV4(LBubM*_?Mj}GLZ_uAC*VEABKcO4`ZKQ5r@H=W)s+-5hTXRFFT`C$5_#2+KTo&QEKqw*X`EBVKix3#{?DfV)=P!uUD-_k z&0Lf|Bnj!OnG;j_(o&;D`Pux>D_tL&O3>}!=tkFMb+hkKoD)AQ&oBMBLG6Y+yKY&O zY{flvoDY#_{SidRDm1mk+n@&kzF<7&t>Q$y$?23)h)@9LH>ID4k(v0J^QDx`(UF4Quyy) zJ=*dpUY$|KVC1LMcB5b5^(i^aU1?a!xCe~e+MpEoO5QqtQvYuZ;j=;8U_GdXVt0RX z6`^!ZCV%b@{LL*v6hTY9s-+dJJ6{@i6YYgp(DL+koyQrp0;lKlmAvXTBW7j z8zSGNW+6*jt&QmxRXEsTOE2`?a#|c9g#&FuSs_;IrVWnpPN*wSf7C^whML0 zo2enGzS`79YfI@g7@iey0qXfCZ;}1z=l|Rf7)ve+Px`y{VpV)@5Z{#x46;h#He zFbBs!bR+_TiZgJVr-x@0%!rBWUjSh~JFNbx&uXbeRSY%pC1z6R&Cn8RM3G%x}3 z9LbwK3-kA0pf9NKPf(W*Y3!~ON!1OqnlwNzFyU$nOvP})V&fDusXjOOg8oGf?jCAA ze7$;Z?i0u*!Rj2{`Onv4Gm&cU->00jroy^7Rv}pLwUO=)D>DMYqs3b!S}N{h=%H*V z*#{4&qakgGK*u=!6}ww?^q}IUf$e2wKeBSL{JWBeq4xr57kv2Zdv zFigvJN#4oX@|DK1FtTL_GHOM;raER8{Ub1byqUfrN0P4t8Klm(yOjskKapG@$=L%l z3-YE`{V&9jxmVN#J=f1YeVrP8PI5AQMD#2vcnE9YeACcmZoZ>A4O3%@i&PJ%GkXA$ zhGXL>8*=*N>;T>Y`{%(CZ%j_Ee0i%hj<~LbM&5TX{H@lM$Xg2irLm&LuWOPjB2v|& z1p_3T)ktTwz(Zse8jW3!#wJAWPjA-dsU=#c_Vcf~)#p|eV`Yvc|6i-{4q*bhktHAf2JNvOS#fSFH9jE>K<2T4}*{nv4l^gl)kXkcdLQ$0LR^R_X8T!RO_ZYW52L%U7amM0pFQ~V@4XO)N5~1bgoic0U2HWU1 zjS)hU=4ZE0f<9!xxgUU~?5wqr(e)hhRf*WXjFMPdtc<&kHa8;qeW~Wrtx`E0xn%fQ zw`LeG6iU4ayLwuM6gVm0XqxUSSxgbskuOntGIAwI^T76h?Pj4a1{ihK!cDPRV{o@h zt9{CY{Ay1rV{cGW!=aZ^VomZ#vTpOxA6Yk2z9TSkMmCFtf-V41r`d1fGIW5J+gZwP znZa!z#E1cI-7MWZM|7Bla8l%cph2?lu~Qyhn=7$ez1loA6K#9=zzvs)FJC`TzI2d8 ziS@EE!mY;iK($2u5Ri2;*ohDdx5FI6pIb=SL_%qTbjcziysp;ETi#!#K7G(+6E&ww zq$dIaoITsk3izdnG{KECZVk@*hu+O@+v?!=zScto|3iVk{@bTde|J5Y@XyVH7!ges z2yXJnDA?+~Fc`>bq!_7skzV4fM7eb$c0HXbHKi*O2$>pN(}7&7i^EpZhJ;ydrm%i16fMyNU=gs+$QY@YB6wz9nu1?X?f+OU_e~pCcz?ols^Iwk-)?;_#Yhb$5 zKJG|!5J5Owl5!>>GY#g9=8@&Ec^f!!XYtq(0wygMN_>Bwle4E}hakbNHZJMWnA!5tNrv-R5%9#fo=lj?c>l=7 zm$#D4Sg6@Mrvc}gH2^iB$hvj-F?mZz!8641Qf~e0%Yu4CPcCx;_MJO2?+F|rC-B^C@0j%t3kJ981$$JF zRjXS=$sIGX;I~n7Wz*;Bo;>8Vul&~?5k`|x9kI&+kvB7+_zZ~oGkWy0E_{th*bxIw zDPTy=SzjFG4fiEe5(QJTfotLwHR4a#F}6-zCt%!QOFGH+BzM)kuf)T6Q?zn^v$E76 zAB#bnCw^I*X?ju!i6U=}uBXv%Dd>)g_l+kv(Q)K%U`S!EzTp?m1J@)Okh;ILG&c%Q zfZQDvKQ(aQxLYRge_j*#TbmGpOkwuxSSdSy26r4PwmO(f+I&*)MX!kkhQ=hs@xF30%5-H}>t;->2l*>dHS3i#@x2dEcwSlnVv zxd)pj zs;^MukJaSMd0qMhkZ4>GUI!OvOrhTA{l$u@ z+g=jFTWe7lDDg!Y+zTnieo2Dn3^6)Fkm$2G23(_G+2nj5=$-e+D#U&wUB*;UdUE(8 z_Phr8@;3g|3jMrX(R3#JbwF;vC-VzawC!aJGNx@kj63DB z<}LrgVS&`nHvOgG)=(Rz(vsBPpSiHCdL~fR#7Oh}B5&{AG#xr8Y0o}BOp3EIh~db4 zaSysGSB^`KJ+-Xw<5v+JRs&c@o!1?^4}ljE6ep{x>t99}P(Hv@^89j?fE$A?e=b*H z$|TI3Hc^~`eroBMrtb3$wuw+`UhYQjsbTp z7pdS`0mU-#n}JABi0R70sNPE?)+)2q|3rb?hE9n*gzY0`!|(^he?0qTGx(OcKA1hhoGyBi&B6sfwHejYUb%)Pv(Fr-G@LhNxs1@15nvapIkvsUk8y!0&2Fb zn9leKxy%0MZibV@#U?|KkKG~{b$j~{gkE{@S{FA~<_`IC&O_sQ6vWM*o(P#F`|B@j zQK*VZU;H`|(Qg^!v;C>Z0X`=Lpq>Cc3W+MV)6`snkcM3r|0{ z#5}Xa1XEW8#b%uWnv$|~Tl-s#UL?I9sqFawa+Gp?NT^50CaUgzRDB=)_eII?zCGTB zqSZ|i(*62bYKJ*MVp#Sip>v7klFf#a6`pT3I6xc#Sx#?@DkQ#gEI(DJOL0cJ;I8Bx z00yW0v|c#(5Oe-|<=MA^H+{pUR-IDQhQ}!El+2o&pwu$h5HYzdD^Oy~n0U9J^M;l`)sUP~N{u@gz> z86j%56#Rb8Jcwx?2!DB3AAoIyk|}a>WT=YUgTpL)=glk3u%MG#IPLSQw6xZ2tmI+V z*SI+wasWlEq(veY!T#5~<>-Xkerpu#;xe@D<(T+}K2io?Yz2z9RxI9dA$;Gf7=Z@2 zT0o}^Q;7%=1!?4eEQ7&3q{bQ3*}dmr)UDAS$(!(k%hfN#w2Vo?Bu4^%n;EMTL!5pN zD_}3pPr+j>Q3+U|g1Zt8T1}KT%iVaK!+Q5_BSM$H{7Zh0Bv|rRay&T-=s&^hdX@(iy&EJ^* zcMGGZlYPO7pFTrUP(s=V-AO8uz>E33&EnBdXzjZTLmA={b(NiSn78UgJf$Z%``Jvk z3pdl=fLe;(WS!m?$Hn0hF{`D&J{=fUI!GxJ^Inve`IIn+pL~s*xY~TZps?UBYWA#8 z2i_b(N{2~%emOoc37Uew5Er{q{FT;KGTdXDMJ{6J@Ai@KB#;a4kz7^~Op5`12j=WaU&K~N{|GjI8VG2?r>=`Rc`&qz&UmPd^KKGqaz`>O6WJs3z zaHphaUgLryh~TV9m2|+2<)xxBPtKksjEmcC@#L2fK^8L=Dq(5?UJKFR=DZ;7VA6_E z^G`RN(g%CH?Zo0?*Ol*{A-A<|?|+s%KoOJ95l>#Y;QGfO=X~eq2?XMRRX;c_|FTtO zbgo9qyI-!N>_>eEfgAmyKH^y6=cybKI9J^i086uD7lDwHn zq5nPyhB9OmzK5?rFtTqyx_p}xHTvJBOZqls=pVI + +#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..5d087f082 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/particle_cloud/particle_cloud_display_visual_test.cpp @@ -0,0 +1,64 @@ +/* + * 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 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..fbbb47fe8 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.cpp @@ -0,0 +1,69 @@ +/* + * 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 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..a72fd895c --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/particle_cloud_display_page_object.hpp @@ -0,0 +1,48 @@ +/* + * 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 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..81ea48f81 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/particle_cloud_publisher.hpp @@ -0,0 +1,89 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__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_ From e084c759462e1e227cde8053b390488f59ac7883 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 7 May 2020 09:44:41 +0530 Subject: [PATCH 2/2] Add copyright --- .../displays/particle_cloud/flat_weighted_arrows_array.hpp | 1 + .../displays/particle_cloud/particle_cloud_display.hpp | 1 + .../displays/particle_cloud/flat_weighted_arrows_array.cpp | 1 + .../displays/particle_cloud/particle_cloud_display.cpp | 1 + .../displays/particle_cloud/particle_cloud_display_test.cpp | 1 + .../particle_cloud/particle_cloud_display_visual_test.cpp | 1 + .../page_objects/particle_cloud_display_page_object.cpp | 1 + .../page_objects/particle_cloud_display_page_object.hpp | 1 + .../rviz_default_plugins/publishers/particle_cloud_publisher.hpp | 1 + 9 files changed, 9 insertions(+) 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 index e636eab8e..6ba0ecee1 100644 --- 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 @@ -1,6 +1,7 @@ /* * 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 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 index 29476fc8f..f177472ad 100644 --- 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 @@ -1,6 +1,7 @@ /* * 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 diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.cpp index 11fcf1099..f8f3ce311 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/particle_cloud/flat_weighted_arrows_array.cpp @@ -1,6 +1,7 @@ /* * 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 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 index 3f830d493..8458852f3 100644 --- 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 @@ -1,6 +1,7 @@ /* * 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 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 index 68ba1f69b..f644bfa15 100644 --- 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 @@ -1,5 +1,6 @@ /* * 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 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 index 5d087f082..9857916b5 100644 --- 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 @@ -1,5 +1,6 @@ /* * 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 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 index fbbb47fe8..37731fa5b 100644 --- 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 @@ -1,5 +1,6 @@ /* * 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 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 index a72fd895c..9a71f453b 100644 --- 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 @@ -1,5 +1,6 @@ /* * 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 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 index 81ea48f81..479840dd2 100644 --- 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 @@ -1,5 +1,6 @@ /* * 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