From 049d3995bab8b94ab24ea6761bbec0e6e1dfcced Mon Sep 17 00:00:00 2001 From: X-lanni Date: Tue, 23 Sep 2025 10:11:30 +0800 Subject: [PATCH] feat(navigation2): add RViz charger tool --- navigation2/nav2_rviz_plugins/CMakeLists.txt | 2 + .../nav2_rviz_plugins/charger_tool.hpp | 69 +++++++++++++++ .../nav2_rviz_plugins/plugins_description.xml | 6 ++ .../nav2_rviz_plugins/src/charger_tool.cpp | 87 +++++++++++++++++++ 4 files changed, 164 insertions(+) create mode 100644 navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp create mode 100644 navigation2/nav2_rviz_plugins/src/charger_tool.cpp diff --git a/navigation2/nav2_rviz_plugins/CMakeLists.txt b/navigation2/nav2_rviz_plugins/CMakeLists.txt index 39bdbab..44ac329 100644 --- a/navigation2/nav2_rviz_plugins/CMakeLists.txt +++ b/navigation2/nav2_rviz_plugins/CMakeLists.txt @@ -36,6 +36,7 @@ set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/goal_pose_updater.hpp include/nav2_rviz_plugins/goal_common.hpp include/nav2_rviz_plugins/goal_tool.hpp + include/nav2_rviz_plugins/charger_tool.hpp include/nav2_rviz_plugins/nav2_panel.hpp include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp @@ -49,6 +50,7 @@ set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED src/goal_tool.cpp + src/charger_tool.cpp src/nav2_panel.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp diff --git a/navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp b/navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp new file mode 100644 index 0000000..673bf3e --- /dev/null +++ b/navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_ +#define NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_ + +#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/qos.hpp" +#include + +#include "rviz_default_plugins/tools/pose/pose_tool.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_common +{ + +class DisplayContext; + +namespace properties +{ +class StringProperty; +class QosProfileProperty; +} // namespace properties +} // namespace rviz_common + +namespace nav2_rviz_plugins +{ + +class RVIZ_DEFAULT_PLUGINS_PUBLIC ChargerTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + ChargerTool(); + ~ChargerTool() override; + + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Clock::SharedPtr clock_; + rviz_common::properties::StringProperty * topic_property_; + rviz_common::properties::QosProfileProperty * qos_profile_property_; + + rclcpp::QoS qos_profile_; +}; + +} // namespace nav2_rviz_plugins + +#endif // NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_ diff --git a/navigation2/nav2_rviz_plugins/plugins_description.xml b/navigation2/nav2_rviz_plugins/plugins_description.xml index 197a9a7..0eb7522 100644 --- a/navigation2/nav2_rviz_plugins/plugins_description.xml +++ b/navigation2/nav2_rviz_plugins/plugins_description.xml @@ -6,6 +6,12 @@ A tool used to specify the navigation goal pose. + + A tool used to specify the charging stations goal pose. + + diff --git a/navigation2/nav2_rviz_plugins/src/charger_tool.cpp b/navigation2/nav2_rviz_plugins/src/charger_tool.cpp new file mode 100644 index 0000000..e2e685d --- /dev/null +++ b/navigation2/nav2_rviz_plugins/src/charger_tool.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_rviz_plugins/charger_tool.hpp" + +#include +#include + +#include "rviz_common/display_context.hpp" +#include "rviz_common/load_resource.hpp" +#include "rviz_common/properties/string_property.hpp" +#include "rviz_common/properties/qos_profile_property.hpp" + +namespace nav2_rviz_plugins +{ + +ChargerTool::ChargerTool() +: rviz_default_plugins::tools::PoseTool(), qos_profile_(5) +{ + shortcut_key_ = 'c'; + topic_property_ = new rviz_common::properties::StringProperty( + "Topic", "charger_position_update", + "The topic on which to publish goals.", + getPropertyContainer(), SLOT(updateTopic()), this); + + qos_profile_property_ = new rviz_common::properties::QosProfileProperty( + topic_property_, qos_profile_); +} + +ChargerTool::~ChargerTool() +{ +} + +void ChargerTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("Charger Update"); + setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/SetGoal.png")); + updateTopic(); +} + +void ChargerTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = + context_->getRosNodeAbstraction().lock()->get_raw_node(); + // TODO(anhosi, wjwwood): replace with abstraction for publishers once available + publisher_ = raw_node-> + template create_publisher( + topic_property_->getStdString(), qos_profile_); + clock_ = raw_node->get_clock(); +} + +void +ChargerTool::onPoseSet(double x, double y, double theta) +{ + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = clock_->now(); + goal.header.frame_id = fixed_frame; + + goal.pose.position.x = x; + goal.pose.position.y = y; + goal.pose.position.z = 0.0; + + goal.pose.orientation = orientationAroundZAxis(theta); + + logPose("goal", goal.pose.position, goal.pose.orientation, theta, fixed_frame); + + publisher_->publish(goal); +} + +} // namespace nav2_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::ChargerTool, rviz_common::Tool)