From b1b9a8319efafcbb26f74d245570ce5a6f568938 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 1 Nov 2026 12:32:53 +0100 Subject: [PATCH] navmap_rviz_plugin: 3D Goal Pose tool for goals above the ground MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_rviz_plugin/CMakeLists.txt | 2 + navmap_rviz_plugin/icons/classes/Goal3D.png | Bin 0 -> 446 bytes .../navmap_rviz_plugin/goal_3d_tool.hpp | 96 +++++++ .../navmap_rviz_plugin_description.xml | 11 + .../src/navmap_rviz_plugin/goal_3d_tool.cpp | 239 ++++++++++++++++++ 5 files changed, 348 insertions(+) create mode 100644 navmap_rviz_plugin/icons/classes/Goal3D.png create mode 100644 navmap_rviz_plugin/include/navmap_rviz_plugin/goal_3d_tool.hpp create mode 100644 navmap_rviz_plugin/src/navmap_rviz_plugin/goal_3d_tool.cpp diff --git a/navmap_rviz_plugin/CMakeLists.txt b/navmap_rviz_plugin/CMakeLists.txt index 188769d..c57108f 100644 --- a/navmap_rviz_plugin/CMakeLists.txt +++ b/navmap_rviz_plugin/CMakeLists.txt @@ -25,12 +25,14 @@ find_package(Qt${qt_major} REQUIRED COMPONENTS Widgets) qt_wrap_cpp(NAVMAP_MOC_SRCS include/navmap_rviz_plugin/NavMapDisplay.hpp include/navmap_rviz_plugin/navmap_goal_tool.hpp + include/navmap_rviz_plugin/goal_3d_tool.hpp ) add_library(${PROJECT_NAME} SHARED src/navmap_rviz_plugin/NavMapDisplay.cpp src/navmap_rviz_plugin/navmap_goal_tool.cpp src/navmap_rviz_plugin/navmap_pose_tool.cpp + src/navmap_rviz_plugin/goal_3d_tool.cpp ${NAVMAP_MOC_SRCS} ) target_link_libraries(navmap_rviz_plugin PUBLIC diff --git a/navmap_rviz_plugin/icons/classes/Goal3D.png b/navmap_rviz_plugin/icons/classes/Goal3D.png new file mode 100644 index 0000000000000000000000000000000000000000..91de9c0ca99d26b125243e6dbd6c10ef153d97f1 GIT binary patch literal 446 zcmeAS@N?(olHy`uVBq!ia0vp^0wB!61|;P_|4#%`jKx9jP7LeL$-D$|*pj^6T^Rm@ z;DWu&Cj&(|3p^r=85p>QL70(Y)*K0-AbW|YuPgffmmbgZgIOpf) zrskC}I2WZRmZYXAlxLP?D7bt2281{Ai36>Y^mK6yu{eEolC3v$qCiW!s%wCb57R>) z_Ic9Lr}XdyJ(rl>`p{E&N=sG4iZX77nTj%>Riu?RoqBg&`rYn%su43q zvYb~;4CtIEuUvAv`Q+X?2Uw(?YDLcluW)%Jyl{7p`RA~A0>3XG@!rb6JoVA7*zNqU zR~9LTEQ)+%zN4`G?SXCK4nf?lUR!LN=Q2ILvGdp3KhH()rHdQRvpp`c?8k?njbS0~ z$9*;kZ)daq{>OC20R`TU3wsZj-P?HI{8N96my2Ctq}5R`vt<6|2Rj2gpKBF9%jx@c k*}Gzn=LMZ>AN9ZTem`E+yjR%38W@rcp00i_>zopr04Qg(VgLXD literal 0 HcmV?d00001 diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/goal_3d_tool.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/goal_3d_tool.hpp new file mode 100644 index 0000000..09867cc --- /dev/null +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/goal_3d_tool.hpp @@ -0,0 +1,96 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 NAVMAP_RVIZ_PLUGIN__GOAL_3D_TOOL_HPP_ +#define NAVMAP_RVIZ_PLUGIN__GOAL_3D_TOOL_HPP_ + +#include + +#include + +#include // NOLINT cpplint cannot handle include order here + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rviz_common/tool.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_rendering +{ +class Arrow; +class Line; +} // namespace rviz_rendering + +namespace rviz_common +{ +namespace properties +{ +class FloatProperty; +class StringProperty; +class QosProfileProperty; +} // namespace properties +} // namespace rviz_common + +namespace navmap_rviz_plugin +{ + +/// @brief A goal anywhere in space (aerial robots): on the line from the camera through the +/// cursor, at a distance the mouse wheel changes. Click to place it, drag to set its yaw. +class RVIZ_DEFAULT_PLUGINS_PUBLIC Goal3DTool : public rviz_common::Tool +{ + Q_OBJECT + +public: + Goal3DTool(); + ~Goal3DTool() override; + + void onInitialize() override; + void activate() override; + void deactivate() override; + int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; + +private Q_SLOTS: + void updateTopic(); + +private: + /// The point at the current distance on the ray through (x, y); false without a camera. + bool point_on_ray(int x, int y, Ogre::Vector3 & point) const; + /// Where the ray through (x, y) meets the horizontal plane at height z. + bool point_on_plane(int x, int y, double z, Ogre::Vector3 & point) const; + void show(const Ogre::Vector3 & position, double yaw); + void hide(); + void update_status(); + + enum class State {Position, Orientation}; + State state_ {State::Position}; + Ogre::Vector3 position_; + double yaw_ {0.0}; + + std::shared_ptr arrow_; + std::shared_ptr drop_line_; ///< Down to z = 0: where it is in depth. + std::shared_ptr foot_x_, foot_y_; ///< A cross where the line lands. + + rviz_common::properties::FloatProperty * distance_property_; + rviz_common::properties::FloatProperty * wheel_step_property_; + rviz_common::properties::StringProperty * topic_property_; + rviz_common::properties::QosProfileProperty * qos_profile_property_; + rclcpp::QoS qos_profile_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace navmap_rviz_plugin + +#endif // NAVMAP_RVIZ_PLUGIN__GOAL_3D_TOOL_HPP_ diff --git a/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml b/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml index c4984f6..864a4a1 100644 --- a/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml +++ b/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml @@ -16,4 +16,15 @@ + + + Publish a goal pose anywhere in space (aerial robots): on the line from the camera through + the cursor, at a distance set with the mouse wheel; drag to set its yaw. + + + diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/goal_3d_tool.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/goal_3d_tool.cpp new file mode 100644 index 0000000..96d545f --- /dev/null +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/goal_3d_tool.cpp @@ -0,0 +1,239 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 "navmap_rviz_plugin/goal_3d_tool.hpp" + +#include +#include +#include + +#include "OgreCamera.h" +#include "OgrePlane.h" +#include "OgreRay.h" +#include "OgreSceneNode.h" +#include "OgreViewport.h" +#include "rviz_common/display_context.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/qos_profile_property.hpp" +#include "rviz_common/properties/string_property.hpp" +#include "rviz_common/view_controller.hpp" +#include "rviz_common/view_manager.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/line.hpp" + +namespace navmap_rviz_plugin +{ + +namespace +{ + +bool camera_ray(rviz_common::DisplayContext * context, int x, int y, Ogre::Ray & ray) +{ + auto * view = context->getViewManager()->getCurrent(); + Ogre::Camera * camera = view ? view->getCamera() : nullptr; + if (!camera || !camera->getViewport()) { + return false; + } + Ogre::Viewport * viewport = camera->getViewport(); + ray = camera->getCameraToViewportRay( + static_cast(x) / static_cast(viewport->getActualWidth()), + static_cast(y) / static_cast(viewport->getActualHeight())); + return true; +} + +} // namespace + +Goal3DTool::Goal3DTool() +: qos_profile_(5) +{ + shortcut_key_ = 'h'; + distance_property_ = new rviz_common::properties::FloatProperty( + "Distance", 10.0, "Distance from the camera to the goal (m); the mouse wheel changes it.", + getPropertyContainer()); + distance_property_->setMin(0.5); + wheel_step_property_ = new rviz_common::properties::FloatProperty( + "Wheel step", 1.0, "Distance change per wheel notch (m).", getPropertyContainer()); + wheel_step_property_->setMin(0.01); + topic_property_ = new rviz_common::properties::StringProperty( + "Topic", "goal_pose", "The topic on which to publish goals.", getPropertyContainer(), + SLOT(updateTopic()), this); + qos_profile_property_ = new rviz_common::properties::QosProfileProperty( + topic_property_, qos_profile_); +} + +Goal3DTool::~Goal3DTool() = default; + +void Goal3DTool::onInitialize() +{ + arrow_ = std::make_shared( + scene_manager_, nullptr, 2.0f, 0.2f, 0.5f, 0.35f); + arrow_->setColor(0.0f, 0.8f, 1.0f, 1.0f); + arrow_->getSceneNode()->setVisible(false); + drop_line_ = std::make_shared(scene_manager_, nullptr); + drop_line_->setColor(0.0f, 0.8f, 1.0f, 0.6f); + drop_line_->setVisible(false); + foot_x_ = std::make_shared(scene_manager_, nullptr); + foot_y_ = std::make_shared(scene_manager_, nullptr); + for (const auto & line : {foot_x_, foot_y_}) { + line->setColor(0.0f, 0.8f, 1.0f, 1.0f); + line->setVisible(false); + } + qos_profile_property_->initialize( + [this](rclcpp::QoS profile) {this->qos_profile_ = profile;}); + setName("3D Goal Pose"); + updateTopic(); +} + +void Goal3DTool::updateTopic() +{ + rclcpp::Node::SharedPtr node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + publisher_ = node->create_publisher( + topic_property_->getStdString(), qos_profile_); + clock_ = node->get_clock(); +} + +void Goal3DTool::activate() +{ + state_ = State::Position; + update_status(); +} + +void Goal3DTool::deactivate() +{ + hide(); +} + +void Goal3DTool::hide() +{ + arrow_->getSceneNode()->setVisible(false); + for (const auto & line : {drop_line_, foot_x_, foot_y_}) { + line->setVisible(false); + } +} + +void Goal3DTool::update_status() +{ + setStatus( + QString("Distance %1 m (mouse wheel). Click to place the goal, drag to set its yaw.") + .arg(distance_property_->getFloat(), 0, 'f', 1)); +} + +bool Goal3DTool::point_on_ray(int x, int y, Ogre::Vector3 & point) const +{ + Ogre::Ray ray; + if (!camera_ray(context_, x, y, ray)) { + return false; + } + point = ray.getPoint(distance_property_->getFloat()); + return true; +} + +bool Goal3DTool::point_on_plane(int x, int y, double z, Ogre::Vector3 & point) const +{ + Ogre::Ray ray; + if (!camera_ray(context_, x, y, ray)) { + return false; + } + const auto hit = ray.intersects(Ogre::Plane(Ogre::Vector3::UNIT_Z, static_cast(z))); + if (!hit.first) { + return false; + } + point = ray.getPoint(hit.second); + return true; +} + +void Goal3DTool::show(const Ogre::Vector3 & position, double yaw) +{ + arrow_->setPosition(position); + // The arrow points along -z by default + arrow_->setOrientation( + Ogre::Quaternion(Ogre::Radian(yaw), Ogre::Vector3::UNIT_Z) * + Ogre::Quaternion(Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Y)); + arrow_->getSceneNode()->setVisible(true); + const Ogre::Vector3 foot(position.x, position.y, 0.0f); + drop_line_->setPoints(position, foot); + const Ogre::Vector3 dx(0.5f, 0.0f, 0.0f), dy(0.0f, 0.5f, 0.0f); + foot_x_->setPoints(foot - dx, foot + dx); + foot_y_->setPoints(foot - dy, foot + dy); + for (const auto & line : {drop_line_, foot_x_, foot_y_}) { + line->setVisible(true); + } +} + +int Goal3DTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (event.type == QEvent::Wheel && state_ == State::Position) { + const float notches = static_cast(event.wheel_delta) / 120.0f; + distance_property_->setValue( + std::max(0.5f, distance_property_->getFloat() + notches * wheel_step_property_->getFloat())); + update_status(); + Ogre::Vector3 p; + if (point_on_ray(event.x, event.y, p)) { + position_ = p; + show(position_, yaw_); + } + return Render; + } + + if (state_ == State::Position) { + Ogre::Vector3 p; + if (!point_on_ray(event.x, event.y, p)) { + return 0; + } + position_ = p; + show(position_, yaw_); + if (event.leftDown()) { + state_ = State::Orientation; + } + return Render; + } + + // Orientation: towards the cursor, on the goal's horizontal plane + Ogre::Vector3 p; + if ((event.type == QEvent::MouseMove && event.left()) && + point_on_plane(event.x, event.y, position_.z, p) && + (p - position_).length() > 1e-3f) + { + yaw_ = std::atan2(p.y - position_.y, p.x - position_.x); + show(position_, yaw_); + return Render; + } + if (event.leftUp()) { + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = clock_->now(); + goal.header.frame_id = context_->getFixedFrame().toStdString(); + goal.pose.position.x = position_.x; + goal.pose.position.y = position_.y; + goal.pose.position.z = position_.z; + goal.pose.orientation.z = std::sin(yaw_ / 2.0); + goal.pose.orientation.w = std::cos(yaw_ / 2.0); + RVIZ_COMMON_LOG_INFO_STREAM( + "Setting 3D goal: Frame:" << goal.header.frame_id << ", Position(" << + goal.pose.position.x << ", " << goal.pose.position.y << ", " << goal.pose.position.z << + "), Yaw: " << yaw_); + publisher_->publish(goal); + state_ = State::Position; + hide(); + return Finished | Render; + } + return 0; +} + +} // namespace navmap_rviz_plugin + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(navmap_rviz_plugin::Goal3DTool, rviz_common::Tool)