From 30a4b07bfc48b15f078e8132891417372c86dab5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 15 Feb 2024 11:29:15 +0100 Subject: [PATCH] Use Rviz as a component MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rviz_common/CMakeLists.txt | 3 + .../ros_integration/ros_component_manager.hpp | 59 +++++++++++++++++++ .../rviz_common/visualization_manager.hpp | 2 + rviz_common/package.xml | 1 + .../ros_integration/ros_node_abstraction.cpp | 2 +- .../src/rviz_common/visualization_manager.cpp | 11 +++- 6 files changed, 76 insertions(+), 2 deletions(-) create mode 100644 rviz_common/include/rviz_common/ros_integration/ros_component_manager.hpp diff --git a/rviz_common/CMakeLists.txt b/rviz_common/CMakeLists.txt index 516ba3980..230de1d9f 100644 --- a/rviz_common/CMakeLists.txt +++ b/rviz_common/CMakeLists.txt @@ -36,6 +36,7 @@ find_package(Qt5 REQUIRED COMPONENTS Widgets) find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(resource_retriever REQUIRED) find_package(rviz_rendering REQUIRED) find_package(sensor_msgs REQUIRED) @@ -239,6 +240,8 @@ target_link_libraries(rviz_common PUBLIC pluginlib::pluginlib Qt5::Widgets rclcpp::rclcpp + rclcpp_components::component + rclcpp_components::component_manager rviz_ogre_vendor::OgreMain rviz_ogre_vendor::OgreOverlay rviz_rendering::rviz_rendering diff --git a/rviz_common/include/rviz_common/ros_integration/ros_component_manager.hpp b/rviz_common/include/rviz_common/ros_integration/ros_component_manager.hpp new file mode 100644 index 000000000..7d7eeedcf --- /dev/null +++ b/rviz_common/include/rviz_common/ros_integration/ros_component_manager.hpp @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2024, Open Source Robotics Foundation. + * 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_COMMON__ROS_INTEGRATION__ROS_COMPONENT_MANAGER_HPP_ +#define RVIZ_COMMON__ROS_INTEGRATION__ROS_COMPONENT_MANAGER_HPP_ + +#include "rclcpp_components/component_manager.hpp" +#include "rclcpp_components/node_instance_wrapper.hpp" +#include "rviz_common/visibility_control.hpp" + +namespace rviz_common +{ +namespace ros_integration +{ +class RVIZ_COMMON_PUBLIC ComponentManager : public rclcpp_components::ComponentManager{ +public: + using rclcpp_components::ComponentManager::ComponentManager; + + void + add_node_to_executor( + rclcpp_components::NodeInstanceWrapper & node) + { + auto node_id = unique_id_++; + node_wrappers_[node_id] = node; + if (auto exec = this->executor_.lock()) { + exec->add_node(node.get_node_base_interface(), true); + } + } +}; +} // namespace ros_integration +} // namespace rviz_common + +#endif // RVIZ_COMMON__ROS_INTEGRATION__ROS_COMPONENT_MANAGER_HPP_ diff --git a/rviz_common/include/rviz_common/visualization_manager.hpp b/rviz_common/include/rviz_common/visualization_manager.hpp index 86630a95b..da70ab94d 100644 --- a/rviz_common/include/rviz_common/visualization_manager.hpp +++ b/rviz_common/include/rviz_common/visualization_manager.hpp @@ -44,6 +44,7 @@ #include "rviz_common/display_context.hpp" #include "rviz_common/frame_manager_iface.hpp" #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" +#include "rviz_common/ros_integration/ros_component_manager.hpp" #include "rviz_common/transformation/transformation_manager.hpp" class QTimer; @@ -399,6 +400,7 @@ private Q_SLOTS: rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node_; rviz_common::transformation::TransformationManager * transformation_manager_; + std::shared_ptr mgr_; }; } // namespace rviz_common diff --git a/rviz_common/package.xml b/rviz_common/package.xml index 8bd06bf01..fa6238c41 100644 --- a/rviz_common/package.xml +++ b/rviz_common/package.xml @@ -40,6 +40,7 @@ geometry_msgs pluginlib rclcpp + rclcpp_components resource_retriever rviz_ogre_vendor rviz_rendering diff --git a/rviz_common/src/rviz_common/ros_integration/ros_node_abstraction.cpp b/rviz_common/src/rviz_common/ros_integration/ros_node_abstraction.cpp index d5f4fbcd5..328eda5d7 100644 --- a/rviz_common/src/rviz_common/ros_integration/ros_node_abstraction.cpp +++ b/rviz_common/src/rviz_common/ros_integration/ros_node_abstraction.cpp @@ -44,7 +44,7 @@ namespace ros_integration { RosNodeAbstraction::RosNodeAbstraction(const std::string & node_name) -: raw_node_(rclcpp::Node::make_shared(node_name)) +: raw_node_(rclcpp::Node::make_shared(node_name, rclcpp::NodeOptions().use_intra_process_comms(true))) {} std::string diff --git a/rviz_common/src/rviz_common/visualization_manager.cpp b/rviz_common/src/rviz_common/visualization_manager.cpp index 99fd2e1e6..b32bb21a9 100644 --- a/rviz_common/src/rviz_common/visualization_manager.cpp +++ b/rviz_common/src/rviz_common/visualization_manager.cpp @@ -56,6 +56,7 @@ #include "rclcpp/clock.hpp" #include "rclcpp/time.hpp" +#include #include "rviz_rendering/material_manager.hpp" #include "rviz_rendering/render_window.hpp" @@ -225,7 +226,15 @@ VisualizationManager::VisualizationManager( connect(this, SIGNAL(timeJumped()), this, SLOT(resetTime())); - executor_->add_node(rviz_ros_node_.lock()->get_raw_node()); + if (!mgr_) { + mgr_ = std::make_shared(executor_); + executor_->add_node(mgr_); + + auto inst = rclcpp_components::NodeInstanceWrapper( + rviz_ros_node_.lock()->get_raw_node(), + std::bind(&rclcpp::Node::get_node_base_interface, rviz_ros_node_.lock()->get_raw_node())); + mgr_->add_node_to_executor(inst); + } display_factory_ = new DisplayFactory();