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/29] 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/29] 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/29] 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/29] 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/29] 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/29] 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/29] 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/29] 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/29] 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 df483f5bbc9532e10d17f0e89efee2217b5cc45b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 24 Nov 2025 11:46:14 +0100 Subject: [PATCH 10/29] Fix pcl_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/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/navmap_ros/CMakeLists.txt b/navmap_ros/CMakeLists.txt index e2b6a3e..727269d 100644 --- a/navmap_ros/CMakeLists.txt +++ b/navmap_ros/CMakeLists.txt @@ -27,17 +27,18 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $ ${PCL_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} ) 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} + ${pcl_conversions_LIBRARIES} ) add_executable(slam_server_app src/slam_server_app.cpp From 9afa93c93370f1caf7082b3a700db6558bdd51d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 24 Nov 2025 11:54:01 +0100 Subject: [PATCH 11/29] Update Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_core/CHANGELOG.rst | 8 ++++++++ navmap_examples/CHANGELOG.rst | 6 ++++++ navmap_ros/CHANGELOG.rst | 14 ++++++++++++++ navmap_ros_interfaces/CHANGELOG.rst | 5 +++++ navmap_rviz_plugin/CHANGELOG.rst | 9 +++++++++ 5 files changed, 42 insertions(+) diff --git a/navmap_core/CHANGELOG.rst b/navmap_core/CHANGELOG.rst index ad00f61..a0c20e7 100644 --- a/navmap_core/CHANGELOG.rst +++ b/navmap_core/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package navmap_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Sppedup the navcel location +* Cleanup unused headers +* Fix potential linker error and warning +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_examples/CHANGELOG.rst b/navmap_examples/CHANGELOG.rst index e37829d..645d063 100644 --- a/navmap_examples/CHANGELOG.rst +++ b/navmap_examples/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package navmap_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Cleanup unused headers +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_ros/CHANGELOG.rst b/navmap_ros/CHANGELOG.rst index 676c47b..9bf37fc 100644 --- a/navmap_ros/CHANGELOG.rst +++ b/navmap_ros/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package navmap_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Cleanup unused headers +* Occupancy works +* Add occupancy grid constants +* Fix surface creation from points +* Remove unused field +* Remove some comments +* Final working version +* Acelerated respecting floors +* Working slow with many points +* Initial working version +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ * Fix pcl_conversions build diff --git a/navmap_ros_interfaces/CHANGELOG.rst b/navmap_ros_interfaces/CHANGELOG.rst index cda5d5f..613efae 100644 --- a/navmap_ros_interfaces/CHANGELOG.rst +++ b/navmap_ros_interfaces/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package navmap_ros_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_rviz_plugin/CHANGELOG.rst b/navmap_rviz_plugin/CHANGELOG.rst index 8b5da4d..62c8c6c 100644 --- a/navmap_rviz_plugin/CHANGELOG.rst +++ b/navmap_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package navmap_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Cleanup unused headers +* NavMap Goal Pose +* Occupancy works +* FREE_SPACE as white +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ From ec3512dd6a717ce6bda654dffce7c1606f8738e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 24 Nov 2025 11:54:47 +0100 Subject: [PATCH 12/29] 0.3.0 --- navmap_core/CHANGELOG.rst | 4 ++-- navmap_core/package.xml | 2 +- navmap_examples/CHANGELOG.rst | 4 ++-- navmap_examples/package.xml | 2 +- navmap_ros/CHANGELOG.rst | 4 ++-- navmap_ros/package.xml | 2 +- navmap_ros_interfaces/CHANGELOG.rst | 4 ++-- navmap_ros_interfaces/package.xml | 2 +- navmap_rviz_plugin/CHANGELOG.rst | 4 ++-- navmap_rviz_plugin/package.xml | 2 +- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/navmap_core/CHANGELOG.rst b/navmap_core/CHANGELOG.rst index a0c20e7..581c52b 100644 --- a/navmap_core/CHANGELOG.rst +++ b/navmap_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.0 (2025-11-24) +------------------ * Sppedup the navcel location * Cleanup unused headers * Fix potential linker error and warning diff --git a/navmap_core/package.xml b/navmap_core/package.xml index e47a17b..4f7b75d 100644 --- a/navmap_core/package.xml +++ b/navmap_core/package.xml @@ -2,7 +2,7 @@ navmap_core - 0.2.5 + 0.3.0 Core C++ library for NavMap. Francisco Martín Rico diff --git a/navmap_examples/CHANGELOG.rst b/navmap_examples/CHANGELOG.rst index 645d063..1d385ca 100644 --- a/navmap_examples/CHANGELOG.rst +++ b/navmap_examples/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.0 (2025-11-24) +------------------ * Cleanup unused headers * Merge branch 'jazzy' into rolling * Contributors: Francisco Martín Rico, Francisco Miguel Moreno diff --git a/navmap_examples/package.xml b/navmap_examples/package.xml index 7bd3095..b3f8597 100644 --- a/navmap_examples/package.xml +++ b/navmap_examples/package.xml @@ -2,7 +2,7 @@ navmap_examples - 0.2.5 + 0.3.0 Examples related to navmap_core y navmap_ros. Francisco Martín Rico diff --git a/navmap_ros/CHANGELOG.rst b/navmap_ros/CHANGELOG.rst index 9bf37fc..308fd6a 100644 --- a/navmap_ros/CHANGELOG.rst +++ b/navmap_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.0 (2025-11-24) +------------------ * Cleanup unused headers * Occupancy works * Add occupancy grid constants diff --git a/navmap_ros/package.xml b/navmap_ros/package.xml index 4f0ec59..a49c9ea 100644 --- a/navmap_ros/package.xml +++ b/navmap_ros/package.xml @@ -1,7 +1,7 @@ navmap_ros - 0.2.5 + 0.3.0 Conversions between navmap_core and ROS messages Francisco Martín Rico diff --git a/navmap_ros_interfaces/CHANGELOG.rst b/navmap_ros_interfaces/CHANGELOG.rst index 613efae..b4f4ae2 100644 --- a/navmap_ros_interfaces/CHANGELOG.rst +++ b/navmap_ros_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_ros_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.0 (2025-11-24) +------------------ * Merge branch 'jazzy' into rolling * Contributors: Francisco Martín Rico diff --git a/navmap_ros_interfaces/package.xml b/navmap_ros_interfaces/package.xml index 209368a..1683d88 100644 --- a/navmap_ros_interfaces/package.xml +++ b/navmap_ros_interfaces/package.xml @@ -1,7 +1,7 @@ navmap_ros_interfaces - 0.2.5 + 0.3.0 ROS 2 interfaces for NavMap (messages for visualization and layers) Francisco Martín Rico Apache License, Version 2.0 diff --git a/navmap_rviz_plugin/CHANGELOG.rst b/navmap_rviz_plugin/CHANGELOG.rst index 62c8c6c..48a1622 100644 --- a/navmap_rviz_plugin/CHANGELOG.rst +++ b/navmap_rviz_plugin/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.3.0 (2025-11-24) +------------------ * Cleanup unused headers * NavMap Goal Pose * Occupancy works diff --git a/navmap_rviz_plugin/package.xml b/navmap_rviz_plugin/package.xml index 0f3ad3f..e374b11 100644 --- a/navmap_rviz_plugin/package.xml +++ b/navmap_rviz_plugin/package.xml @@ -3,7 +3,7 @@ navmap_rviz_plugin - 0.2.5 + 0.3.0 RViz2 display plugin for NavMap surfaces and layers. Francisco Martín Rico 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 13/29] 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 14/29] 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 15/29] 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 16/29] 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 17/29] 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 18/29] 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() From 4854c476ff268420e61a516d93f4c423a20378b3 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Wed, 29 Apr 2026 11:24:49 +0200 Subject: [PATCH 19/29] Fix test compilation error with rosidl::Buffer --- navmap_ros/tests/test_navmap_io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/navmap_ros/tests/test_navmap_io.cpp b/navmap_ros/tests/test_navmap_io.cpp index 8168125..f0b7ace 100644 --- a/navmap_ros/tests/test_navmap_io.cpp +++ b/navmap_ros/tests/test_navmap_io.cpp @@ -61,7 +61,7 @@ void fill_basic_header(navmap_ros_interfaces::msg::NavMap & msg, const std::stri // --- helpers: semantic comparison for messages --- template -static void ExpectVecEq(const std::vector & a, const std::vector & b, const char * what) +static void ExpectVecEq(const T & a, const T & b, const char * what) { ASSERT_EQ(a.size(), b.size()) << what << " size mismatch"; for (size_t i = 0; i < a.size(); ++i) { From 7c3e2ef1b6fb8c99f5845ded7d7dc1df43d34d6d Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Wed, 29 Apr 2026 12:00:00 +0200 Subject: [PATCH 20/29] Bump CI to Ubuntu 26 --- .github/workflows/rolling.yaml | 4 ++-- .github/workflows/rolling_cron.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 8d5ec03..c5d8791 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -9,9 +9,9 @@ on: - rolling jobs: build-and-test: - runs-on: ubuntu-24.04 + runs-on: ubuntu-26.04 container: - image: ubuntu:noble + image: ubuntu:resolute steps: - uses: actions/checkout@v4 with: diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml index 4134591..f3667ab 100644 --- a/.github/workflows/rolling_cron.yaml +++ b/.github/workflows/rolling_cron.yaml @@ -5,9 +5,9 @@ on: - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ubuntu-24.04 + runs-on: ubuntu-26.04 container: - image: ubuntu:noble + image: ubuntu:resolute steps: - uses: actions/checkout@v4 with: From 2e2451d8e356e53b5894c7db0ff44d373064fc16 Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Sun, 21 Jun 2026 13:13:37 +0200 Subject: [PATCH 21/29] Fix rolling CI --- .github/workflows/rolling.yaml | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index c5d8791..dcacdc1 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -11,15 +11,13 @@ jobs: build-and-test: runs-on: ubuntu-26.04 container: - image: ubuntu:resolute + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-rolling-ros-base:master steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v6 with: ref: rolling - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - name: build and test - uses: ros-tooling/action-ros-ci@0.4.5 + uses: ros-tooling/action-ros-ci@0.4.8 with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: rolling From 2d155b5bdf0dc42a0616849785fbcfe9f3aece3e Mon Sep 17 00:00:00 2001 From: Francisco Miguel Moreno Date: Sun, 21 Jun 2026 13:32:46 +0200 Subject: [PATCH 22/29] CI: Merge rolling cron job into main rolling job --- .github/workflows/rolling.yaml | 3 +++ .github/workflows/rolling_cron.yaml | 38 ----------------------------- 2 files changed, 3 insertions(+), 38 deletions(-) delete mode 100644 .github/workflows/rolling_cron.yaml diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index dcacdc1..72473cd 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -7,6 +7,9 @@ on: push: branches: - rolling + schedule: + - cron: '0 0 * * 6' + workflow_dispatch: jobs: build-and-test: runs-on: ubuntu-26.04 diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml deleted file mode 100644 index f3667ab..0000000 --- a/.github/workflows/rolling_cron.yaml +++ /dev/null @@ -1,38 +0,0 @@ -name: rolling - -on: - schedule: - - cron: '0 0 * * 6' -jobs: - build-and-test: - runs-on: ubuntu-26.04 - container: - image: ubuntu:resolute - steps: - - uses: actions/checkout@v4 - with: - ref: rolling - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - - 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 2b4abc762fd3d1da0b4b2773a0ba91cfc2a5a041 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 23 Jul 2026 19:47:44 +0200 Subject: [PATCH 23/29] Added CI for Lyrical MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/lyrical.yaml | 42 ++++++++++++++++++++++++++++++++++ README.md | 1 + 2 files changed, 43 insertions(+) create mode 100644 .github/workflows/lyrical.yaml diff --git a/.github/workflows/lyrical.yaml b/.github/workflows/lyrical.yaml new file mode 100644 index 0000000..72473cd --- /dev/null +++ b/.github/workflows/lyrical.yaml @@ -0,0 +1,42 @@ +name: rolling + +on: + pull_request: + branches: + - rolling + push: + branches: + - rolling + schedule: + - cron: '0 0 * * 6' + workflow_dispatch: +jobs: + build-and-test: + runs-on: ubuntu-26.04 + container: + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-rolling-ros-base:master + steps: + - uses: actions/checkout@v6 + with: + ref: rolling + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.8 + with: + package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin + target-ros2-distro: 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 diff --git a/README.md b/README.md index 46105df..30bd70a 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # NavMap [![Doxygen Deployment](https://github.com/EasyNavigation/NavMap/actions/workflows/doxygen-doc.yml/badge.svg)](https://github.com/EasyNavigation/NavMap/actions/workflows/doxygen-doc.yml) [![rolling](https://github.com/EasyNavigation/NavMap/actions/workflows/rolling.yaml/badge.svg?branch=rolling)](https://github.com/EasyNavigation/NavMap/actions/workflows/rolling.yaml) +[![lyrical](https://github.com/EasyNavigation/NavMap/actions/workflows/lyrical.yaml/badge.svg?branch=lyrical)](https://github.com/EasyNavigation/NavMap/actions/workflows/lyrical.yaml) [![kilted](https://github.com/EasyNavigation/NavMap/actions/workflows/kilted.yaml/badge.svg?branch=kilted)](https://github.com/EasyNavigation/NavMap/actions/workflows/kilted.yaml) [![jazzy](https://github.com/EasyNavigation/NavMap/actions/workflows/jazzy.yaml/badge.svg?branch=jazzy)](https://github.com/EasyNavigation/NavMap/actions/workflows/jazzy.yaml) [![humble](https://github.com/EasyNavigation/NavMap/actions/workflows/humble.yaml/badge.svg?branch=humble)](https://github.com/EasyNavigation/NavMap/actions/workflows/humble.yaml) From 68fd4ebdbfb23ef2e029bb8730d4f186577c3087 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 23 Jul 2026 19:56:27 +0200 Subject: [PATCH 24/29] Added CI for Lyrical MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/lyrical.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/lyrical.yaml b/.github/workflows/lyrical.yaml index 72473cd..60f9109 100644 --- a/.github/workflows/lyrical.yaml +++ b/.github/workflows/lyrical.yaml @@ -1,12 +1,12 @@ -name: rolling +name: lyrical on: pull_request: branches: - - rolling + - lyrical push: branches: - - rolling + - lyrical schedule: - cron: '0 0 * * 6' workflow_dispatch: @@ -14,16 +14,16 @@ jobs: build-and-test: runs-on: ubuntu-26.04 container: - image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-rolling-ros-base:master + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-lyrical-ros-base:master steps: - uses: actions/checkout@v6 with: - ref: rolling + ref: lyrical - name: build and test uses: ros-tooling/action-ros-ci@0.4.8 with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin - target-ros2-distro: rolling + target-ros2-distro: lyrical colcon-defaults: | { "test": { From afe715940096d3edd88858c62b1e7bca0c850808 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 25 Jul 2026 09:51:29 +0200 Subject: [PATCH 25/29] Update version MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_core/CHANGELOG.rst | 9 +++++++++ navmap_core/package.xml | 2 +- navmap_examples/CHANGELOG.rst | 6 ++++++ navmap_examples/package.xml | 2 +- navmap_ros/CHANGELOG.rst | 14 ++++++++++++++ navmap_ros/package.xml | 2 +- navmap_ros_interfaces/CHANGELOG.rst | 6 ++++++ navmap_ros_interfaces/package.xml | 2 +- navmap_rviz_plugin/CHANGELOG.rst | 9 +++++++++ navmap_rviz_plugin/package.xml | 2 +- 10 files changed, 49 insertions(+), 5 deletions(-) diff --git a/navmap_core/CHANGELOG.rst b/navmap_core/CHANGELOG.rst index ad00f61..9d17729 100644 --- a/navmap_core/CHANGELOG.rst +++ b/navmap_core/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package navmap_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2025-11-24) +------------------ +* Speedup the navcel location +* Sppedup the navcel location +* Cleanup unused headers +* Fix potential linker error and warning +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_core/package.xml b/navmap_core/package.xml index e47a17b..d660924 100644 --- a/navmap_core/package.xml +++ b/navmap_core/package.xml @@ -2,7 +2,7 @@ navmap_core - 0.2.5 + 0.4.0 Core C++ library for NavMap. Francisco Martín Rico diff --git a/navmap_examples/CHANGELOG.rst b/navmap_examples/CHANGELOG.rst index e37829d..d62042f 100644 --- a/navmap_examples/CHANGELOG.rst +++ b/navmap_examples/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package navmap_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2025-11-24) +------------------ +* Cleanup unused headers +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_examples/package.xml b/navmap_examples/package.xml index 7bd3095..bd1d0d7 100644 --- a/navmap_examples/package.xml +++ b/navmap_examples/package.xml @@ -2,7 +2,7 @@ navmap_examples - 0.2.5 + 0.4.0 Examples related to navmap_core y navmap_ros. Francisco Martín Rico diff --git a/navmap_ros/CHANGELOG.rst b/navmap_ros/CHANGELOG.rst index 676c47b..38e0ced 100644 --- a/navmap_ros/CHANGELOG.rst +++ b/navmap_ros/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package navmap_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2025-11-24) +------------------ +* Cleanup unused headers +* Occupancy works +* Add occupancy grid constants +* Fix surface creation from points +* Remove unused field +* Remove some comments +* Final working version +* Acelerated respecting floors +* Working slow with many points +* Initial working version +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ * Fix pcl_conversions build diff --git a/navmap_ros/package.xml b/navmap_ros/package.xml index 4f0ec59..5fd87b4 100644 --- a/navmap_ros/package.xml +++ b/navmap_ros/package.xml @@ -1,7 +1,7 @@ navmap_ros - 0.2.5 + 0.4.0 Conversions between navmap_core and ROS messages Francisco Martín Rico diff --git a/navmap_ros_interfaces/CHANGELOG.rst b/navmap_ros_interfaces/CHANGELOG.rst index cda5d5f..bceb6c9 100644 --- a/navmap_ros_interfaces/CHANGELOG.rst +++ b/navmap_ros_interfaces/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package navmap_ros_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2025-11-24) +------------------ +* Merge branch 'rolling' into kilted +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_ros_interfaces/package.xml b/navmap_ros_interfaces/package.xml index 209368a..342cb68 100644 --- a/navmap_ros_interfaces/package.xml +++ b/navmap_ros_interfaces/package.xml @@ -1,7 +1,7 @@ navmap_ros_interfaces - 0.2.5 + 0.4.0 ROS 2 interfaces for NavMap (messages for visualization and layers) Francisco Martín Rico Apache License, Version 2.0 diff --git a/navmap_rviz_plugin/CHANGELOG.rst b/navmap_rviz_plugin/CHANGELOG.rst index 8b5da4d..10783af 100644 --- a/navmap_rviz_plugin/CHANGELOG.rst +++ b/navmap_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package navmap_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.0 (2025-11-24) +------------------ +* Cleanup unused headers +* NavMap Goal Pose +* Occupancy works +* FREE_SPACE as white +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_rviz_plugin/package.xml b/navmap_rviz_plugin/package.xml index 0f3ad3f..d97d180 100644 --- a/navmap_rviz_plugin/package.xml +++ b/navmap_rviz_plugin/package.xml @@ -3,7 +3,7 @@ navmap_rviz_plugin - 0.2.5 + 0.4.0 RViz2 display plugin for NavMap surfaces and layers. Francisco Martín Rico From eae9437dc8fbfdad7fbbba87b1b76b630e63cbb0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 25 Jul 2026 09:55:08 +0200 Subject: [PATCH 26/29] Update Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_core/CHANGELOG.rst | 10 +++++++++- navmap_examples/CHANGELOG.rst | 5 +++++ navmap_ros/CHANGELOG.rst | 12 ++++++++++++ navmap_ros_interfaces/CHANGELOG.rst | 5 +++++ navmap_rviz_plugin/CHANGELOG.rst | 10 ++++++++++ 5 files changed, 41 insertions(+), 1 deletion(-) diff --git a/navmap_core/CHANGELOG.rst b/navmap_core/CHANGELOG.rst index 9d17729..affa585 100644 --- a/navmap_core/CHANGELOG.rst +++ b/navmap_core/CHANGELOG.rst @@ -2,10 +2,18 @@ Changelog for package navmap_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update version +* Speedup the navcel location +* Cleanup unused headers +* Fix potential linker error and warning +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.4.0 (2025-11-24) ------------------ * Speedup the navcel location -* Sppedup the navcel location * Cleanup unused headers * Fix potential linker error and warning * Merge branch 'jazzy' into rolling diff --git a/navmap_examples/CHANGELOG.rst b/navmap_examples/CHANGELOG.rst index d62042f..e36c3a9 100644 --- a/navmap_examples/CHANGELOG.rst +++ b/navmap_examples/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package navmap_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.4.0 (2025-11-24) ------------------ * Cleanup unused headers diff --git a/navmap_ros/CHANGELOG.rst b/navmap_ros/CHANGELOG.rst index 38e0ced..194cf68 100644 --- a/navmap_ros/CHANGELOG.rst +++ b/navmap_ros/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package navmap_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix test compilation error with rosidl::Buffer +* PCL private linkage: avoid Qt5/6 conflicts +* Fix doc in header +* Add headers in conversions +* Cleanup unused headers +* Add occupancy grid constants +* Acelerated respecting floors +* Working slow with many points +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, estherag + 0.4.0 (2025-11-24) ------------------ * Cleanup unused headers diff --git a/navmap_ros_interfaces/CHANGELOG.rst b/navmap_ros_interfaces/CHANGELOG.rst index bceb6c9..1de3e6d 100644 --- a/navmap_ros_interfaces/CHANGELOG.rst +++ b/navmap_ros_interfaces/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package navmap_ros_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico + 0.4.0 (2025-11-24) ------------------ * Merge branch 'rolling' into kilted diff --git a/navmap_rviz_plugin/CHANGELOG.rst b/navmap_rviz_plugin/CHANGELOG.rst index 10783af..85ea7ea 100644 --- a/navmap_rviz_plugin/CHANGELOG.rst +++ b/navmap_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package navmap_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fully commit to Qt6 only and cleanup CMake +* Set Qt6 references and moc to proper plugin export +* PCL private linkage: avoid Qt5/6 conflicts +* NavMap Goal Pose +* FREE_SPACE as white +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, estherag + 0.4.0 (2025-11-24) ------------------ * Cleanup unused headers From 01c42c35cf06a2eed7b75a55d0970eaf0bbcf78c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 25 Jul 2026 09:55:34 +0200 Subject: [PATCH 27/29] 0.5.0 --- navmap_core/CHANGELOG.rst | 4 ++-- navmap_core/package.xml | 2 +- navmap_examples/CHANGELOG.rst | 4 ++-- navmap_examples/package.xml | 2 +- navmap_ros/CHANGELOG.rst | 4 ++-- navmap_ros/package.xml | 2 +- navmap_ros_interfaces/CHANGELOG.rst | 4 ++-- navmap_ros_interfaces/package.xml | 2 +- navmap_rviz_plugin/CHANGELOG.rst | 4 ++-- navmap_rviz_plugin/package.xml | 2 +- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/navmap_core/CHANGELOG.rst b/navmap_core/CHANGELOG.rst index affa585..1c49cf0 100644 --- a/navmap_core/CHANGELOG.rst +++ b/navmap_core/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2026-07-25) +------------------ * Update version * Speedup the navcel location * Cleanup unused headers diff --git a/navmap_core/package.xml b/navmap_core/package.xml index d660924..19c1667 100644 --- a/navmap_core/package.xml +++ b/navmap_core/package.xml @@ -2,7 +2,7 @@ navmap_core - 0.4.0 + 0.5.0 Core C++ library for NavMap. Francisco Martín Rico diff --git a/navmap_examples/CHANGELOG.rst b/navmap_examples/CHANGELOG.rst index e36c3a9..0b55802 100644 --- a/navmap_examples/CHANGELOG.rst +++ b/navmap_examples/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2026-07-25) +------------------ * Merge branch 'jazzy' into rolling * Contributors: Francisco Martín Rico, Francisco Miguel Moreno diff --git a/navmap_examples/package.xml b/navmap_examples/package.xml index bd1d0d7..adbc164 100644 --- a/navmap_examples/package.xml +++ b/navmap_examples/package.xml @@ -2,7 +2,7 @@ navmap_examples - 0.4.0 + 0.5.0 Examples related to navmap_core y navmap_ros. Francisco Martín Rico diff --git a/navmap_ros/CHANGELOG.rst b/navmap_ros/CHANGELOG.rst index 194cf68..e3801d6 100644 --- a/navmap_ros/CHANGELOG.rst +++ b/navmap_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2026-07-25) +------------------ * Fix test compilation error with rosidl::Buffer * PCL private linkage: avoid Qt5/6 conflicts * Fix doc in header diff --git a/navmap_ros/package.xml b/navmap_ros/package.xml index 5fd87b4..791cd1e 100644 --- a/navmap_ros/package.xml +++ b/navmap_ros/package.xml @@ -1,7 +1,7 @@ navmap_ros - 0.4.0 + 0.5.0 Conversions between navmap_core and ROS messages Francisco Martín Rico diff --git a/navmap_ros_interfaces/CHANGELOG.rst b/navmap_ros_interfaces/CHANGELOG.rst index 1de3e6d..c47690d 100644 --- a/navmap_ros_interfaces/CHANGELOG.rst +++ b/navmap_ros_interfaces/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_ros_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2026-07-25) +------------------ * Merge branch 'jazzy' into rolling * Contributors: Francisco Martín Rico diff --git a/navmap_ros_interfaces/package.xml b/navmap_ros_interfaces/package.xml index 342cb68..4940da8 100644 --- a/navmap_ros_interfaces/package.xml +++ b/navmap_ros_interfaces/package.xml @@ -1,7 +1,7 @@ navmap_ros_interfaces - 0.4.0 + 0.5.0 ROS 2 interfaces for NavMap (messages for visualization and layers) Francisco Martín Rico Apache License, Version 2.0 diff --git a/navmap_rviz_plugin/CHANGELOG.rst b/navmap_rviz_plugin/CHANGELOG.rst index 85ea7ea..d97987f 100644 --- a/navmap_rviz_plugin/CHANGELOG.rst +++ b/navmap_rviz_plugin/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package navmap_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.5.0 (2026-07-25) +------------------ * Fully commit to Qt6 only and cleanup CMake * Set Qt6 references and moc to proper plugin export * PCL private linkage: avoid Qt5/6 conflicts diff --git a/navmap_rviz_plugin/package.xml b/navmap_rviz_plugin/package.xml index d97d180..1638776 100644 --- a/navmap_rviz_plugin/package.xml +++ b/navmap_rviz_plugin/package.xml @@ -3,7 +3,7 @@ navmap_rviz_plugin - 0.4.0 + 0.5.0 RViz2 display plugin for NavMap surfaces and layers. Francisco Martín Rico From 8c12c64df561de689238ee0f6b0c90a0c849714d Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 31 Jul 2026 20:44:51 +0200 Subject: [PATCH 28/29] Jazzy ports Signed-off-by: Juan S. Cely G. --- navmap_core/include/navmap_core/NavMap.hpp | 5 +- navmap_core/src/navmap_core/NavMap.cpp | 13 +-- navmap_examples/src/01_flat_plane.cpp | 4 +- navmap_examples/src/04_layers.cpp | 5 +- .../src/05_neighbors_and_centroids.cpp | 2 +- navmap_examples/src/06_area_marking.cpp | 15 ++-- navmap_examples/src/08_copy_and_assign.cpp | 2 +- navmap_examples/src_ros2/01_from_occgrid.cpp | 5 +- navmap_examples/src_ros2/02_to_occgrid.cpp | 8 +- navmap_examples/src_ros2/03_save_load.cpp | 22 ++--- navmap_ros/include/navmap_ros/conversions.hpp | 6 +- navmap_ros/src/navmap_ros/conversions.cpp | 85 +++++++++++-------- navmap_ros/src/slam_server_app.cpp | 26 +++--- navmap_ros/tests/test_navmap_io.cpp | 20 ++--- .../navmap_rviz_plugin/NavMapDisplay.hpp | 8 +- .../src/navmap_rviz_plugin/NavMapDisplay.cpp | 49 ++++++----- 16 files changed, 154 insertions(+), 121 deletions(-) diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 1322dfe..cb837a8 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -221,8 +221,9 @@ std::uint64_t LayerView::content_hash() const 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."); + 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; diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index a915bf3..627c8bd 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -648,12 +648,13 @@ bool NavMap::locate_navcel_core( // 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)) + if (locate_by_walking( + start, + p_world, + cid, + bary, + hit_pt, + opts.planar_eps)) { for (size_t s = 0; s < surfaces.size(); ++s) { const auto & surf = surfaces[s]; diff --git a/navmap_examples/src/01_flat_plane.cpp b/navmap_examples/src/01_flat_plane.cpp index 326d8a0..50a2404 100644 --- a/navmap_examples/src/01_flat_plane.cpp +++ b/navmap_examples/src/01_flat_plane.cpp @@ -50,7 +50,7 @@ int main() NavMap nm; make_flat_square(nm); auto occ = nm.layers.add_or_get("occupancy", nm.navcels.size(), LayerType::U8); - if(!occ) {cerr << "Cannot create 'occupancy'\n"; return 1;} + if (!occ) {cerr << "Cannot create 'occupancy'\n"; return 1;} (*occ)[0] = 0; (*occ)[1] = 254; Vector3f p(0.75f, 0.75f, 0.4f); @@ -58,7 +58,7 @@ int main() bool ok = nm.locate_navcel(p, sidx, cid, bary, &hit); cout << "locate=" << ok << " sidx=" << sidx << " cid=" << cid << " hit=(" << hit.x() << "," << hit.y() << "," << hit.z() << ")\n"; - if(ok) { + if (ok) { cout << "occ at cid: " << (int)nm.navcel_value(cid, *occ) << endl; } return 0; diff --git a/navmap_examples/src/04_layers.cpp b/navmap_examples/src/04_layers.cpp index efb2617..c376bc4 100644 --- a/navmap_examples/src/04_layers.cpp +++ b/navmap_examples/src/04_layers.cpp @@ -44,11 +44,12 @@ int main() nm.layer_set("cost", c0, 5.5f); auto names = nm.list_layers(); - cout << "Layers:"; for(auto & n:names) { + cout << "Layers:"; for (auto & n:names) { cout << " " << n; } cout << endl; - cout << "occ=" << (int)nm.layer_get("occ", c0, + cout << "occ=" << (int)nm.layer_get( + "occ", c0, 0) << ", cost=" << nm.layer_get("cost", c0, -1.0) << endl; return 0; } diff --git a/navmap_examples/src/05_neighbors_and_centroids.cpp b/navmap_examples/src/05_neighbors_and_centroids.cpp index 954c595..5d99476 100644 --- a/navmap_examples/src/05_neighbors_and_centroids.cpp +++ b/navmap_examples/src/05_neighbors_and_centroids.cpp @@ -49,7 +49,7 @@ int main() auto neigh = nm.navcel_neighbors(c0); cout << "centroid0=(" << cc0.x() << "," << cc0.y() << "," << cc0.z() << ")" << endl; cout << "centroid1=(" << cc1.x() << "," << cc1.y() << "," << cc1.z() << ")" << endl; - cout << "neighbors of c0:"; for(auto n:neigh) { + cout << "neighbors of c0:"; for (auto n:neigh) { cout << " " << n; } cout << endl; diff --git a/navmap_examples/src/06_area_marking.cpp b/navmap_examples/src/06_area_marking.cpp index 603bc95..bef573e 100644 --- a/navmap_examples/src/06_area_marking.cpp +++ b/navmap_examples/src/06_area_marking.cpp @@ -44,14 +44,17 @@ int main() nm.add_layer("obstacles", "occupancy obstacles", "%", 0); // Circular in the center radius 0.3 → marks both centroids - bool ok1 = nm.set_area(Vector3f(0.5f, 0.5f, 10.0f), (uint8_t)254, - "obstacles", navmap::AreaShape::CIRCULAR, 0.3f); + bool ok1 = nm.set_area( + Vector3f(0.5f, 0.5f, 10.0f), (uint8_t)254, + "obstacles", navmap::AreaShape::CIRCULAR, 0.3f); // Rectangular near (0.8,0.2) side 0.35 → mark one - bool ok2 = nm.set_area(Vector3f(0.80f, 0.20f, -5.0f), (uint8_t)200, - "obstacles", navmap::AreaShape::RECTANGULAR, 0.35f); + bool ok2 = nm.set_area( + Vector3f(0.80f, 0.20f, -5.0f), (uint8_t)200, + "obstacles", navmap::AreaShape::RECTANGULAR, 0.35f); cout << "set_area circle=" << ok1 << " rect=" << ok2 << endl; - cout << "c0=" << (int)nm.layer_get("obstacles", c0, - 0) << " c1=" << (int)nm.layer_get("obstacles", c1, 0) << endl; + cout << "c0=" << (int)nm.layer_get( + "obstacles", c0, + 0) << " c1=" << (int)nm.layer_get("obstacles", c1, 0) << endl; return 0; } diff --git a/navmap_examples/src/08_copy_and_assign.cpp b/navmap_examples/src/08_copy_and_assign.cpp index a11affb..691d1de 100644 --- a/navmap_examples/src/08_copy_and_assign.cpp +++ b/navmap_examples/src/08_copy_and_assign.cpp @@ -50,7 +50,7 @@ int main() dst = src; auto names_after = dst.list_layers(); std::cout << "assign equal geom ok; layers after:"; - for(auto & n:names_after) { + for (auto & n:names_after) { std::cout << " " << n; } std::cout << "\n"; diff --git a/navmap_examples/src_ros2/01_from_occgrid.cpp b/navmap_examples/src_ros2/01_from_occgrid.cpp index eda2662..1a64de1 100644 --- a/navmap_examples/src_ros2/01_from_occgrid.cpp +++ b/navmap_examples/src_ros2/01_from_occgrid.cpp @@ -21,7 +21,8 @@ using std::placeholders::_1; -class GridToNavMapNode : public rclcpp::Node { +class GridToNavMapNode : public rclcpp::Node +{ public: GridToNavMapNode() : Node("navmap_from_occgrid") @@ -36,7 +37,7 @@ class GridToNavMapNode : public rclcpp::Node { navmap::NavMap nm = navmap_ros::from_occupancy_grid(*msg); size_t sidx{}; navmap::NavCelId cid{}; Eigen::Vector3f bary, hit; - if(nm.locate_navcel(Eigen::Vector3f(0.5f, 0.5f, 0.5f), sidx, cid, bary, &hit)) { + if (nm.locate_navcel(Eigen::Vector3f(0.5f, 0.5f, 0.5f), sidx, cid, bary, &hit)) { RCLCPP_INFO(this->get_logger(), "hit on surface %zu cell %u", sidx, (unsigned)cid); } } diff --git a/navmap_examples/src_ros2/02_to_occgrid.cpp b/navmap_examples/src_ros2/02_to_occgrid.cpp index 71d68c9..c72fc44 100644 --- a/navmap_examples/src_ros2/02_to_occgrid.cpp +++ b/navmap_examples/src_ros2/02_to_occgrid.cpp @@ -19,13 +19,15 @@ #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" -class NavMapToGridNode : public rclcpp::Node { +class NavMapToGridNode : public rclcpp::Node +{ public: NavMapToGridNode() : Node("navmap_to_occgrid") { pub_ = this->create_publisher("navmap_grid", 10); - timer_ = this->create_wall_timer(std::chrono::seconds(1), + timer_ = this->create_wall_timer( + std::chrono::seconds(1), std::bind(&NavMapToGridNode::tick, this)); } @@ -34,7 +36,7 @@ class NavMapToGridNode : public rclcpp::Node { { static bool init = false; static navmap::NavMap nm; - if(!init) { + if (!init) { auto v0 = nm.add_vertex({0, 0, 0}); auto v1 = nm.add_vertex({1, 0, 0}); auto v2 = nm.add_vertex({1, 1, 0}); diff --git a/navmap_examples/src_ros2/03_save_load.cpp b/navmap_examples/src_ros2/03_save_load.cpp index fa43e54..b87587a 100644 --- a/navmap_examples/src_ros2/03_save_load.cpp +++ b/navmap_examples/src_ros2/03_save_load.cpp @@ -27,37 +27,37 @@ static void save_json(const navmap::NavMap & nm, const std::string & path) json j; j["x"] = nm.positions.x; j["y"] = nm.positions.y; j["z"] = nm.positions.z; j["tris"] = json::array(); - for(const auto & c: nm.navcels) { + for (const auto & c: nm.navcels) { j["tris"].push_back({c.v[0], c.v[1], c.v[2]}); } // Solo capa "occupancy" si existe auto occ_any = nm.layers.get("occupancy"); - if(occ_any) { + if (occ_any) { auto occ = std::dynamic_pointer_cast>(occ_any); - if(occ) {j["occupancy"] = occ->data();} + if (occ) {j["occupancy"] = occ->data();} } std::ofstream ofs(path); ofs << j.dump(2); } static bool load_json(navmap::NavMap & nm, const std::string & path) { - std::ifstream ifs(path); if(!ifs) {return false;} + std::ifstream ifs(path); if (!ifs) {return false;} json j; ifs >> j; nm.positions.x = j["x"].get>(); nm.positions.y = j["y"].get>(); nm.positions.z = j["z"].get>(); nm.navcels.resize(j["tris"].size()); - for(size_t i = 0; i < nm.navcels.size(); ++i) { + for (size_t i = 0; i < nm.navcels.size(); ++i) { auto t = j["tris"][i]; nm.navcels[i].v[0] = t[0]; nm.navcels[i].v[1] = t[1]; nm.navcels[i].v[2] = t[2]; } nm.surfaces.clear(); auto s = nm.create_surface("map"); - for(size_t i = 0; i < nm.navcels.size(); ++i) { + for (size_t i = 0; i < nm.navcels.size(); ++i) { nm.add_navcel_to_surface(s, (navmap::NavCelId)i); } nm.rebuild_geometry_accels(); - if(j.contains("occupancy")) { + if (j.contains("occupancy")) { auto occ = nm.add_layer("occupancy", "occ", "%", 0); auto & v = occ->mutable_data(); v = j["occupancy"].get>(); @@ -65,7 +65,8 @@ static bool load_json(navmap::NavMap & nm, const std::string & path) return true; } -class SaveLoadNode : public rclcpp::Node { +class SaveLoadNode : public rclcpp::Node +{ public: SaveLoadNode() : Node("navmap_save_load") @@ -86,8 +87,9 @@ class SaveLoadNode : public rclcpp::Node { navmap::NavMap re; (void)load_json(re, "/tmp/navmap.json"); - RCLCPP_INFO(get_logger(), "Loaded back: vertices=%zu tris=%zu", - re.positions.size(), re.navcels.size()); + RCLCPP_INFO( + get_logger(), "Loaded back: vertices=%zu tris=%zu", + re.positions.size(), re.navcels.size()); } }; diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 1d5ef88..af054b2 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -218,13 +218,13 @@ struct BuildParams float neighbor_radius = 2.0f; // search radius /** @brief Alternative to radius: number of nearest neighbors (k-NN). */ - int k_neighbors = 20; // k-NN alternative to radius + int k_neighbors = 20; // k-NN alternative to radius /** @brief Minimum triangle area (square meters) to reject degenerate faces. */ float min_area = 1e-6f; // minimum triangle area to avoid degenerates /** @brief If true, use radius-based neighborhoods; otherwise use k-NN. */ - bool use_radius = true; + bool use_radius = true; /** @brief Minimum interior angle (degrees) to avoid sliver triangles. */ float min_angle_deg = 20.0f; // minimum interior angle (deg) to avoid sliver triangles @@ -256,7 +256,7 @@ struct BuildParams * @throw std::runtime_error If meshing fails due to inconsistent parameters or empty input. */ navmap::NavMap from_points( - const pcl::PointCloud & input_points, + const pcl::PointCloud & input_points, navmap_ros_interfaces::msg::NavMap & out_msg, BuildParams params); diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 276e1dd..daf3614 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -167,8 +167,9 @@ navmap::NavMap from_msg(const NavMap & msg) nm.surfaces.resize(msg.surfaces.size()); for (size_t i = 0; i < msg.surfaces.size(); ++i) { nm.surfaces[i].frame_id = msg.surfaces[i].frame_id; - nm.surfaces[i].navcels.assign(msg.surfaces[i].navcels.begin(), - msg.surfaces[i].navcels.end()); + nm.surfaces[i].navcels.assign( + msg.surfaces[i].navcels.begin(), + msg.surfaces[i].navcels.end()); } // Fallback: create a single surface if none provided and triangles exist. @@ -247,7 +248,7 @@ void from_msg( { switch (msg.type) { case navmap_ros_interfaces::msg::NavMapLayer::U8: { - auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", uint8_t{}); + auto dst = nm.add_layer(msg.name, /*desc*/ "", /*unit*/ "", uint8_t{}); if (dst->data().size() != msg.data_u8.size()) { dst->data().resize(msg.data_u8.size()); } @@ -255,7 +256,7 @@ void from_msg( break; } case navmap_ros_interfaces::msg::NavMapLayer::F32: { - auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", 0.0f); + auto dst = nm.add_layer(msg.name, /*desc*/ "", /*unit*/ "", 0.0f); if (dst->data().size() != msg.data_f32.size()) { dst->data().resize(msg.data_f32.size()); } @@ -263,7 +264,7 @@ void from_msg( break; } case navmap_ros_interfaces::msg::NavMapLayer::F64: { - auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", 0.0); + auto dst = nm.add_layer(msg.name, /*desc*/ "", /*unit*/ "", 0.0); if (dst->data().size() != msg.data_f64.size()) { dst->data().resize(msg.data_f64.size()); } @@ -271,8 +272,9 @@ void from_msg( break; } default: - throw std::runtime_error("from_msg(NavMapLayer): unsupported type value " + - std::to_string(msg.type)); + throw std::runtime_error( + "from_msg(NavMapLayer): unsupported type value " + + std::to_string(msg.type)); } } @@ -436,8 +438,9 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) Eigen::Vector3f closest; float sq = 0.0f; - if (nm.closest_navcel({cx, cy, static_cast(g.info.origin.position.z)}, - sidx, cid, closest, sq)) + if (nm.closest_navcel( + {cx, cy, static_cast(g.info.origin.position.z)}, + sidx, cid, closest, sq)) { const uint8_t u8 = (*occ)[cid]; g.data[idx_cell(i, j)] = u8_to_occ(u8); @@ -583,7 +586,7 @@ struct TriHasher std::size_t operator()(const TriKey & t) const noexcept { std::size_t h = 1469598103934665603ull; - auto mix = [&](int k){ + auto mix = [&](int k) { h ^= static_cast(k) + 0x9e3779b97f4a7c15ull + (h << 6) + (h >> 2); }; mix(t.a); mix(t.b); mix(t.c); @@ -696,15 +699,16 @@ downsample_voxelize_topZ_layered( auto & idxs = kv.second; if (idxs.empty()) {continue;} - std::sort(idxs.begin(), idxs.end(), - [&](int a, int b){return input_points[a].z < input_points[b].z;}); + std::sort( + idxs.begin(), idxs.end(), + [&](int a, int b) {return input_points[a].z < input_points[b].z;}); double sum_x = 0.0, sum_y = 0.0; - float z_max = -std::numeric_limits::infinity(); - int count = 0; - float last_z = input_points[idxs.front()].z; + float z_max = -std::numeric_limits::infinity(); + int count = 0; + float last_z = input_points[idxs.front()].z; - auto flush_cluster = [&](){ + auto flush_cluster = [&]() { if (count <= 0) {return;} const float cx = static_cast(sum_x / count); const float cy = static_cast(sum_y / count); @@ -817,9 +821,10 @@ static void keep_top_surfaces_by_size(navmap_ros_interfaces::msg::NavMap & msg, std::vector ids(msg.surfaces.size()); std::iota(ids.begin(), ids.end(), 0); - std::sort(ids.begin(), ids.end(), [&](size_t a, size_t b){ + std::sort( + ids.begin(), ids.end(), [&](size_t a, size_t b) { return msg.surfaces[a].navcels.size() > msg.surfaces[b].navcels.size(); - }); + }); std::vector kept; kept.reserve(static_cast(max_surfaces)); @@ -927,8 +932,9 @@ static void rebuild_surfaces_by_connectivity(navmap_ros_interfaces::msg::NavMap for (auto & kv : comp) { comps.push_back(std::move(kv.second)); } - std::sort(comps.begin(), comps.end(), - [](const auto & A, const auto & B){return A.size() > B.size();}); + std::sort( + comps.begin(), comps.end(), + [](const auto & A, const auto & B) {return A.size() > B.size();}); const std::string fid = msg.header.frame_id; std::vector out; @@ -979,8 +985,9 @@ navmap::NavMap from_points( // Seeds in ascending Z std::vector order(N); std::iota(order.begin(), order.end(), 0); - std::sort(order.begin(), order.end(), - [&](int a, int b){return cloud[a].z < cloud[b].z;}); + std::sort( + order.begin(), order.end(), + [&](int a, int b) {return cloud[a].z < cloud[b].z;}); // Global state std::unordered_set tri_set_global; @@ -1007,11 +1014,11 @@ navmap::NavMap from_points( } }; - auto dist3f = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B){ + auto dist3f = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B) { const float dx = A.x - B.x, dy = A.y - B.y, dz = A.z - B.z; return std::sqrt(dx * dx + dy * dy + dz * dz); }; - auto distXY = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B){ + auto distXY = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B) { const float dx = A.x - B.x, dy = A.y - B.y; return std::sqrt(dx * dx + dy * dy); }; @@ -1088,18 +1095,22 @@ navmap::NavMap from_points( // Quick filters if (P.max_edge_len > 0.0f) { - neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), - [&](int j){ - const auto & Q = cloud[j]; if (!pcl::isFinite(Q)) { - return true; - } - return dist3f(cloud[seed_idx], Q) > P.max_edge_len; - }), + neigh_seed.erase( + std::remove_if( + neigh_seed.begin(), neigh_seed.end(), + [&](int j) { + const auto & Q = cloud[j]; if (!pcl::isFinite(Q)) { + return true; + } + return dist3f(cloud[seed_idx], Q) > P.max_edge_len; + }), neigh_seed.end()); } { - neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), - [&](int j){return std::fabs(cloud[j].z - cloud[seed_idx].z) > z_window_seed;}), + neigh_seed.erase( + std::remove_if( + neigh_seed.begin(), neigh_seed.end(), + [&](int j) {return std::fabs(cloud[j].z - cloud[seed_idx].z) > z_window_seed;}), neigh_seed.end()); } @@ -1111,11 +1122,12 @@ navmap::NavMap from_points( // Angular sort in XY { const auto & Cc = cloud[seed_idx]; - std::sort(neigh_seed.begin(), neigh_seed.end(), [&](int a, int b){ + std::sort( + neigh_seed.begin(), neigh_seed.end(), [&](int a, int b) { const float ax = cloud[a].x - Cc.x, ay = cloud[a].y - Cc.y; const float bx = cloud[b].x - Cc.x, by = cloud[b].y - Cc.y; return std::atan2(ay, ax) < std::atan2(by, bx); - }); + }); } const size_t tri_off = triangles.size(); @@ -1151,7 +1163,8 @@ navmap::NavMap from_points( const int k = neigh_seed.front(); bool dup = false; if (precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) { - if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set_global, edge_set_global, + if (try_add_triangle( + seed_idx, j, k, cloud, P, tri_set_global, edge_set_global, triangles)) { ++comp_fan_accept; diff --git a/navmap_ros/src/slam_server_app.cpp b/navmap_ros/src/slam_server_app.cpp index ed28550..803e633 100644 --- a/navmap_ros/src/slam_server_app.cpp +++ b/navmap_ros/src/slam_server_app.cpp @@ -32,7 +32,8 @@ class SLAMServerNode : public rclcpp::Node SLAMServerNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("slam_server_node", options) { - navmap_pub_ = create_publisher("navmap", + navmap_pub_ = create_publisher( + "navmap", rclcpp::QoS(1).transient_local().reliable()); incoming_occ_map_sub_ = create_subscription( @@ -64,17 +65,18 @@ class SLAMServerNode : public rclcpp::Node navmap_pub_->publish(navmap_msg_); }); - savemap_srv_ = create_service("savemap", - [this]( - const std::shared_ptr request, - std::shared_ptr response) - { - (void)request; - (void)response; - RCLCPP_INFO(get_logger(), "Saving NavMap from /tmp/map.navmap"); - - navmap_ros::io::save_to_file(navmap_, "/tmp/map.navmap"); - }); + savemap_srv_ = create_service( + "savemap", + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + (void)request; + (void)response; + RCLCPP_INFO(get_logger(), "Saving NavMap from /tmp/map.navmap"); + + navmap_ros::io::save_to_file(navmap_, "/tmp/map.navmap"); + }); } private: diff --git a/navmap_ros/tests/test_navmap_io.cpp b/navmap_ros/tests/test_navmap_io.cpp index c6674cb..3756fc0 100644 --- a/navmap_ros/tests/test_navmap_io.cpp +++ b/navmap_ros/tests/test_navmap_io.cpp @@ -290,24 +290,24 @@ TEST(NavMapIoCore, RoundtripViaCoreAndMsgCompare) msg.layers = {u8, f32}; // msg -> core -navmap::NavMap core = navmap_ros::from_msg(msg); + navmap::NavMap core = navmap_ros::from_msg(msg); // save(core) -> load(core) -std::string path = (std::filesystem::temp_directory_path() / + std::string path = (std::filesystem::temp_directory_path() / ("core_roundtrip_" + std::to_string(::getpid()) + ".navmap")).string(); -std::error_code ec; -ASSERT_TRUE(navmap_ros::io::save_to_file(core, path, {}, &ec)) << ec.message(); + std::error_code ec; + ASSERT_TRUE(navmap_ros::io::save_to_file(core, path, {}, &ec)) << ec.message(); -navmap::NavMap core_loaded; -ASSERT_TRUE(navmap_ros::io::load_from_file(path, core_loaded, &ec)) << ec.message(); + 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); + auto msg_from_core = navmap_ros::to_msg(core); + auto msg_from_core_loaded = navmap_ros::to_msg(core_loaded); // Semantic comparison (order and FP tolerant) -ExpectNavMapMsgEqualSemantic(msg_from_core, msg_from_core_loaded); + ExpectNavMapMsgEqualSemantic(msg_from_core, msg_from_core_loaded); -std::filesystem::remove(path); + std::filesystem::remove(path); 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 1e62d8f..24b9e5a 100644 --- a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp @@ -56,9 +56,9 @@ #define NAVMAP_RVIZ_PLUGIN_PUBLIC_TYPE NAVMAP_RVIZ_PLUGIN_PUBLIC #define NAVMAP_RVIZ_PLUGIN_LOCAL #else - #define NAVMAP_RVIZ_PLUGIN_PUBLIC __attribute__ ((visibility ("default"))) + #define NAVMAP_RVIZ_PLUGIN_PUBLIC __attribute__ ((visibility("default"))) #define NAVMAP_RVIZ_PLUGIN_PUBLIC_TYPE - #define NAVMAP_RVIZ_PLUGIN_LOCAL __attribute__ ((visibility ("hidden"))) + #define NAVMAP_RVIZ_PLUGIN_LOCAL __attribute__ ((visibility("hidden"))) #endif // Forward declarations to avoid hard coupling here @@ -146,8 +146,8 @@ private Q_SLOTS: // ---- Status counters ---- std::uint64_t navmap_msg_count_{0}; std::uint64_t layer_update_count_{0}; - rclcpp::Time last_navmap_stamp_; - rclcpp::Time last_layer_stamp_; + rclcpp::Time last_navmap_stamp_; + rclcpp::Time last_layer_stamp_; // ---- Data state ---- NavMapMsg::SharedPtr last_msg_; diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index ad64a76..0085beb 100644 --- a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp @@ -40,7 +40,7 @@ inline void hsv2rgb(float H, float S, float V, float & R, float & G, float & B) const float m = V - C; float r1 = 0.f, g1 = 0.f, b1 = 0.f; - if (H < 60.f) {r1 = C; g1 = X; b1 = 0.f;} else if (H < 120.f) { + if (H < 60.f) {r1 = C; g1 = X; b1 = 0.f;} else if (H < 120.f) { r1 = X; g1 = C; b1 = 0.f; } else if (H < 180.f) {r1 = 0.f; g1 = C; b1 = X;} else if (H < 240.f) { r1 = 0.f; g1 = X; b1 = C; @@ -199,10 +199,11 @@ void NavMapDisplay::processMessage(const NavMapMsg::ConstSharedPtr msg) geometry_msgs::msg::Pose identity; if (!context_->getFrameManager()->transform( - msg->header, identity, position, orientation)) + msg->header, identity, position, orientation)) { - setStatus(rviz_common::properties::StatusProperty::Error, - "TF", "Unable to transform " + QString::fromStdString(msg->header.frame_id)); + setStatus( + rviz_common::properties::StatusProperty::Error, + "TF", "Unable to transform " + QString::fromStdString(msg->header.frame_id)); return; } @@ -268,24 +269,27 @@ void NavMapDisplay::subscribeToLayerTopic() s << "Some layer messages were lost. New lost: " << info.total_count_change << " | Total lost: " << info.total_count; - setStatus(rviz_common::properties::StatusProperty::Warn, "Layer Update Topic", + setStatus( + rviz_common::properties::StatusProperty::Warn, "Layer Update Topic", s.str().c_str()); }; layer_subscription_ = node->create_subscription( - layer_topic_property_->getTopicStd(), - layer_profile_, + layer_topic_property_->getTopicStd(), + layer_profile_, [this](NavMapLayerMsg::ConstSharedPtr msg) {incomingLayer(msg);}, - sub_opts); + sub_opts); layer_subscription_start_time_ = node->now(); setStatus(rviz_common::properties::StatusProperty::Ok, "Layer Update Topic", "OK"); } catch (const rclcpp::exceptions::InvalidTopicNameError & e) { - setStatus(rviz_common::properties::StatusProperty::Error, + setStatus( + rviz_common::properties::StatusProperty::Error, "Layer Update Topic", QString("Invalid topic: ") + e.what()); } catch (const std::exception & e) { - setStatus(rviz_common::properties::StatusProperty::Error, + setStatus( + rviz_common::properties::StatusProperty::Error, "Layer Update Topic", QString("Failed to subscribe: ") + e.what()); } } @@ -319,13 +323,15 @@ void NavMapDisplay::incomingLayer(const NavMapLayerMsg::ConstSharedPtr & msg) const int non_empty = (n_u8 ? 1 : 0) + (n_f32 ? 1 : 0) + (n_f64 ? 1 : 0); if (non_empty != 1) { - setStatus(rviz_common::properties::StatusProperty::Error, "Layer Update", + setStatus( + rviz_common::properties::StatusProperty::Error, "Layer Update", "Exactly one of data_u8 / data_f32 / data_f64 must be non-empty."); return; } const size_t eff_len = n_u8 ? n_u8 : (n_f32 ? n_f32 : n_f64); if (eff_len != n_tris) { - setStatus(rviz_common::properties::StatusProperty::Error, "Layer Update", + setStatus( + rviz_common::properties::StatusProperty::Error, "Layer Update", QString("Layer size (%1) does not match number of triangles (%2)") .arg(eff_len).arg(n_tris)); return; @@ -350,8 +356,9 @@ void NavMapDisplay::incomingLayer(const NavMapLayerMsg::ConstSharedPtr & msg) .arg(QString::fromStdString(msg->name)) .arg(type_str) .arg(qulonglong(len)); - setStatus(rviz_common::properties::StatusProperty::Ok, - "Layer Update Topic", line); + setStatus( + rviz_common::properties::StatusProperty::Ok, + "Layer Update Topic", line); if (currentSelectedLayer_() == msg->name) { updateColorSchemeOptions_(); @@ -551,13 +558,13 @@ void NavMapDisplay::ensureMeshBuilt_() // Create the dynamic colour buffer (one 32-bit colour per vertex) const Ogre::VertexElementType col_type = Ogre::VET_COLOUR_ARGB; // we'll pack ARGB - decl->addElement(COLOR_SRC, /*offset=*/0, col_type, Ogre::VES_DIFFUSE); + decl->addElement(COLOR_SRC, /*offset=*/ 0, col_type, Ogre::VES_DIFFUSE); Ogre::HardwareVertexBufferSharedPtr colour_vbuf = Ogre::HardwareBufferManager::getSingleton().createVertexBuffer( - Ogre::VertexElement::getTypeSize(col_type), // should be 4 - vertex_count, - Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY_DISCARDABLE); + Ogre::VertexElement::getTypeSize(col_type), // should be 4 + vertex_count, + Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY_DISCARDABLE); bind->setBinding(COLOR_SRC, colour_vbuf); @@ -658,7 +665,7 @@ void NavMapDisplay::updateColorsOnly_() uint32_t * p = reinterpret_cast(base); auto packARGB = [&](const Ogre::ColourValue & c) -> uint32_t { - // Pack into ARGB to match VET_COLOUR_ARGB used at creation + // Pack into ARGB to match VET_COLOUR_ARGB used at creation return Ogre::VertexElement::convertColourValue(c, Ogre::VET_COLOUR_ARGB); }; @@ -696,7 +703,7 @@ void NavMapDisplay::updateColorsOnly_() for (size_t t = 0; t < V0.size(); ++t) { const Ogre::ColourValue col = use_rainbow ? colorFromRainbow(selected_layer->data_f32[t], max_val, alpha) : - colorFromHeat (selected_layer->data_f32[t], max_val, alpha); + colorFromHeat(selected_layer->data_f32[t], max_val, alpha); const uint32_t packed = packARGB(col); *p++ = packed; *p++ = packed; *p++ = packed; } @@ -705,7 +712,7 @@ void NavMapDisplay::updateColorsOnly_() const float v = static_cast(selected_layer->data_f64[t]); const Ogre::ColourValue col = use_rainbow ? colorFromRainbow(v, max_val, alpha) : - colorFromHeat (v, max_val, alpha); + colorFromHeat(v, max_val, alpha); const uint32_t packed = packARGB(col); *p++ = packed; *p++ = packed; *p++ = packed; } From c047a44df6f7f10b2681482e97c0c3e82c9e0322 Mon Sep 17 00:00:00 2001 From: "Juan S. Cely G." Date: Fri, 31 Jul 2026 21:51:20 +0200 Subject: [PATCH 29/29] Migration works Signed-off-by: Juan S. Cely G. --- navmap_ros/CMakeLists.txt | 14 +++++++------- navmap_rviz_plugin/CMakeLists.txt | 24 ++++++++++++++---------- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/navmap_ros/CMakeLists.txt b/navmap_ros/CMakeLists.txt index 7d8463c..b4466c6 100644 --- a/navmap_ros/CMakeLists.txt +++ b/navmap_ros/CMakeLists.txt @@ -27,13 +27,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $ ${PCL_INCLUDE_DIRS} - ${pcl_conversions_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME} PRIVATE - pcl_conversions::pcl_conversions - ${PCL_LIBRARIES} -) -target_link_libraries(${PROJECT_NAME} PUBLIC + +target_link_libraries(${PROJECT_NAME} rclcpp::rclcpp navmap_core::navmap_core ${navmap_ros_interfaces_TARGETS} @@ -41,6 +37,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ${nav_msgs_TARGETS} ${sensor_msgs_TARGETS} ${std_srvs_TARGETS} + ${PCL_LIBRARIES} ) add_executable(slam_server_app src/slam_server_app.cpp @@ -72,6 +69,10 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() +ament_target_dependencies(${PROJECT_NAME} + pcl_conversions +) + ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) @@ -83,7 +84,6 @@ ament_export_dependencies( geometry_msgs sensor_msgs std_srvs - PCL pcl_conversions ) ament_package() \ No newline at end of file diff --git a/navmap_rviz_plugin/CMakeLists.txt b/navmap_rviz_plugin/CMakeLists.txt index ed73758..d910fce 100644 --- a/navmap_rviz_plugin/CMakeLists.txt +++ b/navmap_rviz_plugin/CMakeLists.txt @@ -1,7 +1,17 @@ cmake_minimum_required(VERSION 3.10) project(navmap_rviz_plugin) -find_package(Qt6 REQUIRED COMPONENTS Widgets) +set(CMAKE_AUTOMOC ON) + +find_package(Qt6 QUIET COMPONENTS Widgets) +if(Qt6_FOUND) + set(QT_DEPENDENCY Qt6) + set(QT_WIDGETS_TARGET Qt6::Widgets) +else() + find_package(Qt5 REQUIRED COMPONENTS Widgets) + set(QT_DEPENDENCY Qt5) + set(QT_WIDGETS_TARGET Qt5::Widgets) +endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -14,20 +24,14 @@ find_package(navmap_ros_interfaces REQUIRED) find_package(navmap_ros REQUIRED) find_package(navmap_core REQUIRED) -qt_wrap_cpp(NAVMAP_MOC_SRCS - include/navmap_rviz_plugin/NavMapDisplay.hpp - include/navmap_rviz_plugin/navmap_goal_tool.hpp -) - add_library(${PROJECT_NAME} SHARED src/navmap_rviz_plugin/NavMapDisplay.cpp src/navmap_rviz_plugin/navmap_goal_tool.cpp src/navmap_rviz_plugin/navmap_pose_tool.cpp - ${NAVMAP_MOC_SRCS} ) target_link_libraries(navmap_rviz_plugin PUBLIC ${navmap_ros_interfaces_TARGETS} - Qt6::Widgets + ${QT_WIDGETS_TARGET} navmap_ros::navmap_ros navmap_core::navmap_core pluginlib::pluginlib @@ -70,7 +74,7 @@ endif() ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( - Qt6 + ${QT_DEPENDENCY} rclcpp pluginlib rviz_common @@ -78,6 +82,6 @@ ament_export_dependencies( rviz_default_plugins navmap_ros_interfaces geometry_msgs - std_msg + std_msgs ) ament_package()