diff --git a/navmap_rviz_plugin/CMakeLists.txt b/navmap_rviz_plugin/CMakeLists.txt index 5ceccfe..e33505b 100644 --- a/navmap_rviz_plugin/CMakeLists.txt +++ b/navmap_rviz_plugin/CMakeLists.txt @@ -4,34 +4,49 @@ project(navmap_rviz_plugin) # 1) Qt y AUTOMOC find_package(Qt5 REQUIRED COMPONENTS Core Widgets) set(CMAKE_AUTOMOC ON) -add_definitions(-DQT_NO_KEYWORDS) +set(CMAKE_AUTORCC ON) +set(CMAKE_AUTOUIC ON) + +# add_definitions(-DQT_NO_KEYWORDS) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) +find_package(rviz_default_plugins REQUIRED) find_package(pluginlib REQUIRED) find_package(rosidl_default_runtime REQUIRED) find_package(navmap_ros_interfaces REQUIRED) +find_package(navmap_ros REQUIRED) +find_package(navmap_core REQUIRED) set(CMAKE_CXX_STANDARD 23) set(CMAKE_CXX_STANDARD_REQUIRED ON) qt5_wrap_cpp(NAVMAP_MOC_SRCS include/navmap_rviz_plugin/NavMapDisplay.hpp + include/navmap_rviz_plugin/navmap_goal_tool.hpp + include/navmap_rviz_plugin/navmap_pose_tool.hpp ) add_library(${PROJECT_NAME} SHARED - src/NavMapDisplay.cpp + src/navmap_rviz_plugin/NavMapDisplay.cpp + src/navmap_rviz_plugin/navmap_goal_tool.cpp + src/navmap_rviz_plugin/navmap_pose_tool.cpp include/navmap_rviz_plugin/NavMapDisplay.hpp + include/navmap_rviz_plugin/navmap_goal_tool.hpp + include/navmap_rviz_plugin/navmap_pose_tool.hpp ${NAVMAP_MOC_SRCS} ) target_link_libraries(navmap_rviz_plugin PUBLIC ${navmap_ros_interfaces_TARGETS} + navmap_ros::navmap_ros + navmap_core::navmap_core pluginlib::pluginlib rclcpp::rclcpp rviz_common::rviz_common rviz_rendering::rviz_rendering + rviz_default_plugins::rviz_default_plugins Qt5::Core Qt5::Widgets ) @@ -50,6 +65,11 @@ install(TARGETS RUNTIME DESTINATION lib/${PROJECT_NAME} ) +install( + DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + DESTINATION "share/${PROJECT_NAME}" +) + install(FILES resource/navmap_rviz_plugin_description.xml DESTINATION share/${PROJECT_NAME} ) diff --git a/navmap_rviz_plugin/icons/classes/NavMapSetGoal.png b/navmap_rviz_plugin/icons/classes/NavMapSetGoal.png new file mode 100644 index 0000000..91de9c0 Binary files /dev/null and b/navmap_rviz_plugin/icons/classes/NavMapSetGoal.png differ diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp index d42830f..4ef3f86 100644 --- a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp @@ -40,6 +40,8 @@ #include #include +#include "navmap_core/NavMap.hpp" + #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define NAVMAP_RVIZ_PLUGIN_EXPORT __attribute__ ((dllexport)) @@ -74,6 +76,8 @@ class HardwareVertexBuffer; namespace navmap_rviz_plugin { +inline navmap::NavMap received_navmap; + class NAVMAP_RVIZ_PLUGIN_PUBLIC NavMapDisplay : public rviz_common::MessageFilterDisplay { diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp new file mode 100644 index 0000000..7c00499 --- /dev/null +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp @@ -0,0 +1,70 @@ +// Copyright 2025 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__NAVMAP_GOAL_TOOL_HPP_ +#define NAVMAP_RVIZ_PLUGIN__NAVMAP_GOAL_TOOL_HPP_ + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/qos.hpp" + +#include "navmap_rviz_plugin/navmap_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 navmap_rviz_plugin +{ +class RVIZ_DEFAULT_PLUGINS_PUBLIC NavMapGoalTool : public NavMapPoseTool +{ + Q_OBJECT + +public: + NavMapGoalTool(); + + ~NavMapGoalTool() override; + + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double z, 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 navmap_rviz_plugin + +#endif // NAVMAP_RVIZ_PLUGIN__NAVMAP_GOAL_TOOL_HPP_ diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp new file mode 100644 index 0000000..9b20898 --- /dev/null +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp @@ -0,0 +1,96 @@ +// Copyright 2025 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__NAVMAP_POSE_TOOL_HPP_ +#define NAVMAP_RVIZ_PLUGIN__NAVMAP_POSE_TOOL_HPP_ + +#include +#include +#include + +#include + +#include // NOLINT cpplint cannot handle include order here + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/quaternion.hpp" + +#include "rviz_common/tool.hpp" +#include "rviz_rendering/viewport_projection_finder.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +#include "navmap_rviz_plugin/NavMapDisplay.hpp" + + +namespace rviz_rendering +{ +class Arrow; +} // namespace rviz_rendering + +namespace navmap_rviz_plugin +{ + +class RVIZ_DEFAULT_PLUGINS_PUBLIC NavMapPoseTool : public rviz_common::Tool +{ +public: + NavMapPoseTool(); + + ~NavMapPoseTool() override; + + void onInitialize() override; + + void activate() override; + + void deactivate() override; + + int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; + +protected: + virtual void onPoseSet(double x, double y, double z, double theta) = 0; + + geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle); + + void logPose( + std::string designation, + geometry_msgs::msg::Point position, + geometry_msgs::msg::Quaternion orientation, + double angle, + std::string frame); + + std::shared_ptr arrow_; + + enum State + { + Position, + Orientation + }; + State state_; + double angle_; + + Ogre::Vector3 arrow_position_; + std::shared_ptr projection_finder_; + +private: + int processMouseLeftButtonPressed(std::pair xy_plane_intersection); + int processMouseMoved(std::pair xy_plane_intersection); + int processMouseLeftButtonReleased(); + void makeArrowVisibleAndSetOrientation(double angle); + double calculateAngle(Ogre::Vector3 start_point, Ogre::Vector3 end_point); +}; + +} // namespace navmap_rviz_plugin + +#endif // NAVMAP_RVIZ_PLUGIN__NAVMAP_POSE_TOOL_HPP_ diff --git a/navmap_rviz_plugin/package.xml b/navmap_rviz_plugin/package.xml index 3f17dc4..0f3ad3f 100644 --- a/navmap_rviz_plugin/package.xml +++ b/navmap_rviz_plugin/package.xml @@ -17,6 +17,8 @@ rviz_rendering rviz_default_plugins navmap_ros_interfaces + navmap_ros + navmap_core geometry_msgs std_msgs sensor_msgs diff --git a/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml b/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml index 7e0ef71..c4984f6 100644 --- a/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml +++ b/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml @@ -5,4 +5,15 @@ base_class_type="rviz_common::Display"> Visualize NavMap triangles and layers, with optional normals and alpha. + + + + Publish a goal pose for the robot. After one use, reverts to default tool. + + + diff --git a/navmap_rviz_plugin/src/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp similarity index 99% rename from navmap_rviz_plugin/src/NavMapDisplay.cpp rename to navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index a6d4d2e..a64d61f 100644 --- a/navmap_rviz_plugin/src/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp @@ -39,6 +39,10 @@ #include #include +#include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" + + #include #include #include @@ -223,6 +227,7 @@ void NavMapDisplay::processMessage(const NavMapMsg::ConstSharedPtr msg) root_node_->setOrientation(orientation); last_msg_ = std::make_shared(*msg); + received_navmap = navmap_ros::from_msg(*last_msg_); ++navmap_msg_count_; last_navmap_stamp_ = rviz_ros_node_.lock()->get_raw_node()->now(); diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp new file mode 100644 index 0000000..887a433 --- /dev/null +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp @@ -0,0 +1,88 @@ +// Copyright 2025 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/navmap_goal_tool.hpp" + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_common/properties/string_property.hpp" +#include "rviz_common/properties/qos_profile_property.hpp" + +namespace navmap_rviz_plugin +{ + +NavMapGoalTool::NavMapGoalTool() +: navmap_rviz_plugin::NavMapPoseTool(), qos_profile_(5) +{ + shortcut_key_ = 'g'; + + 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_); +} + +NavMapGoalTool::~NavMapGoalTool() = default; + +void NavMapGoalTool::onInitialize() +{ + NavMapPoseTool::onInitialize(); + qos_profile_property_->initialize( + [this](rclcpp::QoS profile) {this->qos_profile_ = profile;}); + setName("NavMap Goal Pose"); + updateTopic(); +} + +void NavMapGoalTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = + context_->getRosNodeAbstraction().lock()->get_raw_node(); + publisher_ = raw_node-> + template create_publisher( + topic_property_->getStdString(), qos_profile_); + clock_ = raw_node->get_clock(); +} + +void NavMapGoalTool::onPoseSet(double x, double y, double z, 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 = z; + + goal.pose.orientation = orientationAroundZAxis(theta); + + logPose("goal", goal.pose.position, goal.pose.orientation, theta, fixed_frame); + + publisher_->publish(goal); +} + +} // namespace navmap_rviz_plugin + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(navmap_rviz_plugin::NavMapGoalTool, rviz_common::Tool) diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp new file mode 100644 index 0000000..4897f89 --- /dev/null +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp @@ -0,0 +1,216 @@ +// Copyright 2025 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/navmap_pose_tool.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "rviz_rendering/geometry.hpp" +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/render_window.hpp" + +#include "rviz_common/logging.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_common/view_manager.hpp" +#include "rviz_common/view_controller.hpp" + +namespace navmap_rviz_plugin +{ + +NavMapPoseTool::NavMapPoseTool() +: rviz_common::Tool(), arrow_(nullptr), angle_(0) +{ + projection_finder_ = std::make_shared(); +} + +NavMapPoseTool::~NavMapPoseTool() = default; + +void NavMapPoseTool::onInitialize() +{ + arrow_ = std::make_shared( + scene_manager_, nullptr, 2.0f, 0.2f, 0.5f, 0.35f); + arrow_->setColor(0.0f, 1.0f, 0.0f, 1.0f); + arrow_->getSceneNode()->setVisible(false); +} + +void NavMapPoseTool::activate() +{ + setStatus("Click and drag mouse to set position/orientation."); + state_ = Position; +} + +void NavMapPoseTool::deactivate() +{ + arrow_->getSceneNode()->setVisible(false); +} + +static std::pair +rayHitOnNavMap( + rviz_common::ViewportMouseEvent & event, + rviz_common::DisplayContext * context) +{ + auto * view_controller = context->getViewManager()->getCurrent(); + if (!view_controller) { + return {false, Ogre::Vector3::ZERO}; + } + + // 2) Cámara Ogre directamente + Ogre::Camera * cam = view_controller->getCamera(); + if (!cam || !cam->getViewport()) { + return {false, Ogre::Vector3::ZERO}; + } + + Ogre::Viewport * vp = cam->getViewport(); + + const float nx = static_cast(event.x) / static_cast(vp->getActualWidth()); + const float ny = static_cast(event.y) / static_cast(vp->getActualHeight()); + const Ogre::Ray ray = cam->getCameraToViewportRay(nx, ny); + + Eigen::Vector3f o(ray.getOrigin().x, ray.getOrigin().y, ray.getOrigin().z); + Eigen::Vector3f d(ray.getDirection().x, ray.getDirection().y, ray.getDirection().z); + if (d.squaredNorm() == 0.0f) { + return {false, Ogre::Vector3::ZERO}; + } + d.normalize(); + + ::navmap::NavCelId cid = 0; + float t = 0.0f; + Eigen::Vector3f hit; + const bool ok = received_navmap.raycast(o, d, cid, t, hit); + if (!ok) { + return {false, Ogre::Vector3::ZERO}; + } + + return {true, Ogre::Vector3(hit.x(), hit.y(), hit.z())}; +} + +int NavMapPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + std::pair hit = {false, Ogre::Vector3::ZERO}; + + if (!received_navmap.surfaces.empty()) { + hit = rayHitOnNavMap(event, context_); + } + + if (!hit.first) { // Fallback: If there is not an intersection with navmap, intersect with z = 0 + hit = projection_finder_->getViewportPointProjectionOnXYPlane( + event.panel->getRenderWindow(), event.x, event.y); + } + + if (event.leftDown()) { + return processMouseLeftButtonPressed(hit); // espera pair + } else if (event.type == QEvent::MouseMove && event.left()) { + return processMouseMoved(hit); + } else if (event.leftUp()) { + return processMouseLeftButtonReleased(); + } + return 0; +} + +int NavMapPoseTool::processMouseLeftButtonPressed( + std::pair xy_plane_intersection) +{ + int flags = 0; + assert(state_ == Position); + if (xy_plane_intersection.first) { + arrow_position_ = xy_plane_intersection.second; + arrow_->setPosition(arrow_position_); + + state_ = Orientation; + flags |= Render; + } + return flags; +} + +int NavMapPoseTool::processMouseMoved(std::pair xy_plane_intersection) +{ + int flags = 0; + if (state_ == Orientation) { + // compute angle in x-y plane + if (xy_plane_intersection.first) { + angle_ = calculateAngle(xy_plane_intersection.second, arrow_position_); + makeArrowVisibleAndSetOrientation(angle_); + + flags |= Render; + } + } + + return flags; +} + +void NavMapPoseTool::makeArrowVisibleAndSetOrientation(double angle) +{ + arrow_->getSceneNode()->setVisible(true); + + // we need base_orient, since the arrow goes along the -z axis by default + // (for historical reasons) + Ogre::Quaternion orient_x = Ogre::Quaternion( + Ogre::Radian(-Ogre::Math::HALF_PI), + Ogre::Vector3::UNIT_Y); + + arrow_->setOrientation(Ogre::Quaternion(Ogre::Radian(angle), Ogre::Vector3::UNIT_Z) * orient_x); +} + +int NavMapPoseTool::processMouseLeftButtonReleased() +{ + int flags = 0; + if (state_ == Orientation) { + onPoseSet(arrow_position_.x, arrow_position_.y, arrow_position_.z, angle_); + flags |= (Finished | Render); + } + + return flags; +} + +double NavMapPoseTool::calculateAngle(Ogre::Vector3 start_point, Ogre::Vector3 end_point) +{ + return atan2(start_point.y - end_point.y, start_point.x - end_point.x); +} + +geometry_msgs::msg::Quaternion NavMapPoseTool::orientationAroundZAxis(double angle) +{ + auto orientation = geometry_msgs::msg::Quaternion(); + orientation.x = 0.0; + orientation.y = 0.0; + orientation.z = sin(angle) / (2 * cos(angle / 2)); + orientation.w = cos(angle / 2); + return orientation; +} + +void NavMapPoseTool::logPose( + std::string designation, geometry_msgs::msg::Point position, + geometry_msgs::msg::Quaternion orientation, double angle, std::string frame) +{ + RVIZ_COMMON_LOG_INFO_STREAM( + "Setting " << designation << " pose: Frame:" << frame << ", Position(" << position.x << ", " << + position.y << ", " << position.z << "), Orientation(" << orientation.x << ", " << + orientation.y << ", " << orientation.z << ", " << orientation.w << + ") = Angle: " << angle); +} + +} // namespace navmap_rviz_plugin