From b5f93c2144d6eaf4002f96dff0df7e300258765c Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Sat, 8 Nov 2025 10:42:55 +0100 Subject: [PATCH 01/15] Fix errors in periodic CI --- .github/workflows/humble.yaml | 1 + .github/workflows/jazzy.yaml | 1 + .github/workflows/kilted.yaml | 1 + .github/workflows/rolling.yaml | 1 + 4 files changed, 4 insertions(+) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 790de84..129427d 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -29,6 +29,7 @@ jobs: with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: humble + ref: humble colcon-defaults: | { "test": { diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 4c6cc39..31ba809 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -29,6 +29,7 @@ jobs: with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: jazzy + ref: jazzy colcon-defaults: | { "test": { diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 503f6e7..1774f6e 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -29,6 +29,7 @@ jobs: with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: kilted + ref: kilted colcon-defaults: | { "test": { diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index bfcbdaf..a579a72 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -29,6 +29,7 @@ jobs: with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: rolling + ref: rolling colcon-defaults: | { "test": { From 8692e63d046e3c0db0a76c71e31a25905b359019 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:00:27 +0100 Subject: [PATCH 02/15] Add occupancy grid constants MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/include/navmap_ros/conversions.hpp | 20 +++++++++++++++++++ navmap_ros/src/navmap_ros/conversions.cpp | 10 +++++----- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 068f56f..a7e9f49 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -57,6 +57,26 @@ namespace navmap_ros { +/** + * @name Costmap value semantics + * @brief Standardized occupancy/cost values used when projecting NavMap layers + * onto a 2D grid (compatible with `costmap_2d` conventions). + * + * These constants follow the same meaning as in `costmap_2d`: + * - `NO_INFORMATION` (255): Unknown or unobserved area. + * - `LETHAL_OBSTACLE` (254): Non-traversable obstacle. + * - `INSCRIBED_INFLATED_OBSTACLE` (253): Inside the robot’s inscribed radius. + * - `MAX_NON_OBSTACLE` (252): Highest cost still considered traversable. + * - `FREE_SPACE` (0): Known free space. + * @{ + */ +constexpr uint8_t NO_INFORMATION = 255; +constexpr uint8_t LETHAL_OBSTACLE = 254; +constexpr uint8_t INSCRIBED_INFLATED_OBSTACLE = 253; +constexpr uint8_t MAX_NON_OBSTACLE = 252; +constexpr uint8_t FREE_SPACE = 0; +/** @} */ // end of Costmap value semantics group + // --------- NavMap <-> ROS message --------- /** diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index d04e0b6..65a3eb1 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -53,15 +53,15 @@ using navmap_ros_interfaces::msg::NavMapSurface; static inline uint8_t occ_to_u8(int8_t v) { - if (v < 0) {return 255u;} - if (v >= 100) {return 254u;} - return static_cast(std::lround((v / 100.0) * 254.0)); + if (v < 0) {return NO_INFORMATION;} + if (v >= 100) {return LETHAL_OBSTACLE;} + return static_cast(std::lround((v / 100.0) * static_cast(LETHAL_OBSTACLE))); } static inline int8_t u8_to_occ(uint8_t u) { - if (u == 255u) {return -1;} - return static_cast(std::lround((u / 254.0) * 100.0)); + if (u == NO_INFORMATION) {return -1;} + return static_cast(std::lround((u / static_cast(LETHAL_OBSTACLE)) * 100.0)); } static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32_t W) From e9556e1ee894bf17c6ecea655a460c8e4e7cc5e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:00:50 +0100 Subject: [PATCH 03/15] FREE_SPACE as white 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/src/NavMapDisplay.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/navmap_rviz_plugin/src/NavMapDisplay.cpp b/navmap_rviz_plugin/src/NavMapDisplay.cpp index f377532..037e89e 100644 --- a/navmap_rviz_plugin/src/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/NavMapDisplay.cpp @@ -76,8 +76,8 @@ inline Ogre::ColourValue colorFromRainbow(float value, float max_value, float al inline Ogre::ColourValue colorFromU8(uint8_t v, float alpha) { - if (v == 0) {return Ogre::ColourValue(0.5f, 0.5f, 0.5f, alpha);} - if (v == 255) {return Ogre::ColourValue(0.0f, 0.39f, 0.0f, alpha);} + if (v == 0) {return Ogre::ColourValue(1.0f, 1.0f, 1.0f, alpha);} + if (v == 255) { return Ogre::ColourValue(0.25f, 0.25f, 0.25f, alpha); } if (v == 254) {return Ogre::ColourValue(0.0f, 0.0f, 0.0f, alpha);} float occ = static_cast(v) / 253.0f; float c = 1.0f - occ; From cc8425466eefe53603568c7e6c5facd657958009 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 11 Nov 2025 20:01:45 +0100 Subject: [PATCH 04/15] Linting 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/src/NavMapDisplay.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/navmap_rviz_plugin/src/NavMapDisplay.cpp b/navmap_rviz_plugin/src/NavMapDisplay.cpp index 037e89e..a6d4d2e 100644 --- a/navmap_rviz_plugin/src/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/NavMapDisplay.cpp @@ -77,7 +77,7 @@ inline Ogre::ColourValue colorFromRainbow(float value, float max_value, float al inline Ogre::ColourValue colorFromU8(uint8_t v, float alpha) { if (v == 0) {return Ogre::ColourValue(1.0f, 1.0f, 1.0f, alpha);} - if (v == 255) { return Ogre::ColourValue(0.25f, 0.25f, 0.25f, alpha); } + if (v == 255) {return Ogre::ColourValue(0.25f, 0.25f, 0.25f, alpha);} if (v == 254) {return Ogre::ColourValue(0.0f, 0.0f, 0.0f, alpha);} float occ = static_cast(v) / 253.0f; float c = 1.0f - occ; From c285b1b085b4946c7f99f13d9822b0627128eb80 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 13 Nov 2025 07:45:18 +0100 Subject: [PATCH 05/15] NavMap Goal Pose 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 | 24 +- .../icons/classes/NavMapSetGoal.png | Bin 0 -> 446 bytes .../navmap_rviz_plugin/NavMapDisplay.hpp | 4 + .../navmap_rviz_plugin/navmap_goal_tool.hpp | 70 ++++++ .../navmap_rviz_plugin/navmap_pose_tool.hpp | 96 ++++++++ navmap_rviz_plugin/package.xml | 2 + .../navmap_rviz_plugin_description.xml | 11 + .../NavMapDisplay.cpp | 5 + .../navmap_rviz_plugin/navmap_goal_tool.cpp | 88 +++++++ .../navmap_rviz_plugin/navmap_pose_tool.cpp | 216 ++++++++++++++++++ 10 files changed, 514 insertions(+), 2 deletions(-) create mode 100644 navmap_rviz_plugin/icons/classes/NavMapSetGoal.png create mode 100644 navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp create mode 100644 navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp rename navmap_rviz_plugin/src/{ => navmap_rviz_plugin}/NavMapDisplay.cpp (99%) create mode 100644 navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp create mode 100644 navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp 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 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/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 From a9532a61cfe24c6be965e3df954c93e47dc3c706 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 15 Nov 2025 08:24:02 +0100 Subject: [PATCH 06/15] Separate periodic CI from PR/push MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/humble.yaml | 3 --- .github/workflows/humble_cron.yaml | 42 +++++++++++++++++++++++++++++ .github/workflows/jazzy.yaml | 3 --- .github/workflows/jazzy_cron.yaml | 42 +++++++++++++++++++++++++++++ .github/workflows/kilted.yaml | 3 --- .github/workflows/kilted_cron.yaml | 42 +++++++++++++++++++++++++++++ .github/workflows/rolling.yaml | 3 --- .github/workflows/rolling_cron.yaml | 42 +++++++++++++++++++++++++++++ 8 files changed, 168 insertions(+), 12 deletions(-) create mode 100644 .github/workflows/humble_cron.yaml create mode 100644 .github/workflows/jazzy_cron.yaml create mode 100644 .github/workflows/kilted_cron.yaml create mode 100644 .github/workflows/rolling_cron.yaml diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 129427d..35fd8f8 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -7,8 +7,6 @@ on: push: branches: - humble - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: runs-on: ${{ matrix.os }} @@ -29,7 +27,6 @@ jobs: with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: humble - ref: humble colcon-defaults: | { "test": { diff --git a/.github/workflows/humble_cron.yaml b/.github/workflows/humble_cron.yaml new file mode 100644 index 0000000..e08d607 --- /dev/null +++ b/.github/workflows/humble_cron.yaml @@ -0,0 +1,42 @@ +name: humble + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-22.04] + fail-fast: false + steps: + - uses: actions/checkout@v4 + with: + ref: humble + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + with: + required-ros-distributions: humble + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin + target-ros2-distro: humble + ref: humble + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 31ba809..5aa2840 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -7,8 +7,6 @@ on: push: branches: - jazzy - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: runs-on: ${{ matrix.os }} @@ -29,7 +27,6 @@ jobs: with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: jazzy - ref: jazzy colcon-defaults: | { "test": { diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml new file mode 100644 index 0000000..ca6bfca --- /dev/null +++ b/.github/workflows/jazzy_cron.yaml @@ -0,0 +1,42 @@ +name: jazzy + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-24.04] + fail-fast: false + steps: + - uses: actions/checkout@v4 + with: + ref: jazzy + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + with: + required-ros-distributions: jazzy + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin + target-ros2-distro: jazzy + ref: jazzy + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 1774f6e..cdd9384 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -7,8 +7,6 @@ on: push: branches: - kilted - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: runs-on: ${{ matrix.os }} @@ -29,7 +27,6 @@ jobs: with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: kilted - ref: kilted colcon-defaults: | { "test": { diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml new file mode 100644 index 0000000..3d1ba39 --- /dev/null +++ b/.github/workflows/kilted_cron.yaml @@ -0,0 +1,42 @@ +name: kilted + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-24.04] + fail-fast: false + steps: + - uses: actions/checkout@v4 + with: + ref: kilted + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + with: + required-ros-distributions: kilted + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin + target-ros2-distro: kilted + ref: kilted + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index a579a72..e50131c 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -7,8 +7,6 @@ on: push: branches: - rolling - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: runs-on: ${{ matrix.os }} @@ -29,7 +27,6 @@ jobs: with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: rolling - ref: rolling colcon-defaults: | { "test": { diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml new file mode 100644 index 0000000..039d9f9 --- /dev/null +++ b/.github/workflows/rolling_cron.yaml @@ -0,0 +1,42 @@ +name: rolling + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-24.04] + fail-fast: false + steps: + - uses: actions/checkout@v4 + with: + ref: rolling + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + with: + required-ros-distributions: rolling + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin + target-ros2-distro: rolling + ref: rolling + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false From dc47f4682b46c51816b8d9532039a2be754c56e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 21 Nov 2025 10:08:47 +0100 Subject: [PATCH 07/15] Fix potential linker error and warning MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_core/include/navmap_core/NavMap.hpp | 33 ++++++++++++++++++++ navmap_core/src/navmap_core/NavMap.cpp | 35 +--------------------- 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 7a155d0..6ad0a41 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -199,6 +199,39 @@ struct LayerView : LayerViewBase ///@} }; +/** @cond INTERNAL */ +namespace detail +{ +inline std::uint64_t fnv1a64_bytes( + const void * data, std::size_t n, + std::uint64_t seed = 1469598103934665603ULL) +{ + const auto * p = static_cast(data); + std::uint64_t h = seed; + for (std::size_t i = 0; i < n; ++i) { + h ^= p[i]; h *= 1099511628211ULL; + } + return h; +} +} // namespace detail +/** @endcond */ + +template +std::uint64_t LayerView::content_hash() const +{ + if (!hash_dirty_) {return hash_cache_;} + const std::size_t n = data_.size(); + std::uint64_t h = navmap::detail::fnv1a64_bytes(&n, sizeof(n)); + if (n) { + static_assert(std::is_trivially_copyable::value, + "LayerView requires trivially copyable T."); + h = navmap::detail::fnv1a64_bytes(data_.data(), n * sizeof(T), h); + } + hash_cache_ = h; + hash_dirty_ = false; + return hash_cache_; +} + /** * \brief Registry of named layers (per-NavCel). * diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index 6a47303..0eb0380 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -25,39 +25,6 @@ namespace navmap { -/** @cond INTERNAL */ -namespace navmap -{namespace detail -{ -inline std::uint64_t fnv1a64_bytes( - const void * data, std::size_t n, - std::uint64_t seed = 1469598103934665603ULL) -{ - const auto * p = static_cast(data); - std::uint64_t h = seed; - for (std::size_t i = 0; i < n; ++i) { - h ^= p[i]; h *= 1099511628211ULL; - } - return h; -} -}} // namespaces -/** @endcond */ - -template -std::uint64_t LayerView::content_hash() const -{ - if (!hash_dirty_) {return hash_cache_;} - const std::size_t n = data_.size(); - std::uint64_t h = navmap::detail::fnv1a64_bytes(&n, sizeof(n)); - if (n) { - static_assert(std::is_trivially_copyable::value, - "LayerView requires trivially copyable T."); - h = navmap::detail::fnv1a64_bytes(data_.data(), n * sizeof(T), h); - } - hash_cache_ = h; - hash_dirty_ = false; - return hash_cache_; -} namespace { @@ -472,7 +439,7 @@ bool NavMap::raycast( { bool any = false; float best_t = std::numeric_limits::infinity(); - Vec3 best_p; + Vec3 best_p = Vec3::Zero(); NavCelId best_cid = 0; for (const auto & s : surfaces) { From 3849f2b0f767397315bd73e54f50e16163fc43ce Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Fri, 21 Nov 2025 18:34:10 +0100 Subject: [PATCH 08/15] Cleanup unused headers --- .gitignore | 8 +++++++ navmap_core/include/navmap_core/Geometry.hpp | 1 - navmap_core/include/navmap_core/NavMap.hpp | 2 -- navmap_core/src/navmap_core/NavMap.cpp | 1 + navmap_core/tests/test_geometry.cpp | 1 - .../tests/test_navmap_uniform_and_closest.cpp | 1 - navmap_examples/src/01_flat_plane.cpp | 4 ---- navmap_examples/src/02_two_floors.cpp | 8 +------ navmap_examples/src/03_slope_surface.cpp | 8 +------ navmap_examples/src/04_layers.cpp | 8 +------ .../src/05_neighbors_and_centroids.cpp | 8 +------ navmap_examples/src/06_area_marking.cpp | 8 +------ navmap_examples/src/07_raycast.cpp | 7 ------- navmap_examples/src/08_copy_and_assign.cpp | 6 ------ navmap_ros/include/navmap_ros/conversions.hpp | 2 -- navmap_ros/include/navmap_ros/navmap_io.hpp | 1 - navmap_ros/src/navmap_ros/conversions.cpp | 8 ++----- navmap_ros/tests/test_conversions.cpp | 1 - navmap_ros/tests/test_navmap_io.cpp | 5 ++--- .../navmap_rviz_plugin/NavMapDisplay.hpp | 2 -- .../navmap_rviz_plugin/navmap_goal_tool.hpp | 3 +-- .../navmap_rviz_plugin/navmap_pose_tool.hpp | 3 --- .../src/navmap_rviz_plugin/NavMapDisplay.cpp | 21 ++----------------- .../navmap_rviz_plugin/navmap_goal_tool.cpp | 1 - .../navmap_rviz_plugin/navmap_pose_tool.cpp | 4 ++++ 25 files changed, 25 insertions(+), 97 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..46e553e --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +# VS Code stuff +/.vscode/** +**/__pycache__/ + +# ROS 2 build files +build/ +install/ +log/ diff --git a/navmap_core/include/navmap_core/Geometry.hpp b/navmap_core/include/navmap_core/Geometry.hpp index 8b63286..c251d03 100644 --- a/navmap_core/include/navmap_core/Geometry.hpp +++ b/navmap_core/include/navmap_core/Geometry.hpp @@ -34,7 +34,6 @@ #include #include #include -#include namespace navmap { diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 6ad0a41..b380c72 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -45,10 +45,8 @@ #include #include #include -#include #include #include -#include #include #include #include diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index 0eb0380..3131485 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -15,6 +15,7 @@ #include "navmap_core/NavMap.hpp" +#include #include #include #include diff --git a/navmap_core/tests/test_geometry.cpp b/navmap_core/tests/test_geometry.cpp index 2e63771..492a435 100644 --- a/navmap_core/tests/test_geometry.cpp +++ b/navmap_core/tests/test_geometry.cpp @@ -15,7 +15,6 @@ #include #include -#include #include "navmap_core/Geometry.hpp" using namespace navmap; diff --git a/navmap_core/tests/test_navmap_uniform_and_closest.cpp b/navmap_core/tests/test_navmap_uniform_and_closest.cpp index 446a3cc..9e49c29 100644 --- a/navmap_core/tests/test_navmap_uniform_and_closest.cpp +++ b/navmap_core/tests/test_navmap_uniform_and_closest.cpp @@ -15,7 +15,6 @@ #include #include -#include #include "navmap_core/NavMap.hpp" using namespace navmap; diff --git a/navmap_examples/src/01_flat_plane.cpp b/navmap_examples/src/01_flat_plane.cpp index 130bba3..326d8a0 100644 --- a/navmap_examples/src/01_flat_plane.cpp +++ b/navmap_examples/src/01_flat_plane.cpp @@ -17,16 +17,12 @@ #include #include #include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; using navmap::LayerType; using Eigen::Vector3f; using std::cout; using std::cerr; using std::endl; diff --git a/navmap_examples/src/02_two_floors.cpp b/navmap_examples/src/02_two_floors.cpp index e3ccd1b..ac89f44 100644 --- a/navmap_examples/src/02_two_floors.cpp +++ b/navmap_examples/src/02_two_floors.cpp @@ -16,20 +16,14 @@ #include #include -#include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 02_two_floors: two stacked floors, locate & closest_navcel static void make_two_floors(NavMap & nm, float z0, float z1) diff --git a/navmap_examples/src/03_slope_surface.cpp b/navmap_examples/src/03_slope_surface.cpp index c4cbcb6..f479cb9 100644 --- a/navmap_examples/src/03_slope_surface.cpp +++ b/navmap_examples/src/03_slope_surface.cpp @@ -16,20 +16,14 @@ #include #include -#include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 03_slope_surface: sloped z, sample_layer_at int main() diff --git a/navmap_examples/src/04_layers.cpp b/navmap_examples/src/04_layers.cpp index 5edfffe..efb2617 100644 --- a/navmap_examples/src/04_layers.cpp +++ b/navmap_examples/src/04_layers.cpp @@ -15,21 +15,15 @@ #include -#include #include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 04_layers: add/list/set/get int main() diff --git a/navmap_examples/src/05_neighbors_and_centroids.cpp b/navmap_examples/src/05_neighbors_and_centroids.cpp index 7f11065..954c595 100644 --- a/navmap_examples/src/05_neighbors_and_centroids.cpp +++ b/navmap_examples/src/05_neighbors_and_centroids.cpp @@ -16,20 +16,14 @@ #include #include -#include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 05_neighbors_and_centroids static void make_flat_square(NavMap & nm) diff --git a/navmap_examples/src/06_area_marking.cpp b/navmap_examples/src/06_area_marking.cpp index fdda86c..603bc95 100644 --- a/navmap_examples/src/06_area_marking.cpp +++ b/navmap_examples/src/06_area_marking.cpp @@ -15,21 +15,15 @@ #include -#include #include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 06_area_marking: set_area CIRCULAR y RECTANGULAR sobre una malla 1x1 de 2 tris #include diff --git a/navmap_examples/src/07_raycast.cpp b/navmap_examples/src/07_raycast.cpp index ce97653..4ff7d8d 100644 --- a/navmap_examples/src/07_raycast.cpp +++ b/navmap_examples/src/07_raycast.cpp @@ -16,20 +16,13 @@ #include #include -#include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; // 07_raycast: simple y batch (raycast_many) int main() diff --git a/navmap_examples/src/08_copy_and_assign.cpp b/navmap_examples/src/08_copy_and_assign.cpp index 0b84de4..a11affb 100644 --- a/navmap_examples/src/08_copy_and_assign.cpp +++ b/navmap_examples/src/08_copy_and_assign.cpp @@ -17,19 +17,13 @@ #include #include #include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; // 08_copy_and_assign: muestra operator= optimizado (igual geometría) y completo (distinta) static void fill_one_tri_map(navmap::NavMap & m) diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index a7e9f49..1d5ef88 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -37,8 +37,6 @@ */ #include -#include -#include #include #include #include diff --git a/navmap_ros/include/navmap_ros/navmap_io.hpp b/navmap_ros/include/navmap_ros/navmap_io.hpp index a2b8d60..1500b28 100644 --- a/navmap_ros/include/navmap_ros/navmap_io.hpp +++ b/navmap_ros/include/navmap_ros/navmap_io.hpp @@ -41,7 +41,6 @@ #include #include "navmap_core/NavMap.hpp" -#include "navmap_ros/conversions.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 65a3eb1..276e1dd 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -25,21 +25,17 @@ #include #include "geometry_msgs/msg/pose.hpp" -#include +#include "std_msgs/msg/header.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" -#include "navmap_core/Geometry.hpp" - #include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" +#include "pcl/common/point_tests.h" -#include "pcl/common/transforms.h" #include "pcl/point_cloud.h" #include "pcl/point_types.h" -#include "pcl/PointIndices.h" #include "pcl/kdtree/kdtree_flann.h" namespace navmap_ros diff --git a/navmap_ros/tests/test_conversions.cpp b/navmap_ros/tests/test_conversions.cpp index 54bc3f0..8d0d021 100644 --- a/navmap_ros/tests/test_conversions.cpp +++ b/navmap_ros/tests/test_conversions.cpp @@ -17,7 +17,6 @@ #include #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" -#include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros/conversions.hpp" #include "navmap_core/NavMap.hpp" diff --git a/navmap_ros/tests/test_navmap_io.cpp b/navmap_ros/tests/test_navmap_io.cpp index 07a6209..c6674cb 100644 --- a/navmap_ros/tests/test_navmap_io.cpp +++ b/navmap_ros/tests/test_navmap_io.cpp @@ -18,7 +18,6 @@ #include #include -#include #include #include #include @@ -92,7 +91,7 @@ static void ExpectNavMapMsgEqualSemantic( const navmap_ros_interfaces::msg::NavMap & A, const navmap_ros_interfaces::msg::NavMap & B) { - // Header: frame must match; stamp puede variar → lo ignoramos + // Header: frame must match; stamp may change -> we ignore it EXPECT_EQ(A.header.frame_id, B.header.frame_id); // Geometry @@ -306,7 +305,7 @@ ASSERT_TRUE(navmap_ros::io::load_from_file(path, core_loaded, &ec)) << ec.messag auto msg_from_core = navmap_ros::to_msg(core); auto msg_from_core_loaded = navmap_ros::to_msg(core_loaded); -// Comparación semántica (tolerante a orden y FP) +// Semantic comparison (order and FP tolerant) ExpectNavMapMsgEqualSemantic(msg_from_core, msg_from_core_loaded); std::filesystem::remove(path); diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp index 4ef3f86..1e62d8f 100644 --- a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp @@ -18,10 +18,8 @@ #define NAVMAP_RVIZ_PLUGIN__NAVMAP_DISPLAY_HPP_ #include -#include #include #include -#include #include 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 index 7c00499..7a4562d 100644 --- a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp @@ -20,8 +20,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/qos.hpp" +#include "rclcpp/rclcpp.hpp" #include "navmap_rviz_plugin/navmap_pose_tool.hpp" #include "rviz_default_plugins/visibility_control.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 index 9b20898..0cdae6a 100644 --- a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp @@ -32,9 +32,6 @@ #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; diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index a64d61f..ad64a76 100644 --- a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp @@ -14,39 +14,22 @@ // limitations under the License. -#include "navmap_rviz_plugin/NavMapDisplay.hpp" +#include #include #include -#include #include #include #include -#include -#include #include #include -#include #include #include -#include -#include - -#include -#include -#include -#include -#include -#include #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" - -#include -#include -#include - +#include "navmap_rviz_plugin/NavMapDisplay.hpp" namespace { 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 index 887a433..1d1aa85 100644 --- a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp @@ -21,7 +21,6 @@ #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" 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 index 4897f89..398f5bd 100644 --- a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp @@ -33,11 +33,15 @@ #include "rviz_rendering/render_window.hpp" #include "rviz_common/logging.hpp" +#include "rviz_common/display_context.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" +#include "navmap_core/NavMap.hpp" +#include "navmap_rviz_plugin/NavMapDisplay.hpp" + namespace navmap_rviz_plugin { From ef4ae1ce139081b6e25f5afdac99d02f0df28231 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 21 Nov 2025 20:17:59 +0100 Subject: [PATCH 09/15] Sppedup the navcel location MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_core/src/navmap_core/NavMap.cpp | 77 ++++++++++++++++++++------ 1 file changed, 61 insertions(+), 16 deletions(-) diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index 0eb0380..a8477c0 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -546,7 +546,7 @@ bool NavMap::locate_by_walking( Vec3 * hit_pt, float planar_eps) const { - const int kMaxSteps = 64; + const int kMaxSteps = 16; NavCelId cid = start_cid; for (int step = 0; step < kMaxSteps; ++step) { @@ -599,25 +599,70 @@ bool NavMap::locate_navcel_core( Vec3 * hit_pt, const LocateOpts & opts) const { - // 1) Try walking if there is a valid hint. + // 0) Fast path: directly test the hinted triangle, if any. if (opts.hint_cid.has_value()) { - if (locate_by_walking(opts.hint_cid.value(), - p_world, - cid, - bary, - hit_pt, - opts.planar_eps)) - { - for (size_t s = 0; s < surfaces.size(); ++s) { - const auto & surf = surfaces[s]; - if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != - surf.navcels.end()) + const NavCelId hint = *opts.hint_cid; + if (hint < navcels.size()) { + const auto & c = navcels[hint]; + const Vec3 a = positions.at(c.v[0]); + const Vec3 b = positions.at(c.v[1]); + const Vec3 d = positions.at(c.v[2]); + const Vec3 & n = c.normal; + + const float dist = n.dot(p_world - a); + const Vec3 q = p_world - dist * n; + + Vec3 bary_hint; + if (point_in_triangle_bary(q, a, b, d, bary_hint, opts.planar_eps) && + std::fabs(dist) <= opts.height_eps) + { + cid = hint; + bary = bary_hint; + if (hit_pt) { + *hit_pt = q; + } + + // Find the surface that owns this navcel. + for (size_t s = 0; s < surfaces.size(); ++s) { + const auto & surf = surfaces[s]; + if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != surf.navcels.end()) { + surface_idx = s; + return true; + } + } + // If no surface owns the hinted navcel, fall through to the generic search. + } + } + } + + // 1) Try walking if there is a valid hint and we are not far from its plane. + if (opts.hint_cid.has_value()) { + const NavCelId start = *opts.hint_cid; + if (start < navcels.size()) { + const auto & c0 = navcels[start]; + const Vec3 a0 = positions.at(c0.v[0]); + const Vec3 & n0 = c0.normal; + const float dist0 = n0.dot(p_world - a0); + + // Do not walk if the query point is clearly off the hinted plane. + if (std::fabs(dist0) <= opts.height_eps) { + if (locate_by_walking(start, + p_world, + cid, + bary, + hit_pt, + opts.planar_eps)) { - surface_idx = s; - return true; + for (size_t s = 0; s < surfaces.size(); ++s) { + const auto & surf = surfaces[s]; + if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != surf.navcels.end()) { + surface_idx = s; + return true; + } + } + // Fall through if surface not found. } } - // Fall through if surface not found. } } From f18f70510a8d27f0ac2b56eba73f817b98849d57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 10 Jan 2026 08:36:49 +0100 Subject: [PATCH 10/15] Add headers in conversions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/include/navmap_ros/conversions.hpp | 52 +++++++++- navmap_ros/src/navmap_ros/conversions.cpp | 67 +++++++++++-- navmap_ros/tests/test_conversions.cpp | 94 ++++++++++++++++--- navmap_ros/tests/test_navmap_io.cpp | 12 ++- 4 files changed, 200 insertions(+), 25 deletions(-) diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 1d5ef88..f073f08 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -41,6 +41,7 @@ #include #include +#include "std_msgs/msg/header.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" @@ -81,6 +82,7 @@ constexpr uint8_t FREE_SPACE = 0; * @brief Convert a core `navmap::NavMap` into its compact ROS transport message. * * @param[in] nm Core NavMap to be serialized into a ROS message. + * @param[in] header Header to assign to the resulting message. * @return A `navmap_ros_interfaces::msg::NavMap` containing geometry (vertices, triangles), * surfaces metadata and user-defined layers. * @@ -91,12 +93,20 @@ constexpr uint8_t FREE_SPACE = 0; * @note This function does not perform IO; it only builds the message in-memory. */ navmap_ros_interfaces::msg::NavMap to_msg( - const navmap::NavMap & nm); + const navmap::NavMap & nm, const std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (no header provided). + * + * The returned message header will be default-constructed. + */ +navmap_ros_interfaces::msg::NavMap to_msg(const navmap::NavMap & nm); /** * @brief Reconstruct a core `navmap::NavMap` from the ROS transport message. * * @param[in] msg Input `navmap_ros_interfaces::msg::NavMap` message. + * @param[out] header Header extracted from the message. * @return A core `navmap::NavMap` equivalent to the content of @p msg. * * @details @@ -105,6 +115,13 @@ navmap_ros_interfaces::msg::NavMap to_msg( * * @throw std::runtime_error If the message describes inconsistent geometry or layer sizes. */ +navmap::NavMap from_msg( + const navmap_ros_interfaces::msg::NavMap & msg, + std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (ignores message header). + */ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); /** @@ -112,6 +129,7 @@ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); * * @param[in] nm Input NavMap. * @param[in] layer Name of the layer to export. + * @param[in] header Header to assign to the resulting message. * @return A NavMapLayer message containing the layer values and metadata. * * @details @@ -121,6 +139,14 @@ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); * * @throw std::runtime_error If the layer does not exist or has an unsupported type. */ +navmap_ros_interfaces::msg::NavMapLayer to_msg( + const navmap::NavMap & nm, + const std::string & layer, + const std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (no header provided). + */ navmap_ros_interfaces::msg::NavMapLayer to_msg( const navmap::NavMap & nm, const std::string & layer); @@ -133,6 +159,7 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( * * @param[in] msg Input NavMapLayer message. * @param[in,out] nm Destination NavMap (must already have navcels sized correctly). + * @param[in] header Header to assign to the resulting message. * * @details * - The function verifies that the length of the populated data array matches @@ -141,6 +168,14 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( * * @throw std::runtime_error If sizes are inconsistent or the message is ill-formed. */ +void from_msg( + const navmap_ros_interfaces::msg::NavMapLayer & msg, + navmap::NavMap & nm, + std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (ignores message header). + */ void from_msg( const navmap_ros_interfaces::msg::NavMapLayer & msg, navmap::NavMap & nm); @@ -152,6 +187,7 @@ void from_msg( * using a regular triangular surface with shared vertices. * * @param[in] grid Input ROS OccupancyGrid (row-major, width×height, resolution and origin). + * @param[in] header Header to assign to the resulting message. * @return A core `navmap::NavMap` with: * - Vertices: `(W+1) * (H+1)` laid on the grid plane, with `Z = grid.info.origin.position.z`. * - Triangles: `2 * W * H` (two per cell), using diagonal pattern = 0. @@ -167,6 +203,13 @@ void from_msg( * @note The grid origin pose may contain a rotation. The vertex Z is taken from the origin Z; * handling of non-zero yaw/roll/pitch (if any) is implementation-defined in the builder. */ +navmap::NavMap from_occupancy_grid( + const nav_msgs::msg::OccupancyGrid & grid, + std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (ignores grid header). + */ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid); /** @@ -192,6 +235,13 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid); * @warning If the map does not carry grid metadata or the `"occupancy"` layer is missing, * the result may be incomplete or implementation-defined. */ +nav_msgs::msg::OccupancyGrid to_occupancy_grid( + const navmap::NavMap & nm, + const std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload. + */ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm); /** diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 276e1dd..e77d552 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -67,9 +67,10 @@ static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32 // ----------------- NavMap <-> ROS message ----------------- -NavMap to_msg(const navmap::NavMap & nm) +NavMap to_msg(const navmap::NavMap & nm, const std_msgs::msg::Header & header) { NavMap out; + out.header = header; // positions out.positions_x.assign(nm.positions.x.begin(), nm.positions.x.end()); @@ -135,8 +136,14 @@ NavMap to_msg(const navmap::NavMap & nm) return out; } -navmap::NavMap from_msg(const NavMap & msg) +NavMap to_msg(const navmap::NavMap & nm) { + return to_msg(nm, std_msgs::msg::Header()); +} + +navmap::NavMap from_msg(const NavMap & msg, std_msgs::msg::Header & header) +{ + header = msg.header; navmap::NavMap nm; // positions @@ -203,11 +210,19 @@ navmap::NavMap from_msg(const NavMap & msg) return nm; } +navmap::NavMap from_msg(const NavMap & msg) +{ + std_msgs::msg::Header unused; + return from_msg(msg, unused); +} + navmap_ros_interfaces::msg::NavMapLayer to_msg( const navmap::NavMap & nm, - const std::string & layer_name) + const std::string & layer_name, + const std_msgs::msg::Header & header) { navmap_ros_interfaces::msg::NavMapLayer msg; + msg.header = header; msg.name = layer_name; auto base = nm.layers.get(layer_name); @@ -241,10 +256,19 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( return msg; } +navmap_ros_interfaces::msg::NavMapLayer to_msg( + const navmap::NavMap & nm, + const std::string & layer_name) +{ + return to_msg(nm, layer_name, std_msgs::msg::Header()); +} + void from_msg( const navmap_ros_interfaces::msg::NavMapLayer & msg, - navmap::NavMap & nm) + navmap::NavMap & nm, + std_msgs::msg::Header & header) { + header = msg.header; switch (msg.type) { case navmap_ros_interfaces::msg::NavMapLayer::U8: { auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", uint8_t{}); @@ -276,10 +300,21 @@ void from_msg( } } +void from_msg( + const navmap_ros_interfaces::msg::NavMapLayer & msg, + navmap::NavMap & nm) +{ + std_msgs::msg::Header unused; + from_msg(msg, nm, unused); +} + // ----------------- OccupancyGrid <-> NavMap ----------------- -navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) +navmap::NavMap from_occupancy_grid( + const nav_msgs::msg::OccupancyGrid & grid, + std_msgs::msg::Header & header) { + header = grid.header; navmap::NavMap nm; const uint32_t W = grid.info.width; @@ -349,10 +384,21 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) return nm; } -nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) +navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) +{ + std_msgs::msg::Header unused; + return from_occupancy_grid(grid, unused); +} + +nav_msgs::msg::OccupancyGrid to_occupancy_grid( + const navmap::NavMap & nm, + const std_msgs::msg::Header & header) { nav_msgs::msg::OccupancyGrid g; - g.header.frame_id = (nm.surfaces.empty() ? std::string() : nm.surfaces[0].frame_id); + g.header = header; + if (g.header.frame_id.empty() && !nm.surfaces.empty()) { + g.header.frame_id = nm.surfaces[0].frame_id; + } auto base = nm.layers.get("occupancy"); if (!base || base->type() != navmap::LayerType::U8) { @@ -449,6 +495,13 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) return g; } +nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) +{ + std_msgs::msg::Header h; + h.frame_id = (nm.surfaces.empty() ? std::string() : nm.surfaces[0].frame_id); + return to_occupancy_grid(nm, h); +} + bool build_navmap_from_mesh( const pcl::PointCloud & cloud, const std::vector & triangles, diff --git a/navmap_ros/tests/test_conversions.cpp b/navmap_ros/tests/test_conversions.cpp index 8d0d021..266fe6e 100644 --- a/navmap_ros/tests/test_conversions.cpp +++ b/navmap_ros/tests/test_conversions.cpp @@ -17,6 +17,7 @@ #include #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" +#include "std_msgs/msg/header.hpp" #include "navmap_ros/conversions.hpp" #include "navmap_core/NavMap.hpp" @@ -124,8 +125,17 @@ static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32 TEST(NavMap_FullConversions, RoundTrip_All) { navmap::NavMap a; build_square_with_layers(a); - auto msg = navmap_ros::to_msg(a); - navmap::NavMap b = navmap_ros::from_msg(msg); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 123; + h.stamp.nanosec = 456; + auto msg = navmap_ros::to_msg(a, h); + std_msgs::msg::Header h2; + navmap::NavMap b = navmap_ros::from_msg(msg, h2); + + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_EQ(a.positions.size(), b.positions.size()); for (size_t i = 0; i < a.positions.size(); ++i) { @@ -170,8 +180,16 @@ TEST(NavMap_FullConversions, RoundTrip_All) TEST(NavMap_FullConversions, EmptyMap_RoundTrip) { navmap::NavMap a; - auto msg = navmap_ros::to_msg(a); - navmap::NavMap b = navmap_ros::from_msg(msg); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 1; + h.stamp.nanosec = 2; + auto msg = navmap_ros::to_msg(a, h); + std_msgs::msg::Header h2; + navmap::NavMap b = navmap_ros::from_msg(msg, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(b.positions.size(), 0u); EXPECT_EQ(b.navcels.size(), 0u); EXPECT_EQ(b.surfaces.size(), 0u); @@ -184,7 +202,14 @@ TEST(NavMap_LayerConversions, U8_RoundTrip) auto occ = nm.add_layer("occupancy", "occ", "", uint8_t(0)); occ->data()[0] = 10u; occ->data()[1] = 250u; - auto msg = navmap_ros::to_msg(nm, "occupancy"); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 10; + h.stamp.nanosec = 20; + auto msg = navmap_ros::to_msg(nm, "occupancy", h); + EXPECT_EQ(msg.header.frame_id, h.frame_id); + EXPECT_EQ(msg.header.stamp.sec, h.stamp.sec); + EXPECT_EQ(msg.header.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(msg.name, "occupancy"); EXPECT_EQ(msg.type, 0u); // 0=U8 ASSERT_EQ(msg.data_u8.size(), 2u); @@ -192,7 +217,11 @@ TEST(NavMap_LayerConversions, U8_RoundTrip) EXPECT_EQ(msg.data_u8[1], 250u); navmap::NavMap nm2; make_flat_square(nm2); - navmap_ros::from_msg(msg, nm2); + std_msgs::msg::Header h2; + navmap_ros::from_msg(msg, nm2, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_TRUE(nm2.has_layer("occupancy")); EXPECT_NEAR(nm2.layer_get("occupancy", 0), 10.0, 1e-6); EXPECT_NEAR(nm2.layer_get("occupancy", 1), 250.0, 1e-6); @@ -204,14 +233,25 @@ TEST(NavMap_LayerConversions, F32_RoundTrip) auto cost = nm.add_layer("cost", "cost", "", 0.0f); cost->data()[0] = 1.25f; cost->data()[1] = 9.5f; - auto msg = navmap_ros::to_msg(nm, "cost"); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 11; + h.stamp.nanosec = 22; + auto msg = navmap_ros::to_msg(nm, "cost", h); + EXPECT_EQ(msg.header.frame_id, h.frame_id); + EXPECT_EQ(msg.header.stamp.sec, h.stamp.sec); + EXPECT_EQ(msg.header.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(msg.type, 1u); // 1=F32 ASSERT_EQ(msg.data_f32.size(), 2u); EXPECT_NEAR(msg.data_f32[0], 1.25f, 1e-6); EXPECT_NEAR(msg.data_f32[1], 9.5f, 1e-6); navmap::NavMap nm2; make_flat_square(nm2); - navmap_ros::from_msg(msg, nm2); + std_msgs::msg::Header h2; + navmap_ros::from_msg(msg, nm2, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_TRUE(nm2.has_layer("cost")); EXPECT_NEAR(nm2.layer_get("cost", 0), 1.25, 1e-6); EXPECT_NEAR(nm2.layer_get("cost", 1), 9.5, 1e-6); @@ -224,14 +264,25 @@ TEST(NavMap_LayerConversions, F64_RoundTrip) elev->data()[0] = 12.345; elev->data()[1] = -2.5; - auto msg = navmap_ros::to_msg(nm, "elevation"); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 12; + h.stamp.nanosec = 24; + auto msg = navmap_ros::to_msg(nm, "elevation", h); + EXPECT_EQ(msg.header.frame_id, h.frame_id); + EXPECT_EQ(msg.header.stamp.sec, h.stamp.sec); + EXPECT_EQ(msg.header.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(msg.type, 2u); // 2=F64 ASSERT_EQ(msg.data_f64.size(), 2u); EXPECT_NEAR(msg.data_f64[0], 12.345, 1e-9); EXPECT_NEAR(msg.data_f64[1], -2.5, 1e-9); navmap::NavMap nm2; make_flat_square(nm2); - navmap_ros::from_msg(msg, nm2); + std_msgs::msg::Header h2; + navmap_ros::from_msg(msg, nm2, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_TRUE(nm2.has_layer("elevation")); EXPECT_NEAR(nm2.layer_get("elevation", 0), 12.345, 1e-9); EXPECT_NEAR(nm2.layer_get("elevation", 1), -2.5, 1e-9); @@ -247,9 +298,23 @@ TEST(TestConversions, RoundTrip_ExactEquality_4m_0p1) { const int W = 40, H = 40; auto g = make_grid_4m_0p1(); - - auto nm = from_occupancy_grid(g); - auto gout = to_occupancy_grid(nm); + g.header.stamp.sec = 111; + g.header.stamp.nanosec = 222; + + std_msgs::msg::Header h_in; + auto nm = from_occupancy_grid(g, h_in); + EXPECT_EQ(h_in.frame_id, g.header.frame_id); + EXPECT_EQ(h_in.stamp.sec, g.header.stamp.sec); + EXPECT_EQ(h_in.stamp.nanosec, g.header.stamp.nanosec); + + std_msgs::msg::Header h_out; + h_out.frame_id = "map"; + h_out.stamp.sec = 333; + h_out.stamp.nanosec = 444; + auto gout = to_occupancy_grid(nm, h_out); + EXPECT_EQ(gout.header.frame_id, h_out.frame_id); + EXPECT_EQ(gout.header.stamp.sec, h_out.stamp.sec); + EXPECT_EQ(gout.header.stamp.nanosec, h_out.stamp.nanosec); ASSERT_EQ(gout.info.width, g.info.width); ASSERT_EQ(gout.info.height, g.info.height); @@ -293,7 +358,8 @@ TEST(TestConversions, TriangleIndicesFollowPattern0) { const int W = 40; auto g = make_grid_4m_0p1(); - auto nm = from_occupancy_grid(g); + std_msgs::msg::Header unused; + auto nm = from_occupancy_grid(g, unused); // Pick a cell and verify its triangles reference the expected 4 vertices. auto v_id = [W](uint32_t i, uint32_t j) -> navmap::PointId { diff --git a/navmap_ros/tests/test_navmap_io.cpp b/navmap_ros/tests/test_navmap_io.cpp index c6674cb..8168125 100644 --- a/navmap_ros/tests/test_navmap_io.cpp +++ b/navmap_ros/tests/test_navmap_io.cpp @@ -23,6 +23,8 @@ #include #include +#include "std_msgs/msg/header.hpp" + #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" #include "navmap_ros/navmap_io.hpp" @@ -290,7 +292,9 @@ TEST(NavMapIoCore, RoundtripViaCoreAndMsgCompare) msg.layers = {u8, f32}; // msg -> core -navmap::NavMap core = navmap_ros::from_msg(msg); +std_msgs::msg::Header h_in; +navmap::NavMap core = navmap_ros::from_msg(msg, h_in); +EXPECT_EQ(h_in.frame_id, msg.header.frame_id); // save(core) -> load(core) std::string path = (std::filesystem::temp_directory_path() / @@ -302,8 +306,10 @@ navmap::NavMap core_loaded; ASSERT_TRUE(navmap_ros::io::load_from_file(path, core_loaded, &ec)) << ec.message(); // core -> msg -auto msg_from_core = navmap_ros::to_msg(core); -auto msg_from_core_loaded = navmap_ros::to_msg(core_loaded); +std_msgs::msg::Header h_out; +h_out.frame_id = msg.header.frame_id; +auto msg_from_core = navmap_ros::to_msg(core, h_out); +auto msg_from_core_loaded = navmap_ros::to_msg(core_loaded, h_out); // Semantic comparison (order and FP tolerant) ExpectNavMapMsgEqualSemantic(msg_from_core, msg_from_core_loaded); From 04139b2c9d63665ee0174e21938101faaac16dc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 10 Jan 2026 08:44:01 +0100 Subject: [PATCH 11/15] Fix doc in header MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/include/navmap_ros/conversions.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index f073f08..b5843a2 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -159,7 +159,7 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( * * @param[in] msg Input NavMapLayer message. * @param[in,out] nm Destination NavMap (must already have navcels sized correctly). - * @param[in] header Header to assign to the resulting message. + * @param[out] header Header extracted from the message. * * @details * - The function verifies that the length of the populated data array matches @@ -187,7 +187,7 @@ void from_msg( * using a regular triangular surface with shared vertices. * * @param[in] grid Input ROS OccupancyGrid (row-major, width×height, resolution and origin). - * @param[in] header Header to assign to the resulting message. + * @param[out] header Header to assign to the resulting message. * @return A core `navmap::NavMap` with: * - Vertices: `(W+1) * (H+1)` laid on the grid plane, with `Z = grid.info.origin.position.z`. * - Triangles: `2 * W * H` (two per cell), using diagonal pattern = 0. From c4b4e263677bc15feed24aaf2b60a08cf8860b5b Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Tue, 3 Feb 2026 16:02:19 +0100 Subject: [PATCH 12/15] Update CI to use container workers --- .github/workflows/humble.yaml | 10 +++------- .github/workflows/humble_cron.yaml | 10 +++------- .github/workflows/jazzy.yaml | 10 +++------- .github/workflows/jazzy_cron.yaml | 10 +++------- .github/workflows/kilted.yaml | 10 +++------- .github/workflows/kilted_cron.yaml | 10 +++------- .github/workflows/rolling.yaml | 10 +++------- .github/workflows/rolling_cron.yaml | 10 +++------- 8 files changed, 24 insertions(+), 56 deletions(-) diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 35fd8f8..b872da0 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -9,19 +9,15 @@ on: - humble jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-22.04] - fail-fast: false + runs-on: ubuntu-22.04 + container: + image: ubuntu:jammy steps: - uses: actions/checkout@v4 with: ref: humble - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: humble - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/humble_cron.yaml b/.github/workflows/humble_cron.yaml index e08d607..061a43d 100644 --- a/.github/workflows/humble_cron.yaml +++ b/.github/workflows/humble_cron.yaml @@ -5,19 +5,15 @@ on: - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-22.04] - fail-fast: false + runs-on: ubuntu-22.04 + container: + image: ubuntu:jammy steps: - uses: actions/checkout@v4 with: ref: humble - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: humble - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 5aa2840..558b75c 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -9,19 +9,15 @@ on: - jazzy jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - uses: actions/checkout@v4 with: ref: jazzy - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: jazzy - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml index ca6bfca..85c2a30 100644 --- a/.github/workflows/jazzy_cron.yaml +++ b/.github/workflows/jazzy_cron.yaml @@ -5,19 +5,15 @@ on: - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - uses: actions/checkout@v4 with: ref: jazzy - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: jazzy - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index cdd9384..c5b75e6 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -9,19 +9,15 @@ on: - kilted jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - uses: actions/checkout@v4 with: ref: kilted - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: kilted - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml index 3d1ba39..df98c2f 100644 --- a/.github/workflows/kilted_cron.yaml +++ b/.github/workflows/kilted_cron.yaml @@ -5,19 +5,15 @@ on: - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - uses: actions/checkout@v4 with: ref: kilted - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: kilted - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index e50131c..8d5ec03 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -9,19 +9,15 @@ on: - rolling jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - uses: actions/checkout@v4 with: ref: rolling - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: rolling - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml index 039d9f9..4134591 100644 --- a/.github/workflows/rolling_cron.yaml +++ b/.github/workflows/rolling_cron.yaml @@ -5,19 +5,15 @@ on: - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - uses: actions/checkout@v4 with: ref: rolling - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: rolling - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: From 5e0103761417e4e9d7e5f6b8ae6d6cb2247c61c1 Mon Sep 17 00:00:00 2001 From: estherag Date: Fri, 6 Feb 2026 09:24:46 +0100 Subject: [PATCH 13/15] PCL private linkage: avoid Qt5/6 conflicts --- navmap_ros/CMakeLists.txt | 7 +++++-- navmap_rviz_plugin/CMakeLists.txt | 18 ------------------ 2 files changed, 5 insertions(+), 20 deletions(-) diff --git a/navmap_ros/CMakeLists.txt b/navmap_ros/CMakeLists.txt index e2b6a3e..cb2e721 100644 --- a/navmap_ros/CMakeLists.txt +++ b/navmap_ros/CMakeLists.txt @@ -28,16 +28,18 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ ${PCL_INCLUDE_DIRS} ) +target_link_libraries(${PROJECT_NAME} PRIVATE + pcl_conversions::pcl_conversions + ${PCL_LIBRARIES} +) target_link_libraries(${PROJECT_NAME} PUBLIC rclcpp::rclcpp navmap_core::navmap_core - pcl_conversions::pcl_conversions ${navmap_ros_interfaces_TARGETS} ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} ${sensor_msgs_TARGETS} ${std_srvs_TARGETS} - ${PCL_LIBRARIES} ) add_executable(slam_server_app src/slam_server_app.cpp @@ -76,6 +78,7 @@ ament_export_dependencies( rclcpp navmap_core navmap_ros_interfaces + nav_msgs geometry_msgs sensor_msgs std_srvs diff --git a/navmap_rviz_plugin/CMakeLists.txt b/navmap_rviz_plugin/CMakeLists.txt index e33505b..c1dd3b8 100644 --- a/navmap_rviz_plugin/CMakeLists.txt +++ b/navmap_rviz_plugin/CMakeLists.txt @@ -1,14 +1,6 @@ cmake_minimum_required(VERSION 3.10) project(navmap_rviz_plugin) -# 1) Qt y AUTOMOC -find_package(Qt5 REQUIRED COMPONENTS Core Widgets) -set(CMAKE_AUTOMOC ON) -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) @@ -23,12 +15,6 @@ 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/navmap_rviz_plugin/NavMapDisplay.cpp src/navmap_rviz_plugin/navmap_goal_tool.cpp @@ -36,7 +22,6 @@ add_library(${PROJECT_NAME} SHARED 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} @@ -47,8 +32,6 @@ target_link_libraries(navmap_rviz_plugin PUBLIC rviz_common::rviz_common rviz_rendering::rviz_rendering rviz_default_plugins::rviz_default_plugins - Qt5::Core - Qt5::Widgets ) target_include_directories(${PROJECT_NAME} PUBLIC $ @@ -89,7 +72,6 @@ ament_export_dependencies( rviz_common rviz_rendering rviz_default_plugins - Qt5 navmap_ros_interfaces geometry_msgs std_msg From 6c50808e7a2b34ccba78be9fa6741afc63e1d134 Mon Sep 17 00:00:00 2001 From: estherag Date: Fri, 6 Feb 2026 11:40:54 +0100 Subject: [PATCH 14/15] Set Qt6 references and moc to proper plugin export --- navmap_rviz_plugin/CMakeLists.txt | 38 +++++++++++++++++++++++++++---- 1 file changed, 33 insertions(+), 5 deletions(-) diff --git a/navmap_rviz_plugin/CMakeLists.txt b/navmap_rviz_plugin/CMakeLists.txt index c1dd3b8..f61fe14 100644 --- a/navmap_rviz_plugin/CMakeLists.txt +++ b/navmap_rviz_plugin/CMakeLists.txt @@ -1,6 +1,12 @@ cmake_minimum_required(VERSION 3.10) project(navmap_rviz_plugin) +set(CMAKE_AUTORCC ON) +set(CMAKE_AUTOUIC ON) + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_common REQUIRED) @@ -12,19 +18,40 @@ 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) +find_package(QT NAMES Qt6 REQUIRED COMPONENTS Widgets Core) +find_package(Qt${QT_VERSION_MAJOR} REQUIRED COMPONENTS Widgets Core) + +set(QT_LIBRARIES + Qt${QT_VERSION_MAJOR}::Core + Qt${QT_VERSION_MAJOR}::Gui +) + +set(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 +) + +if (${QT_VERSION_MAJOR} GREATER "5") + qt_standard_project_setup() + qt_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 +) +endif() 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 - 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} + ${QT_QTCORE_LIBRARY} + ${QT_QTGUI_LIBRARY} + Qt${QT_VERSION_MAJOR}::Widgets navmap_ros::navmap_ros navmap_core::navmap_core pluginlib::pluginlib @@ -67,6 +94,7 @@ endif() ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( + Qt6 rclcpp pluginlib rviz_common From 4c125102115de019f34530a0b80c4df13b83f6e5 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Mon, 9 Feb 2026 16:43:48 +0100 Subject: [PATCH 15/15] Fully commit to Qt6 only and cleanup CMake --- navmap_rviz_plugin/CMakeLists.txt | 36 ++++++------------------------- 1 file changed, 6 insertions(+), 30 deletions(-) diff --git a/navmap_rviz_plugin/CMakeLists.txt b/navmap_rviz_plugin/CMakeLists.txt index f61fe14..ed73758 100644 --- a/navmap_rviz_plugin/CMakeLists.txt +++ b/navmap_rviz_plugin/CMakeLists.txt @@ -1,11 +1,7 @@ cmake_minimum_required(VERSION 3.10) project(navmap_rviz_plugin) -set(CMAKE_AUTORCC ON) -set(CMAKE_AUTOUIC ON) - -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) +find_package(Qt6 REQUIRED COMPONENTS Widgets) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -18,29 +14,11 @@ find_package(navmap_ros_interfaces REQUIRED) find_package(navmap_ros REQUIRED) find_package(navmap_core REQUIRED) -find_package(QT NAMES Qt6 REQUIRED COMPONENTS Widgets Core) -find_package(Qt${QT_VERSION_MAJOR} REQUIRED COMPONENTS Widgets Core) - -set(QT_LIBRARIES - Qt${QT_VERSION_MAJOR}::Core - Qt${QT_VERSION_MAJOR}::Gui -) - -set(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 +qt_wrap_cpp(NAVMAP_MOC_SRCS + include/navmap_rviz_plugin/NavMapDisplay.hpp + include/navmap_rviz_plugin/navmap_goal_tool.hpp ) -if (${QT_VERSION_MAJOR} GREATER "5") - qt_standard_project_setup() - qt_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 -) -endif() - add_library(${PROJECT_NAME} SHARED src/navmap_rviz_plugin/NavMapDisplay.cpp src/navmap_rviz_plugin/navmap_goal_tool.cpp @@ -49,9 +27,7 @@ add_library(${PROJECT_NAME} SHARED ) target_link_libraries(navmap_rviz_plugin PUBLIC ${navmap_ros_interfaces_TARGETS} - ${QT_QTCORE_LIBRARY} - ${QT_QTGUI_LIBRARY} - Qt${QT_VERSION_MAJOR}::Widgets + Qt6::Widgets navmap_ros::navmap_ros navmap_core::navmap_core pluginlib::pluginlib @@ -104,4 +80,4 @@ ament_export_dependencies( geometry_msgs std_msg ) -ament_package() \ No newline at end of file +ament_package()