From 7b021264080bde4955263cc1e369bad92c67c0c4 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 22 Feb 2024 13:01:43 +0300 Subject: [PATCH] Prevent a possible segmentation fault (#4141) (#4147) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Prevent a possible segmentation fault #4141 Signed-off-by: Joni Pöllänen * Cleanup Signed-off-by: Joni Pöllänen --------- Signed-off-by: Joni Pöllänen Signed-off-by: Alberto Tudela --- .../include/nav2_costmap_2d/costmap_2d.hpp | 98 --- .../costmap_filters/costmap_filter.hpp | 28 - .../plugins/costmap_filters/binary_filter.cpp | 5 +- .../costmap_filters/costmap_filter.cpp | 23 - .../costmap_filters/keepout_filter.cpp | 6 +- .../plugins/costmap_filters/speed_filter.cpp | 5 +- nav2_costmap_2d/plugins/obstacle_layer.cpp | 4 +- nav2_costmap_2d/src/costmap_2d.cpp | 8 +- .../test/regression/CMakeLists.txt | 6 - .../test/regression/costmap_bresenham_2d.cpp | 159 ----- .../test/unit/costmap_filter_test.cpp | 18 +- nav2_map_server/CMakeLists.txt | 27 +- .../nav2_map_server/vector_object_server.hpp | 228 +++++++ .../nav2_map_server/vector_object_shapes.hpp | 456 ++++++++++++++ .../nav2_map_server/vector_object_utils.hpp | 132 ++++ .../launch/vector_object_server.launch.py | 97 +++ .../params/vector_object_server_params.yaml | 31 + .../src/vo_server/vector_object_server.cpp | 582 ++++++++++++++++++ .../vo_server/vector_object_server_node.cpp | 29 + .../src/vo_server/vector_object_shapes.cpp | 579 +++++++++++++++++ nav2_msgs/CMakeLists.txt | 8 +- nav2_msgs/msg/CircleVO.msg | 6 + nav2_msgs/msg/PolygonVO.msg | 5 + nav2_msgs/package.xml | 1 + nav2_msgs/srv/AddShapes.srv | 6 + nav2_msgs/srv/GetShapes.srv | 5 + nav2_msgs/srv/RemoveShapes.srv | 5 + .../include/nav2_util/occ_grid_utils.hpp | 129 ++++ .../include/nav2_util/raytrace_line_2d.hpp | 146 +++++ nav2_util/test/CMakeLists.txt | 2 + nav2_util/test/regression/CMakeLists.txt | 2 + .../test/regression/map_bresenham_2d.cpp | 167 +++++ 32 files changed, 2665 insertions(+), 338 deletions(-) delete mode 100644 nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp create mode 100644 nav2_map_server/include/nav2_map_server/vector_object_server.hpp create mode 100644 nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp create mode 100644 nav2_map_server/include/nav2_map_server/vector_object_utils.hpp create mode 100644 nav2_map_server/launch/vector_object_server.launch.py create mode 100644 nav2_map_server/params/vector_object_server_params.yaml create mode 100644 nav2_map_server/src/vo_server/vector_object_server.cpp create mode 100644 nav2_map_server/src/vo_server/vector_object_server_node.cpp create mode 100644 nav2_map_server/src/vo_server/vector_object_shapes.cpp create mode 100644 nav2_msgs/msg/CircleVO.msg create mode 100644 nav2_msgs/msg/PolygonVO.msg create mode 100644 nav2_msgs/srv/AddShapes.srv create mode 100644 nav2_msgs/srv/GetShapes.srv create mode 100644 nav2_msgs/srv/RemoveShapes.srv create mode 100644 nav2_util/include/nav2_util/occ_grid_utils.hpp create mode 100644 nav2_util/include/nav2_util/raytrace_line_2d.hpp create mode 100644 nav2_util/test/regression/CMakeLists.txt create mode 100644 nav2_util/test/regression/map_bresenham_2d.cpp diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 005dd205d80..16335caa0de 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -426,105 +426,7 @@ class Costmap2D */ virtual void initMaps(unsigned int size_x, unsigned int size_y); - /** - * @brief Raytrace a line and apply some action at each step - * @param at The action to take... a functor - * @param x0 The starting x coordinate - * @param y0 The starting y coordinate - * @param x1 The ending x coordinate - * @param y1 The ending y coordinate - * @param max_length The maximum desired length of the segment... - * allows you to not go all the way to the endpoint - * @param min_length The minimum desired length of the segment - */ - template - inline void raytraceLine( - ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, - unsigned int y1, - unsigned int max_length = UINT_MAX, unsigned int min_length = 0) - { - int dx_full = x1 - x0; - int dy_full = y1 - y0; - - // we need to chose how much to scale our dominant dimension, - // based on the maximum length of the line - double dist = std::hypot(dx_full, dy_full); - if (dist < min_length) { - return; - } - - unsigned int min_x0, min_y0; - if (dist > 0.0) { - // Adjust starting point and offset to start from min_length distance - min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); - min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); - } else { - // dist can be 0 if [x0, y0]==[x1, y1]. - // In this case only this cell should be processed. - min_x0 = x0; - min_y0 = y0; - } - unsigned int offset = min_y0 * size_x_ + min_x0; - - int dx = x1 - min_x0; - int dy = y1 - min_y0; - - unsigned int abs_dx = abs(dx); - unsigned int abs_dy = abs(dy); - - int offset_dx = sign(dx); - int offset_dy = sign(dy) * size_x_; - - double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); - // if x is dominant - if (abs_dx >= abs_dy) { - int error_y = abs_dx / 2; - - bresenham2D( - at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); - return; - } - - // otherwise y is dominant - int error_x = abs_dy / 2; - - bresenham2D( - at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); - } - private: - /** - * @brief A 2D implementation of Bresenham's raytracing algorithm... - * applies an action at each step - */ - template - inline void bresenham2D( - ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, - int offset_a, - int offset_b, unsigned int offset, - unsigned int max_length) - { - unsigned int end = std::min(max_length, abs_da); - for (unsigned int i = 0; i < end; ++i) { - at(offset); - offset += offset_a; - error_b += abs_db; - if ((unsigned int)error_b >= abs_da) { - offset += offset_b; - error_b -= abs_da; - } - } - at(offset); - } - - /** - * @brief get the sign of an int - */ - inline int sign(int x) - { - return x > 0 ? 1.0 : -1.0; - } - mutex_t * access_; protected: diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index d1128b5ccc6..e72649b4b96 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -184,34 +184,6 @@ class CostmapFilter : public Layer const std::string mask_frame, geometry_msgs::msg::Pose2D & mask_pose) const; - /** - * @brief: Convert from world coordinates to mask coordinates. - Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. - * @param filter_mask Filter mask on which to convert - * @param wx The x world coordinate - * @param wy The y world coordinate - * @param mx Will be set to the associated mask x coordinate - * @param my Will be set to the associated mask y coordinate - * @return True if the conversion was successful (legal bounds) false otherwise - */ - bool worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const; - - /** - * @brief Get the data of a cell in the filter mask - * @param filter_mask Filter mask to get the data from - * @param mx The x coordinate of the cell - * @param my The y coordinate of the cell - * @return The data of the selected cell - */ - inline int8_t getMaskData( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - const unsigned int mx, const unsigned int my) const - { - return filter_mask->data[my * filter_mask->info.width + mx]; - } - /** * @brief Get the cost of a cell in the filter mask * @param filter_mask Filter mask to get the cost from diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp index 78b761e8d33..834874c2fc3 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -44,6 +44,7 @@ #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -188,7 +189,7 @@ void BinaryFilter::process( // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + if (!nav2_util::worldToMap(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { // Robot went out of mask range. Set "false" state by-default RCLCPP_WARN( logger_, @@ -198,7 +199,7 @@ void BinaryFilter::process( } // Getting filter_mask data from cell where the robot placed - int8_t mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j); + int8_t mask_data = nav2_util::getMapData(filter_mask_, mask_robot_i, mask_robot_j); if (mask_data == nav2_util::OCC_GRID_UNKNOWN) { // Corresponding filter mask cell is unknown. // Warn and do nothing. diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index f05726a6b32..f8b3f435236 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -189,29 +189,6 @@ bool CostmapFilter::transformPose( return true; } -bool CostmapFilter::worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const -{ - const double origin_x = filter_mask->info.origin.position.x; - const double origin_y = filter_mask->info.origin.position.y; - const double resolution = filter_mask->info.resolution; - const unsigned int size_x = filter_mask->info.width; - const unsigned int size_y = filter_mask->info.height; - - if (wx < origin_x || wy < origin_y) { - return false; - } - - mx = static_cast((wx - origin_x) / resolution); - my = static_cast((wy - origin_y) / resolution); - if (mx >= size_x || my >= size_y) { - return false; - } - - return true; -} - unsigned char CostmapFilter::getMaskCost( nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int & my) const diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 3ac785296a2..450d1965f25 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -35,14 +35,16 @@ * Author: Alexey Merzlyakov *********************************************************************/ +#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" + #include #include #include #include "tf2/convert.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -278,7 +280,7 @@ void KeepoutFilter::process( msk_wy = gl_wy; } // Get mask coordinates corresponding to (i, j) point at filter_mask_ - if (worldToMask(filter_mask_, msk_wx, msk_wy, mx, my)) { + if (nav2_util::worldToMap(filter_mask_, msk_wx, msk_wy, mx, my)) { data = getMaskCost(filter_mask_, mx, my); // Update if mask_ data is valid and greater than existing master_grid's one if (data == NO_INFORMATION) { diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index ac30accf244..10082ba9227 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -43,6 +43,7 @@ #include #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" namespace nav2_costmap_2d { @@ -195,13 +196,13 @@ void SpeedFilter::process( // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + if (!nav2_util::worldToMap(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { return; } // Getting filter_mask data from cell where the robot placed and // calculating speed limit value - int8_t speed_mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j); + int8_t speed_mask_data = nav2_util::getMapData(filter_mask_, mask_robot_i, mask_robot_j); if (speed_mask_data == SPEED_MASK_NO_LIMIT) { // Corresponding filter mask cell is free. // Setting no speed limit there. diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index cccae4d3938..55ad4d511f1 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -45,6 +45,7 @@ #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "nav2_util/raytrace_line_2d.hpp" #include "nav2_costmap_2d/costmap_math.hpp" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) @@ -697,7 +698,8 @@ ObstacleLayer::raytraceFreespace( unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_); MarkCell marker(costmap_, FREE_SPACE); // and finally... we can execute our trace to clear obstacles along that line - raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_max_range, cell_raytrace_min_range); + nav2_util::raytraceLine( + marker, x0, y0, x1, y1, size_x_, cell_raytrace_max_range, cell_raytrace_min_range); updateRaytraceBounds( ox, oy, wx, wy, clearing_observation.raytrace_max_range_, diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 9cb7ce19816..72c3cc7aab4 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -43,6 +43,7 @@ #include #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/raytrace_line_2d.hpp" namespace nav2_costmap_2d { @@ -430,14 +431,15 @@ void Costmap2D::polygonOutlineCells( { PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells); for (unsigned int i = 0; i < polygon.size() - 1; ++i) { - raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y); + nav2_util::raytraceLine( + cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y, size_x_); } if (!polygon.empty()) { unsigned int last_index = polygon.size() - 1; // we also need to close the polygon by going from the last point to the first - raytraceLine( + nav2_util::raytraceLine( cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, - polygon[0].y); + polygon[0].y, size_x_); } } diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index aafc4d561be..46f978859a6 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,9 +1,3 @@ -# Bresenham2D corner cases test -ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) -target_link_libraries(costmap_bresenham_2d - ${PROJECT_NAME}::nav2_costmap_2d_core -) - # OrderLayer for checking Costmap2D plugins API calling order add_library(order_layer SHARED order_layer.cpp) diff --git a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp deleted file mode 100644 index a45cb38a7f9..00000000000 --- a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2022 Samsung Research Russia -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Willow Garage, Inc. nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* Author: Alexey Merzlyakov -*********************************************************************/ -#include -#include - -class CostmapAction -{ -public: - explicit CostmapAction( - unsigned char * costmap, unsigned int size, unsigned char mark_val = 128) - : costmap_(costmap), size_(size), mark_val_(mark_val) - { - } - - inline void operator()(unsigned int off) - { - ASSERT_TRUE(off < size_); - costmap_[off] = mark_val_; - } - - inline unsigned int get(unsigned int off) - { - return costmap_[off]; - } - -private: - unsigned char * costmap_; - unsigned int size_; - unsigned char mark_val_; -}; - -class CostmapTest : public nav2_costmap_2d::Costmap2D -{ -public: - CostmapTest( - unsigned int size_x, unsigned int size_y, double resolution, - double origin_x, double origin_y, unsigned char default_val = 0) - : nav2_costmap_2d::Costmap2D(size_x, size_y, resolution, origin_x, origin_y, default_val) - { - } - - unsigned char * getCostmap() - { - return costmap_; - } - - unsigned int getSize() - { - return size_x_ * size_y_; - } - - void raytraceLine( - CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1, - unsigned int y1, - unsigned int max_length = UINT_MAX, unsigned int min_length = 0) - { - nav2_costmap_2d::Costmap2D::raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } -}; - -TEST(costmap_2d, bresenham2DBoundariesCheck) -{ - const unsigned int sz_x = 60; - const unsigned int sz_y = 60; - const unsigned int max_length = 60; - const unsigned int min_length = 6; - CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); - CostmapAction ca(ct.getCostmap(), ct.getSize()); - - // Initial point - some assymetrically standing point in order to cover most corner cases - const unsigned int x0 = 2; - const unsigned int y0 = 4; - // (x1, y1) point will move - unsigned int x1, y1; - - // Running on (x, 0) edge - y1 = 0; - for (x1 = 0; x1 < sz_x; x1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (x, sz_y) edge - y1 = sz_y - 1; - for (x1 = 0; x1 < sz_x; x1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (0, y) edge - x1 = 0; - for (y1 = 0; y1 < sz_y; y1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } - - // Running on (sz_x, y) edge - x1 = sz_x - 1; - for (y1 = 0; y1 < sz_y; y1++) { - ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); - } -} - -TEST(costmap_2d, bresenham2DSamePoint) -{ - const unsigned int sz_x = 60; - const unsigned int sz_y = 60; - const unsigned int max_length = 60; - const unsigned int min_length = 0; - CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); - CostmapAction ca(ct.getCostmap(), ct.getSize()); - - // Initial point - const double x0 = 2; - const double y0 = 4; - - unsigned int offset = y0 * sz_x + x0; - unsigned char val_before = ca.get(offset); - // Same point to check - ct.raytraceLine(ca, x0, y0, x0, y0, max_length, min_length); - unsigned char val_after = ca.get(offset); - ASSERT_FALSE(val_before == val_after); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp index e5416845cce..d7eb87da3a0 100644 --- a/nav2_costmap_2d/test/unit/costmap_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_filter_test.cpp @@ -19,6 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/occ_grid_utils.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" @@ -30,13 +31,6 @@ class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter public: CostmapFilterWrapper() {} - bool worldToMask( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - double wx, double wy, unsigned int & mx, unsigned int & my) const - { - return nav2_costmap_2d::CostmapFilter::worldToMask(filter_mask, wx, wy, mx, my); - } - unsigned char getMaskCost( nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int & my) const @@ -82,19 +76,19 @@ TEST(CostmapFilter, testWorldToMask) CostmapFilterWrapper cf; unsigned int mx, my; // Point inside mask - ASSERT_TRUE(cf.worldToMask(mask, 4.0, 5.0, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 4.0, 5.0, mx, my)); ASSERT_EQ(mx, 1u); ASSERT_EQ(my, 2u); // Corner cases - ASSERT_TRUE(cf.worldToMask(mask, 3.0, 3.0, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 3.0, 3.0, mx, my)); ASSERT_EQ(mx, 0u); ASSERT_EQ(my, 0u); - ASSERT_TRUE(cf.worldToMask(mask, 5.9, 5.9, mx, my)); + ASSERT_TRUE(nav2_util::worldToMap(mask, 5.9, 5.9, mx, my)); ASSERT_EQ(mx, 2u); ASSERT_EQ(my, 2u); // Point outside mask - ASSERT_FALSE(cf.worldToMask(mask, 2.9, 2.9, mx, my)); - ASSERT_FALSE(cf.worldToMask(mask, 6.0, 6.0, mx, my)); + ASSERT_FALSE(nav2_util::worldToMap(mask, 2.9, 2.9, mx, my)); + ASSERT_FALSE(nav2_util::worldToMap(mask, 6.0, 6.0, mx, my)); } TEST(CostmapFilter, testGetMaskCost) diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index fd4208454bb..204cf78d9cc 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -17,6 +17,8 @@ find_package(nav2_util REQUIRED) find_package(GRAPHICSMAGICKCPP REQUIRED) find_package(yaml-cpp REQUIRED) +pkg_search_module(UUID REQUIRED uuid) + nav2_package() include_directories(include) @@ -29,6 +31,8 @@ set(map_saver_server_executable map_saver_server) set(costmap_filter_info_server_executable costmap_filter_info_server) +set(vector_object_server_executable vector_object_server) + add_executable(${map_server_executable} src/map_server/main.cpp) @@ -41,6 +45,11 @@ add_executable(${map_saver_server_executable} add_executable(${costmap_filter_info_server_executable} src/costmap_filter_info/main.cpp) +add_executable(${vector_object_server_executable} + src/vo_server/vector_object_shapes.cpp + src/vo_server/vector_object_server.cpp + src/vo_server/vector_object_server_node.cpp) + set(map_io_library_name map_io) set(library_name ${map_server_executable}_core) @@ -77,6 +86,13 @@ set(map_saver_dependencies nav2_msgs nav2_util) +set(vo_server_dependencies + rclcpp + rclcpp_lifecycle + nav_msgs + nav2_msgs + nav2_util) + ament_target_dependencies(${map_server_executable} ${map_server_dependencies}) @@ -95,6 +111,10 @@ ament_target_dependencies(${library_name} ament_target_dependencies(${map_io_library_name} ${map_io_dependencies}) +ament_target_dependencies(${vector_object_server_executable} + ${vo_server_dependencies} +) + target_link_libraries(${library_name} ${map_io_library_name}) @@ -127,6 +147,10 @@ if(WIN32) YAML_CPP_DLL) endif() +target_link_libraries(${vector_object_server_executable} + ${UUID_LIBRARIES} +) + rclcpp_components_register_nodes(${library_name} "nav2_map_server::CostmapFilterInfoServer") rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapSaver") rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapServer") @@ -139,13 +163,14 @@ install(TARGETS install(TARGETS ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} - ${costmap_filter_info_server_executable} + ${costmap_filter_info_server_executable} ${vector_object_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp new file mode 100644 index 00000000000..d28582a1104 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -0,0 +1,228 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "nav2_msgs/srv/add_shapes.hpp" +#include "nav2_msgs/srv/remove_shapes.hpp" +#include "nav2_msgs/srv/get_shapes.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/occ_grid_values.hpp" + +#include "nav2_map_server/vector_object_utils.hpp" +#include "nav2_map_server/vector_object_shapes.hpp" + +namespace nav2_map_server +{ + +/// @brief Vector Object server node +class VectorObjectServer : public nav2_util::LifecycleNode +{ +public: + /** + * @brief Constructor for the VectorObjectServer + * @param options Additional options to control creation of the node. + */ + explicit VectorObjectServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +protected: + /** + * @brief: Initializes TF buffer/listener, obtains ROS-parameters, creates incoming services, + * and output map publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Activates output map publisher and creates bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Deactivates map publisher and timer (if any), destroys bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Resets all services, publishers, map and TF-s + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called in shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Transform all vector shapes from their local frame to output map frame + */ + bool transformVectorObjects(); + + /** + * @brief Obtains map boundaries to place all vector objects inside + * @param min_x output min X-boundary of required map + * @param min_y output min Y-boundary of required map + * @param max_x output max X-boundary of required map + * @param max_y output max Y-boundary of required map + * @throw std::exception if can not obtain one of the map boundaries + */ + void getMapBoundaries( + double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Creates or updates existing map with required sizes and fills it with default value + * @param min_x min X-boundary of new map + * @param min_y min Y-boundary of new map + * @param max_x max X-boundary of new map + * @param max_y max Y-boundary of new map + * @throw std::exception if map has negative X or Y size + */ + void updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y); + + /** + * @brief Processes all vector objects on raster output map + */ + void putVectorObjectsOnMap(); + + /** + * @brief Publishes output map + */ + void publishMap(); + + /** + * @brief Calculates new map sizes, updates map, process all vector objects on it + * and publish output map one time + */ + void processMap(); + + /** + * @brief Whether the map should be updated dynamically: + * at least one of the vector shapes is in another than output map frame + * @return True if map should be updated dynamically + */ + bool isMapUpdate(); + + /** + * @brief If map to be update dynamically, creates map processing timer, + * otherwise process map once + */ + void switchMapUpdate(); + + /** + * @brief Callback for AddShapes service call. + * Reads all input vector objects from service request, + * creates them or updates their shape in case of existing objects + * and switches map processing/publishing routine + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void addShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Callback for GetShapes service call. + * Gets all shapes and returns them to the service response + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void getShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Callback for RemoveShapes service call. + * Try to remove all requested objects and switches map processing/publishing routine + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void removeShapesCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + +protected: + /** + * @brief Supporting routine obtaining all ROS-parameters + * @return True if all parameters were obtained or false in the failure case + */ + bool getROSParameters(); + + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief TF listener + std::shared_ptr tf_listener_; + + /// @brief All vector objects vector + std::vector> shapes_; + + /// @brief Output map resolution + double resolution_; + /// @brief Default value the output map to be filled with + int8_t default_value_; + /// @brief @Overlay Type of overlay of vector objects on the map + OverlayType overlay_type_; + + /// @brief Output map with vector objects on it + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + /// @brief Whether to process and publish map + double process_map_; + + /// @brief Frame of output map + std::string frame_id_; + /// @brief Transform tolerance + double transform_tolerance_; + + /// @brief Frequency to dynamically update/publish the map (if necessary) + double update_frequency_; + /// @brief Map update timer + rclcpp::TimerBase::SharedPtr map_timer_; + + /// @brief AddShapes service + rclcpp::Service::SharedPtr add_shapes_service_; + /// @brief GetShapes service + rclcpp::Service::SharedPtr get_shapes_service_; + /// @brief RemoveShapes service + rclcpp::Service::SharedPtr remove_shapes_service_; + + /// @beirf Topic name where the output map to be published to + std::string map_topic_; + /// @brief Output map publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr map_pub_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_SERVER_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp new file mode 100644 index 00000000000..f8252cb513b --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -0,0 +1,456 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ + +#include +#include +#include +#include + +#include "geometry_msgs/msg/polygon.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.h" + +#include "nav2_msgs/msg/polygon_vo.hpp" +#include "nav2_msgs/msg/circle_vo.hpp" +#include "nav2_util/lifecycle_node.hpp" + +#include "nav2_map_server/vector_object_utils.hpp" + +namespace nav2_map_server +{ + +/// @brief Possible VO-shape types +enum ShapeType +{ + UNKNOWN = 0, + POLYGON = 1, + CIRCLE = 2 +}; + +/// @brief Basic class, other vector objects to be inherited from +class Shape +{ +public: + /** + * @brief Shape basic class constructor + * @param node Vector Object server node pointer + */ + explicit Shape(const nav2_util::LifecycleNode::WeakPtr & node); + + /** + * @brief Shape destructor + */ + virtual ~Shape(); + + /** + * @brief Returns type of the shape + * @return Type of the shape + */ + ShapeType getType(); + + /** + * @brief Supporting routine obtaining shape UUID from ROS-parameters + * for the given shape object + * @return True if UUID was obtained or false in failure case + */ + bool getShapeUUID(const std::string & shape_name, unsigned char * out_uuid); + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * Empty virtual method intended to be used in child implementations + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + virtual bool getROSParameters(const std::string & shape_name) = 0; + + /** + * @brief Gets shape boundaries. + * Empty virtual method intended to be used in child implementations + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + virtual void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) = 0; + + /** + * @brief Is the point inside the shape. + * Empty virtual method intended to be used in child implementations + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + virtual bool isPointInside(const double px, const double py) const = 0; + + /** + * @brief Puts shape borders on map. + * Empty virtual method intended to be used in child implementations + * @param map Output map pointer + * @param overlay_type Overlay type + */ + virtual void putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0; + + /** + * @brief Gets the value of the shape. + * Empty virtual method intended to be used in child implementations + * @return OccupancyGrid value of the shape + */ + virtual int8_t getValue() const = 0; + + /** + * @brief Gets frame ID of the shape. + * Empty virtual method intended to be used in child implementations + * @return Frame ID where the shape is + */ + virtual std::string getFrameID() const = 0; + + /** + * @brief Gets UUID of the shape. + * Empty virtual method intended to be used in child implementations + * @return Shape UUID string + */ + virtual std::string getUUID() const = 0; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * Empty virtual method intended to be used in child implementations + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + virtual bool isUUID(const unsigned char * uuid) const = 0; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * Empty virtual method intended to be used in child implementations + * @return True if shape to be filled + */ + virtual bool isFill() const = 0; + + /** + * @brief Transforms shape coordinates to a new frame. + * Empty virtual method intended to be used in child implementations + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + virtual bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) = 0; + +protected: + /// @brief Type of shape + ShapeType type_; + + /// @brief VectorObjectServer node + nav2_util::LifecycleNode::WeakPtr node_; +}; + +/// @brief Polygon shape class +class Polygon : public Shape +{ +public: + /* + * @brief Polygon class constructor + * @param node Vector Object server node pointer + * @param params PolygonVO parameters. In case of nullptr, + * parameters to be read from ROS-parameters. + * @throw std::exception in case of inconsistent shape + */ + Polygon( + const nav2_util::LifecycleNode::WeakPtr & node, + const nav2_msgs::msg::PolygonVO::SharedPtr params = nullptr); + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool getROSParameters(const std::string & shape_name); + + /** + * @brief Gets shape boundaries + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Is the point inside the shape. + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + bool isPointInside(const double px, const double py) const; + + /** + * @brief Puts shape borders on map. + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + + /** + * @brief Gets Polygon parameters + * @return Polygon parameters + */ + nav2_msgs::msg::PolygonVO::SharedPtr getParams() const; + + /** + * @brief Tries to update Polygon parameters + * @throw std::exception in case of inconsistent shape + */ + void setParams(const nav2_msgs::msg::PolygonVO::SharedPtr params); + + /** + * @brief Gets the value of the shape. + * @return OccupancyGrid value of the shape + */ + int8_t getValue() const; + + /** + * @brief Gets frame ID of the shape. + * @return Frame ID where the shape is + */ + std::string getFrameID() const; + + /** + * @brief Gets UUID of the shape. + * @return Shape UUID string + */ + std::string getUUID() const; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + bool isUUID(const unsigned char * uuid) const; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * @return True if shape to be filled + */ + bool isFill() const; + + /** + * @brief Transforms shape coordinates to a new frame. + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance); + +protected: + /** + * @brief Checks that shape is consistent for further operation + * @throw std::exception in case of inconsistent shape + */ + void checkConsistency(); + + /// @brief Input polygon parameters (could be in any frame) + nav2_msgs::msg::PolygonVO::SharedPtr params_; + /// @brief Polygon in the map's frame + geometry_msgs::msg::Polygon::SharedPtr polygon_; +}; + +/// @brief Circle shape class +class Circle : public Shape +{ +public: + /* + * @brief Circle class constructor + * @param node Vector Object server node pointer + * @param params CircleVO parameters. In case of nullptr, + * parameters to be read from ROS-parameters. + * @throw std::exception in case of inconsistent shape + */ + Circle( + const nav2_util::LifecycleNode::WeakPtr & node, + const nav2_msgs::msg::CircleVO::SharedPtr params = nullptr); + + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool getROSParameters(const std::string & shape_name); + + /** + * @brief Gets shape boundaries + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Is the point inside the shape. + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + bool isPointInside(const double px, const double py) const; + + /** + * @brief Puts shape borders on map. + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + + /** + * @brief Gets Circle parameters + * @return Circle parameters + */ + nav2_msgs::msg::CircleVO::SharedPtr getParams() const; + + /** + * @brief Tries to update Circle parameters + * @throw std::exception in case of inconsistent shape + */ + void setParams(const nav2_msgs::msg::CircleVO::SharedPtr params); + + /** + * @brief Gets the value of the shape. + * @return OccupancyGrid value of the shape + */ + int8_t getValue() const; + + /** + * @brief Gets frame ID of the shape. + * @return Frame ID where the shape is + */ + std::string getFrameID() const; + + /** + * @brief Gets UUID of the shape. + * @return Shape UUID string + */ + std::string getUUID() const; + + /** + * @brief Checks whether the shape is equal to a given UUID. + * @param uuid Given UUID to check with + * @return True if the shape has the same as given UUID, otherwise false + */ + bool isUUID(const unsigned char * uuid) const; + + /** + * @brief Whether the shape to be filled or only its borders to be put on map. + * @return True if shape to be filled + */ + bool isFill() const; + + /** + * @brief Transforms shape coordinates to a new frame. + * @param to_frame Frame ID to transform to + * @param tf_buffer TF buffer to use for the transformation + * @param transform_tolerance Transform tolerance + * @return Whether it was transformed successfully + */ + bool toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance); + +protected: + /** + * @brief Checks that shape is consistent for further operation + * @throw std::exception in case of inconsistent shape + */ + void checkConsistency(); + + /** + * @brief Converts circle center to map coordinates + * considering FP-accuracy loosing on small values when using conventional + * nav2_util::worldToMap() approach + * @param map Map pointer + * @param mcx Output X-coordinate of associated circle center on map + * @param mcy Output Y-coordinate of associated circle center on map + * @return True if the conversion was successful + */ + bool centerToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + unsigned int & mcx, unsigned int & mcy); + + /** + * @brief Put Circle's point on map + * @param mx X-coordinate of given point + * @param my Y-coordinate of given point + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putPoint( + unsigned int mx, unsigned int my, + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const OverlayType overlay_type); + + /// @brief Input circle parameters (could be in any frame) + nav2_msgs::msg::CircleVO::SharedPtr params_; + /// @brief Circle center in the map's frame + geometry_msgs::msg::Point32::SharedPtr center_; +}; + +/// @brief Functor class used in raytraceLine algorithm +class MapAction +{ +public: + /** + * @brief MapAction constructor + * @param map Output map pointer + * @param value Value to put on map + * @param overlay_type Overlay type + */ + MapAction( + nav_msgs::msg::OccupancyGrid::SharedPtr map, + int8_t value, OverlayType overlay_type) + : map_(map), value_(value), overlay_type_(overlay_type) + {} + + /** + * @brief Map filling operator + * @param offset Offset on the map where the cell to be changed + */ + inline void operator()(unsigned int offset) + { + fillMap(map_, offset, value_, overlay_type_); + } + +protected: + /// @brief Output map pointer + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + /// @brief Value to put on map + int8_t value_; + /// @brief Overlay type + OverlayType overlay_type_; +}; + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp new file mode 100644 index 00000000000..5065448aa60 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ +#define NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" + +namespace nav2_map_server +{ + +// ---------- Working with ROS-parameters ---------- + +/** + * @brief Declares and obtains ROS-parameter from given node + * @param node LifecycleNode pointer where the parameter belongs to + * @param param_name Parameter name string + * @param default_val Default value of the parameter (in case if parameter is not set) + * @return Obtained parameter value + */ +template +inline rclcpp::Parameter getROSParameter( + nav2_util::LifecycleNode::SharedPtr node, + const std::string & param_name, + const ValT & default_val) +{ + nav2_util::declare_parameter_if_not_declared( + node, param_name, rclcpp::ParameterValue(default_val)); + return node->get_parameter(param_name); +} + +/** + * @brief Declares and obtains ROS-parameter from given node + * @param node LifecycleNode pointer where the parameter belongs to + * @param param_name Parameter name string + * @param val_type Type of obtained parameter + * @return Obtained parameter value + * @throw std::exception if parameter is not set + */ +template<> +inline rclcpp::Parameter getROSParameter( + nav2_util::LifecycleNode::SharedPtr node, + const std::string & param_name, + const rclcpp::ParameterType & val_type) +{ + nav2_util::declare_parameter_if_not_declared( + node, param_name, val_type); + return node->get_parameter(param_name); +} + +// ---------- Working with shapes' overlays ---------- + +/// @brief Type of overlay between different vector objects and map +enum class OverlayType : uint8_t +{ + OVERLAY_SEQ = 0, // Vector objects are superimposed in the order in which they have arrived + OVERLAY_MAX = 1, // Maximum value from vector objects and map is being chosen + OVERLAY_MIN = 2 // Minimum value from vector objects and map is being chosen +}; + +/** + * @brief Updates map value with shape's one according to the given overlay type + * @param map_val Map value. To be updated with new value if overlay is required + * @param shape_val Vector object value to be overlayed on map + * @param overlay_type Type of overlay + * @throw std::exception in case of unknown overlay type + */ +inline void processVal( + int8_t & map_val, const int8_t shape_val, + const OverlayType overlay_type) +{ + switch (overlay_type) { + case OverlayType::OVERLAY_SEQ: + map_val = shape_val; + return; + case OverlayType::OVERLAY_MAX: + if (shape_val > map_val) { + map_val = shape_val; + } + return; + case OverlayType::OVERLAY_MIN: + if ((map_val == nav2_util::OCC_GRID_UNKNOWN || shape_val < map_val) && + shape_val != nav2_util::OCC_GRID_UNKNOWN) + { + map_val = shape_val; + } + return; + default: + throw std::runtime_error{"Unknown overlay type"}; + } +} + +/** + * @brief Fill the cell on the map with given shape value according to the given overlay type + * @param map Output map to be filled with + * @param offset Offset to the cell to be filled + * @param shape_val Vector object value to be overlayed on map + * @param overlay_type Type of overlay + */ +inline void fillMap( + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const unsigned int offset, + const int8_t shape_val, + const OverlayType overlay_type) +{ + int8_t map_val = map->data[offset]; + processVal(map_val, shape_val, overlay_type); + map->data[offset] = map_val; +} + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ diff --git a/nav2_map_server/launch/vector_object_server.launch.py b/nav2_map_server/launch/vector_object_server.launch.py new file mode 100644 index 00000000000..ce0f867e930 --- /dev/null +++ b/nav2_map_server/launch/vector_object_server.launch.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023 Samsung R&D Institute Russia +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Environment + package_dir = get_package_share_directory('nav2_map_server') + + # Constant parameters + lifecycle_nodes = ['vector_object_server'] + autostart = True + + # Launch arguments + # 1. Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + + # 2. Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(package_dir, 'params', 'vector_object_server_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) + + # Nodes launching commands + start_lifecycle_manager_cmd = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + + start_vector_object_server_cmd = Node( + package='nav2_map_server', + executable='vector_object_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params]) + + ld = LaunchDescription() + + # Launch arguments + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + + # Node launching commands + ld.add_action(start_lifecycle_manager_cmd) + ld.add_action(start_vector_object_server_cmd) + + return ld diff --git a/nav2_map_server/params/vector_object_server_params.yaml b/nav2_map_server/params/vector_object_server_params.yaml new file mode 100644 index 00000000000..089e8316e3f --- /dev/null +++ b/nav2_map_server/params/vector_object_server_params.yaml @@ -0,0 +1,31 @@ +vector_object_server: + ros__parameters: + map_topic: "vo_map" + frame_id: "map" + resolution: 0.05 + default_value: -1 + overlay_type: 0 + update_frequency: 1.0 + transform_tolerance: 0.1 + shapes: ["Poly", "CircleA", "CircleB"] + Poly: + type: "polygon" + frame_id: "map" + closed: True + value: 100 + points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + CircleA: + type: "circle" + frame_id: "map" + fill: True + value: 10 + center: [3.0, 3.0] + radius: 0.5 + uuid: "7b3f3d7d-135c-4b6c-aca1-7a84d1050505" + CircleB: + type: "circle" + frame_id: "map" + fill: False + value: 90 + center: [3.5, 3.5] + radius: 1.5 diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp new file mode 100644 index 00000000000..72f100383e6 --- /dev/null +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -0,0 +1,582 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_map_server/vector_object_server.hpp" + +#include +#include +#include +#include +#include + +#include "tf2_ros/create_timer_ros.h" + +#include "nav2_util/occ_grid_utils.hpp" + +using namespace std::placeholders; + +namespace nav2_map_server +{ + +VectorObjectServer::VectorObjectServer(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("vector_object_server", "", options), process_map_(false) +{} + +nav2_util::CallbackReturn +VectorObjectServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Obtaining ROS parameters + if (!getROSParameters()) { + return nav2_util::CallbackReturn::FAILURE; + } + + map_pub_ = create_publisher( + map_topic_, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + // Make name prefix for services + const std::string service_prefix = get_name() + std::string("/"); + + add_shapes_service_ = create_service( + service_prefix + std::string("add_shapes"), + std::bind(&VectorObjectServer::addShapesCallback, this, _1, _2, _3)); + + get_shapes_service_ = create_service( + service_prefix + std::string("get_shapes"), + std::bind(&VectorObjectServer::getShapesCallback, this, _1, _2, _3)); + + remove_shapes_service_ = create_service( + service_prefix + std::string("remove_shapes"), + std::bind(&VectorObjectServer::removeShapesCallback, this, _1, _2, _3)); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +VectorObjectServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + map_pub_->on_activate(); + + // Creating bond connection + createBond(); + + // Trigger map to be published + process_map_ = true; + switchMapUpdate(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +VectorObjectServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + if (map_timer_) { + map_timer_->cancel(); + map_timer_.reset(); + } + process_map_ = false; + + map_pub_->on_deactivate(); + + // Destroying bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +VectorObjectServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + add_shapes_service_.reset(); + get_shapes_service_.reset(); + remove_shapes_service_.reset(); + + map_pub_.reset(); + map_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + + return nav2_util::CallbackReturn::SUCCESS; +} + +bool VectorObjectServer::transformVectorObjects() +{ + for (auto shape : shapes_) { + if (shape->getFrameID() != frame_id_ && !shape->getFrameID().empty()) { + // Shape to be updated dynamically + if (!shape->toFrame(frame_id_, tf_buffer_, transform_tolerance_)) { + RCLCPP_ERROR( + get_logger(), "Can not transform vector object from %s to %s frame", + shape->getFrameID().c_str(), frame_id_.c_str()); + return false; + } + } + } + + return true; +} + +void VectorObjectServer::getMapBoundaries( + double & min_x, double & min_y, double & max_x, double & max_y) +{ + min_x = std::numeric_limits::max(); + min_y = std::numeric_limits::max(); + max_x = std::numeric_limits::lowest(); + max_y = std::numeric_limits::lowest(); + + double min_p_x, min_p_y, max_p_x, max_p_y; + + for (auto shape : shapes_) { + shape->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y); + if (min_p_x < min_x) { + min_x = min_p_x; + } + if (min_p_y < min_y) { + min_y = min_p_y; + } + if (max_p_x > max_x) { + max_x = max_p_x; + } + if (max_p_y > max_y) { + max_y = max_p_y; + } + } + + if ( + min_x == std::numeric_limits::max() || + min_y == std::numeric_limits::max() || + max_x == std::numeric_limits::lowest() || + max_y == std::numeric_limits::lowest()) + { + throw std::runtime_error("Can not obtain map boundaries"); + } +} + +void VectorObjectServer::updateMap( + const double & min_x, const double & min_y, const double & max_x, const double & max_y) +{ + // Calculate size of update map + int size_x = static_cast((max_x - min_x) / resolution_) + 1; + int size_y = static_cast((max_y - min_y) / resolution_) + 1; + + if (size_x < 0) { + throw std::runtime_error("Incorrect map x-size"); + } + if (size_y < 0) { + throw std::runtime_error("Incorrect map y-size"); + } + + if (!map_) { + map_ = std::make_shared(); + } + if ( + map_->info.width != static_cast(size_x) || + map_->info.height != static_cast(size_y)) + { + // Map size was changed + map_->data = std::vector(size_x * size_y, default_value_); + map_->info.width = size_x; + map_->info.height = size_y; + } else if (size_x > 0 && size_y > 0) { + // Map size was not changed + memset(map_->data.data(), default_value_, size_x * size_y * sizeof(int8_t)); + } + map_->header.frame_id = frame_id_; + map_->info.resolution = resolution_; + map_->info.origin.position.x = min_x; + map_->info.origin.position.y = min_y; +} + +void VectorObjectServer::putVectorObjectsOnMap() +{ + // Filling the shapes + for (auto shape : shapes_) { + if (shape->isFill()) { + // Put filled shape on map + double wx1 = std::numeric_limits::max(); + double wy1 = std::numeric_limits::max(); + double wx2 = std::numeric_limits::lowest(); + double wy2 = std::numeric_limits::lowest(); + unsigned int mx1 = 0; + unsigned int my1 = 0; + unsigned int mx2 = 0; + unsigned int my2 = 0; + + shape->getBoundaries(wx1, wy1, wx2, wy2); + if ( + !nav2_util::worldToMap(map_, wx1, wy1, mx1, my1) || + !nav2_util::worldToMap(map_, wx2, wy2, mx2, my2)) + { + RCLCPP_ERROR( + get_logger(), + "Error to get shape boundaries on map (UUID: %s)", + shape->getUUID().c_str()); + return; + } + + unsigned int it; + for (unsigned int my = my1; my <= my2; my++) { + for (unsigned int mx = mx1; mx <= mx2; mx++) { + it = my * map_->info.width + mx; + double wx, wy; + nav2_util::mapToWorld(map_, mx, my, wx, wy); + if (shape->isPointInside(wx, wy)) { + processVal(map_->data[it], shape->getValue(), overlay_type_); + } + } + } + } else { + // Put shape borders on map + shape->putBorders(map_, overlay_type_); + } + } +} + +void VectorObjectServer::publishMap() +{ + if (map_) { + auto map = std::make_unique(*map_); + map_pub_->publish(std::move(map)); + } +} + +void VectorObjectServer::processMap() +{ + if (!process_map_) { + return; + } + + try { + if (shapes_.size()) { + if (!transformVectorObjects()) { + return; + } + double min_x, min_y, max_x, max_y; + + getMapBoundaries(min_x, min_y, max_x, max_y); + updateMap(min_x, min_y, max_x, max_y); + putVectorObjectsOnMap(); + } else { + updateMap(0.0, 0.0, 0.0, 0.0); + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Can not upate map: %s", ex.what()); + return; + } + + publishMap(); +} + +bool VectorObjectServer::isMapUpdate() +{ + for (auto shape : shapes_) { + if (shape->getFrameID() != frame_id_ && !shape->getFrameID().empty()) { + return true; + } + } + + return false; +} + +void VectorObjectServer::switchMapUpdate() +{ + if (isMapUpdate()) { + if (!map_timer_) { + map_timer_ = this->create_timer( + std::chrono::duration(1.0 / update_frequency_), + std::bind(&VectorObjectServer::processMap, this)); + RCLCPP_INFO(get_logger(), "Publishing map dynamically"); + } + } else { + if (map_timer_) { + map_timer_->cancel(); + map_timer_.reset(); + } + RCLCPP_INFO(get_logger(), "Publishing map once"); + processMap(); + } +} + +void VectorObjectServer::addShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // Initialize result with true. If one of the required vector object was not added properly, + // set it to false. + response->success = true; + + auto node = shared_from_this(); + + // Process polygons + for (auto req_poly : request->polygons) { + nav2_msgs::msg::PolygonVO::SharedPtr new_params = + std::make_shared(req_poly); + + bool new_shape = true; // Whether to add a new shape + std::shared_ptr polygon; + for (auto shape : shapes_) { + if (shape->isUUID(new_params->uuid.uuid.data())) { + // Vector Object with given UUID was found: updating it + new_shape = false; + + // Check that found shape has correct type + if (shape->getType() != POLYGON) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a polygon type", + shape->getUUID().c_str()); + response->success = false; + // Do not add this shape + break; + } + + polygon = std::static_pointer_cast(shape); + + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::PolygonVO::SharedPtr old_params = polygon->getParams(); + try { + polygon->setParams(new_params); + } catch (const std::exception & ex) { + // Restore old parameters + polygon->setParams(old_params); + // ... and report the problem + RCLCPP_ERROR(get_logger(), "Can not update polygon: %s", ex.what()); + response->success = false; + } + break; + } + } + + if (new_shape) { + // Creating new polygon + try { + polygon = std::make_shared(node, new_params); + shapes_.push_back(polygon); + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Can not add polygon: %s", ex.what()); + response->success = false; + } + } + } + + // Process circles + std::shared_ptr circle; + for (auto req_crcl : request->circles) { + nav2_msgs::msg::CircleVO::SharedPtr new_params = + std::make_shared(req_crcl); + + bool new_shape = true; // Whether to add a new shape + for (auto shape : shapes_) { + if (shape->isUUID(new_params->uuid.uuid.data())) { + // Vector object with given UUID was found: updating it + new_shape = false; + + // Check that found shape has correct type + if (shape->getType() != CIRCLE) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a circle type", + shape->getUUID().c_str()); + response->success = false; + break; + } + + circle = std::static_pointer_cast(shape); + + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::CircleVO::SharedPtr old_params = circle->getParams(); + try { + circle->setParams(new_params); + } catch (const std::exception & ex) { + // Restore old parameters + circle->setParams(old_params); + // ... and report the problem + RCLCPP_ERROR(get_logger(), "Can not update circle: %s", ex.what()); + response->success = false; + } + break; + } + } + + if (new_shape) { + // Creating new circle + try { + circle = std::make_shared(node, new_params); + shapes_.push_back(circle); + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Can not add circle: %s", ex.what()); + response->success = false; + } + } + } + + switchMapUpdate(); +} + +void VectorObjectServer::getShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + std::shared_ptr polygon; + std::shared_ptr circle; + + for (auto shape : shapes_) { + switch (shape->getType()) { + case POLYGON: + polygon = std::static_pointer_cast(shape); + response->polygons.push_back(*(polygon->getParams())); + break; + case CIRCLE: + circle = std::static_pointer_cast(shape); + response->circles.push_back(*(circle->getParams())); + break; + default: + RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape->getUUID().c_str()); + } + } +} + +void VectorObjectServer::removeShapesCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // Initialize result with true. If one of the required vector object was not found, + // set it to false. + response->success = true; + + for (auto req_uuid : request->uuids) { + bool found = false; + + for ( + std::vector>::iterator it = shapes_.begin(); + it != shapes_.end(); + it++) + { + if ((*it)->isUUID(req_uuid.uuid.data())) { + // Polygon with given UUID was found: remove it + (*it).reset(); + shapes_.erase(it); + found = true; + + break; + } + } + + if (!found) { + // Required vector object was not found + char uuid_str[37]; + uuid_unparse(req_uuid.uuid.data(), uuid_str); + RCLCPP_ERROR(get_logger(), "Can not find shape to remove with UUID: %s", uuid_str); + response->success = false; + } + } + + switchMapUpdate(); +} + +bool VectorObjectServer::getROSParameters() +{ + // Main ROS-parameters + map_topic_ = getROSParameter( + shared_from_this(), "map_topic", "vo_map").as_string(); + frame_id_ = getROSParameter( + shared_from_this(), "frame_id", "map").as_string(); + resolution_ = getROSParameter( + shared_from_this(), "resolution", 0.05).as_double(); + default_value_ = getROSParameter( + shared_from_this(), "default_value", nav2_util::OCC_GRID_UNKNOWN).as_int(); + overlay_type_ = static_cast(getROSParameter( + shared_from_this(), "overlay_type", static_cast(OverlayType::OVERLAY_SEQ)).as_int()); + update_frequency_ = getROSParameter( + shared_from_this(), "update_frequency", 1.0).as_double(); + transform_tolerance_ = getROSParameter( + shared_from_this(), "transform_tolerance", 0.1).as_double(); + + // Shapes + std::vector shape_names = getROSParameter( + shared_from_this(), "shapes", std::vector()).as_string_array(); + for (std::string shape_name : shape_names) { + std::string shape_type; + + try { + shape_type = getROSParameter( + shared_from_this(), shape_name + ".type", rclcpp::PARAMETER_STRING).as_string(); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + get_logger(), + "Error while getting shape %s type: %s", + shape_name.c_str(), ex.what()); + return false; + } + + if (shape_type == "polygon") { + try { + std::shared_ptr polygon = std::make_shared(shared_from_this()); + if (!polygon->getROSParameters(shape_name)) { + return false; + } + shapes_.push_back(polygon); + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Can not create new polygon: %s", ex.what()); + return false; + } + } else if (shape_type == "circle") { + try { + std::shared_ptr circle = std::make_shared(shared_from_this()); + + if (!circle->getROSParameters(shape_name)) { + return false; + } + shapes_.push_back(circle); + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Can not create new circle: %s", ex.what()); + return false; + } + } else { + RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str()); + } + } + + return true; +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/src/vo_server/vector_object_server_node.cpp b/nav2_map_server/src/vo_server/vector_object_server_node.cpp new file mode 100644 index 00000000000..b91bacc0f2c --- /dev/null +++ b/nav2_map_server/src/vo_server/vector_object_server_node.cpp @@ -0,0 +1,29 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// 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 + +#include "rclcpp/rclcpp.hpp" + +#include "nav2_map_server/vector_object_server.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp new file mode 100644 index 00000000000..c4b97dbffeb --- /dev/null +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -0,0 +1,579 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_map_server/vector_object_shapes.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "nav2_util/occ_grid_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/raytrace_line_2d.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace nav2_map_server +{ + +// ---------- Shape ---------- + +Shape::Shape(const nav2_util::LifecycleNode::WeakPtr & node) +: type_(UNKNOWN), node_(node) +{} + +Shape::~Shape() +{} + +ShapeType Shape::getType() +{ + return type_; +} + +bool Shape::getShapeUUID(const std::string & shape_name, unsigned char * out_uuid) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + try { + // Try to get shape UUID from ROS-parameters + std::string uuid_str = getROSParameter( + node, shape_name + ".uuid", rclcpp::PARAMETER_STRING).as_string(); + if (uuid_parse(uuid_str.c_str(), out_uuid)) { + RCLCPP_ERROR( + node->get_logger(), + "Can not parse UUID string for %s shape: %s", + shape_name.c_str(), uuid_str.c_str()); + return false; + } + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + // If no UUID was specified, generate a new one + uuid_generate(out_uuid); + + char uuid_str[37]; + uuid_unparse(out_uuid, uuid_str); + RCLCPP_INFO( + node->get_logger(), + "No UUID is specified for %s shape. Generating a new one: %s", + shape_name.c_str(), uuid_str); + } + + return true; +} + +// ---------- Polygon ---------- + +Polygon::Polygon( + const nav2_util::LifecycleNode::WeakPtr & node, + const nav2_msgs::msg::PolygonVO::SharedPtr params) +: Shape::Shape(node) +{ + type_ = POLYGON; + + if (!params) { + params_ = std::make_shared(); + } else { + params_ = params; + checkConsistency(); + } + if (!polygon_) { + polygon_ = std::make_shared(); + } + polygon_->points = params_->points; + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } +} + +bool Polygon::getROSParameters(const std::string & shape_name) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + params_->header.frame_id = getROSParameter( + node, shape_name + ".frame_id", "map").as_string(); + params_->value = getROSParameter( + node, shape_name + ".value", nav2_util::OCC_GRID_OCCUPIED).as_int(); + + params_->closed = getROSParameter( + node, shape_name + ".closed", true).as_bool(); + + std::vector poly_row; + try { + poly_row = getROSParameter( + node, shape_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY).as_double_array(); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + node->get_logger(), + "Error while getting polygon %s parameters: %s", + shape_name.c_str(), ex.what()); + return false; + } + // Check for points format correctness + if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { + RCLCPP_ERROR( + node->get_logger(), + "Polygon %s has incorrect points description", + shape_name.c_str()); + return false; + } + + // Obtain polygon vertices + geometry_msgs::msg::Point32 point; + bool first = true; + for (double val : poly_row) { + if (first) { + point.x = val; + } else { + point.y = val; + params_->points.push_back(point); + } + first = !first; + } + + // Filling the polygon_ with obtained points in map's frame + polygon_->points = params_->points; + + // Getting shape UUID + return getShapeUUID(shape_name, params_->uuid.uuid.data()); +} + +void Polygon::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) +{ + min_x = std::numeric_limits::max(); + min_y = std::numeric_limits::max(); + max_x = std::numeric_limits::lowest(); + max_y = std::numeric_limits::lowest(); + + for (auto point : polygon_->points) { + if (point.x < min_x) { + min_x = point.x; + } + if (point.y < min_y) { + min_y = point.y; + } + if (point.x > max_x) { + max_x = point.x; + } + if (point.y > max_y) { + max_y = point.y; + } + } +} + +bool Polygon::isPointInside(const double px, const double py) const +{ + // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." + // Communications of the ACM 5.8 (1962): 434. + // Implementation of ray crossings algorithm for point in polygon task solving. + // Y coordinate is fixed. Moving the ray on X+ axis starting from given point. + // Odd number of intersections with polygon boundaries means the point is inside polygon. + const int points_num = polygon_->points.size(); + int i, j; // Polygon vertex iterators + bool res = false; // Final result, initialized with already inverted value + + // Starting from the edge where the last point of polygon is connected to the first + i = points_num - 1; + for (j = 0; j < points_num; j++) { + // Checking the edge only if given point is between edge boundaries by Y coordinates. + // One of the condition should contain equality in order to exclude the edges + // parallel to X+ ray. + if ((py <= polygon_->points[i].y) == (py > polygon_->points[j].y)) { + // Calculating the intersection coordinate of X+ ray + const double x_inter = polygon_->points[i].x + + (py - polygon_->points[i].y) * + (polygon_->points[j].x - polygon_->points[i].x) / + (polygon_->points[j].y - polygon_->points[i].y); + // If intersection with checked edge is greater than point x coordinate, + // inverting the result + if (x_inter > px) { + res = !res; + } + } + i = j; + } + return res; +} + +void Polygon::putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) +{ + unsigned int mx0, my0, mx1, my1; + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!nav2_util::worldToMap(map, polygon_->points[0].x, polygon_->points[0].y, mx1, my1)) { + RCLCPP_ERROR( + node->get_logger(), + "Can not convert (%f, %f) point to map", + polygon_->points[0].x, polygon_->points[0].y); + return; + } + + MapAction ma(map, params_->value, overlay_type); + for (unsigned int i = 1; i < polygon_->points.size(); i++) { + mx0 = mx1; + my0 = my1; + if (!nav2_util::worldToMap(map, polygon_->points[i].x, polygon_->points[i].y, mx1, my1)) { + RCLCPP_ERROR( + node->get_logger(), + "Can not convert (%f, %f) point to map", + polygon_->points[i].x, polygon_->points[i].y); + return; + } + nav2_util::raytraceLine(ma, mx0, my0, mx1, my1, map->info.width); + } +} + +nav2_msgs::msg::PolygonVO::SharedPtr Polygon::getParams() const +{ + return params_; +} + +void Polygon::setParams(const nav2_msgs::msg::PolygonVO::SharedPtr params) +{ + params_ = params; + checkConsistency(); + if (!polygon_) { + polygon_ = std::make_shared(); + } + polygon_->points = params_->points; +} + +int8_t Polygon::getValue() const +{ + return params_->value; +} + +std::string Polygon::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Polygon::getUUID() const +{ + char uuid_str[37]; + uuid_unparse(params_->uuid.uuid.data(), uuid_str); + return std::string(uuid_str); +} + +bool Polygon::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Polygon::isFill() const +{ + return params_->closed; +} + +bool Polygon::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + for (unsigned int i = 0; i < params_->points.size(); i++) { + from_pose.pose.position.x = params_->points[i].x; + from_pose.pose.position.y = params_->points[i].y; + from_pose.pose.position.z = params_->points[i].z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + polygon_->points[i].x = to_pose.pose.position.x; + polygon_->points[i].y = to_pose.pose.position.y; + polygon_->points[i].z = to_pose.pose.position.z; + } else { + return false; + } + } + + return true; +} + +void Polygon::checkConsistency() +{ + if (params_->points.size() < 3) { + throw std::runtime_error("Polygon has incorrect number of vertices"); + } +} + +// ---------- Circle ---------- + +Circle::Circle( + const nav2_util::LifecycleNode::WeakPtr & node, + const nav2_msgs::msg::CircleVO::SharedPtr params) +: Shape::Shape(node) +{ + type_ = CIRCLE; + + if (!params) { + params_ = std::make_shared(); + } else { + params_ = params; + } + if (!center_) { + center_ = std::make_shared(); + } + *center_ = params_->center; + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } + + checkConsistency(); +} + +bool Circle::getROSParameters(const std::string & shape_name) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + params_->header.frame_id = getROSParameter( + node, shape_name + ".frame_id", "map").as_string(); + params_->value = getROSParameter( + node, shape_name + ".value", nav2_util::OCC_GRID_OCCUPIED).as_int(); + + params_->fill = getROSParameter( + node, shape_name + ".fill", true).as_bool(); + + std::vector center_row; + try { + center_row = getROSParameter( + node, shape_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY).as_double_array(); + params_->radius = getROSParameter( + node, shape_name + ".radius", rclcpp::PARAMETER_DOUBLE).as_double(); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + node->get_logger(), + "Error while getting circle %s parameters: %s", + shape_name.c_str(), ex.what()); + return false; + } + // Check for points format correctness + if (center_row.size() != 2) { + RCLCPP_ERROR( + node->get_logger(), + "Circle %s has incorrect center description", + shape_name.c_str()); + return false; + } + + // Obtain circle center + params_->center.x = center_row[0]; + params_->center.y = center_row[1]; + // Setting the center_ with obtained circle center in map's frame + *center_ = params_->center; + + // Getting shape UUID + return getShapeUUID(shape_name, params_->uuid.uuid.data()); +} + +// Get/update shape boundaries +void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) +{ + min_x = center_->x - params_->radius; + min_y = center_->y - params_->radius; + max_x = center_->x + params_->radius; + max_y = center_->y + params_->radius; +} + +bool Circle::isPointInside(const double px, const double py) const +{ + return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <= + params_->radius * params_->radius; +} + +bool Circle::centerToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + unsigned int & mcx, unsigned int & mcy) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Get center of circle in map coordinates + if (center_->x < map->info.origin.position.x || center_->y < map->info.origin.position.y) { + RCLCPP_ERROR( + node->get_logger(), + "Can not convert (%f, %f) circle center to map", + center_->x, center_->y); + return false; + } + // We need the circle center to be always shifted one cell less its logical center + // and to avoid any FP-accuracy loosing on small values, so we are using another + // than nav2_util::worldToMap() approach + mcx = static_cast( + std::round((center_->x - map->info.origin.position.x) / map->info.resolution)) - 1; + mcy = static_cast( + std::round((center_->y - map->info.origin.position.y) / map->info.resolution)) - 1; + if (mcx >= map->info.width || mcy >= map->info.height) { + RCLCPP_ERROR( + node->get_logger(), "Can not convert (%f, %f) point to map", center_->x, center_->y); + return false; + } + + return true; +} + +inline void Circle::putPoint( + unsigned int mx, unsigned int my, + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const OverlayType overlay_type) +{ + fillMap(map, my * map->info.width + mx, params_->value, overlay_type); +} + +// Put params_gons line borders on map +void Circle::putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) +{ + unsigned int mcx, mcy; + if (!centerToMap(map, mcx, mcy)) { + return; + } + + // Implementation of the circle generation algorithm, based on the following work: + // Berthold K.P. Horn "Circle generators for display devices" + // Computer Graphics and Image Processing 5.2 (1976): 280-288. + + // Inputs initialization + const int r = static_cast(std::round(params_->radius / map->info.resolution)); + int x = r; + int y = 1; + + // Error initialization + int s = -r; + + // Calculation algorithm + while (x > y) { // Calculating only first circle octant + // Put 8 points in each octant reflecting symmetrically + putPoint(mcx + x, mcy + y, map, overlay_type); + putPoint(mcx + y, mcy + x, map, overlay_type); + putPoint(mcx - x + 1, mcy + y, map, overlay_type); + putPoint(mcx + y, mcy - x + 1, map, overlay_type); + putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type); + putPoint(mcx - y + 1, mcy - x + 1, map, overlay_type); + putPoint(mcx + x, mcy - y + 1, map, overlay_type); + putPoint(mcx - y + 1, mcy + x, map, overlay_type); + + s = s + 2 * y + 1; + y++; + if (s > 0) { + s = s - 2 * x + 2; + x--; + } + } + + // Corner case for x == y: do not put end points twice + if (x == y) { + putPoint(mcx + x, mcy + y, map, overlay_type); + putPoint(mcx - x + 1, mcy + y, map, overlay_type); + putPoint(mcx - x + 1, mcy - y + 1, map, overlay_type); + putPoint(mcx + x, mcy - y + 1, map, overlay_type); + } +} + +nav2_msgs::msg::CircleVO::SharedPtr Circle::getParams() const +{ + return params_; +} + +void Circle::setParams(const nav2_msgs::msg::CircleVO::SharedPtr params) +{ + params_ = params; + if (!center_) { + center_ = std::make_shared(); + } + *center_ = params_->center; + checkConsistency(); +} + +int8_t Circle::getValue() const +{ + return params_->value; +} + +std::string Circle::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Circle::getUUID() const +{ + char uuid_str[37]; + uuid_unparse(params_->uuid.uuid.data(), uuid_str); + return std::string(uuid_str); +} + +bool Circle::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Circle::isFill() const +{ + return params_->fill; +} + +bool Circle::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + from_pose.pose.position.x = params_->center.x; + from_pose.pose.position.y = params_->center.y; + from_pose.pose.position.z = params_->center.z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + center_->x = to_pose.pose.position.x; + center_->y = to_pose.pose.position.y; + center_->z = to_pose.pose.position.z; + } else { + return false; + } + + return true; +} + +void Circle::checkConsistency() +{ + if (params_->radius < 0.0) { + throw std::runtime_error("Circle has incorrect radius less than zero"); + } +} + +} // namespace nav2_map_server diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index d0ff3527753..2798180d2f3 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) find_package(geographic_msgs) find_package(action_msgs REQUIRED) +find_package(unique_identifier_msgs REQUIRED) nav2_package() @@ -30,6 +31,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" + "msg/PolygonVO.msg" + "msg/CircleVO.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" @@ -40,6 +43,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveMap.srv" "srv/SetInitialPose.srv" "srv/ReloadDockDatabase.srv" + "srv/AddShapes.srv" + "srv/RemoveShapes.srv" + "srv/GetShapes.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" @@ -56,7 +62,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/FollowGPSWaypoints.action" "action/DockRobot.action" "action/UndockRobot.action" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs unique_identifier_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/nav2_msgs/msg/CircleVO.msg b/nav2_msgs/msg/CircleVO.msg new file mode 100644 index 00000000000..4525d46f946 --- /dev/null +++ b/nav2_msgs/msg/CircleVO.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +geometry_msgs/Point32 center +float32 radius +bool fill +int8 value +unique_identifier_msgs/UUID uuid diff --git a/nav2_msgs/msg/PolygonVO.msg b/nav2_msgs/msg/PolygonVO.msg new file mode 100644 index 00000000000..c57b69fcd7a --- /dev/null +++ b/nav2_msgs/msg/PolygonVO.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +geometry_msgs/Point32[] points +bool closed +int8 value +unique_identifier_msgs/UUID uuid diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 48e555532b6..b5c57aa418f 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -20,6 +20,7 @@ action_msgs nav_msgs geographic_msgs + unique_identifier_msgs rosidl_interface_packages diff --git a/nav2_msgs/srv/AddShapes.srv b/nav2_msgs/srv/AddShapes.srv new file mode 100644 index 00000000000..dda8235de13 --- /dev/null +++ b/nav2_msgs/srv/AddShapes.srv @@ -0,0 +1,6 @@ +# Add/update vector objects on map + +CircleVO[] circles +PolygonVO[] polygons +--- +bool success diff --git a/nav2_msgs/srv/GetShapes.srv b/nav2_msgs/srv/GetShapes.srv new file mode 100644 index 00000000000..15dcf18c03a --- /dev/null +++ b/nav2_msgs/srv/GetShapes.srv @@ -0,0 +1,5 @@ +# Get vector objects which are now being applied on map + +--- +CircleVO[] circles +PolygonVO[] polygons diff --git a/nav2_msgs/srv/RemoveShapes.srv b/nav2_msgs/srv/RemoveShapes.srv new file mode 100644 index 00000000000..be91b3535a6 --- /dev/null +++ b/nav2_msgs/srv/RemoveShapes.srv @@ -0,0 +1,5 @@ +# Remove vector objects from map + +unique_identifier_msgs/UUID[] uuids +--- +bool success diff --git a/nav2_util/include/nav2_util/occ_grid_utils.hpp b/nav2_util/include/nav2_util/occ_grid_utils.hpp new file mode 100644 index 00000000000..1e1ebc766b8 --- /dev/null +++ b/nav2_util/include/nav2_util/occ_grid_utils.hpp @@ -0,0 +1,129 @@ +// Copyright (c) 2008, 2013, Willow Garage, Inc. +// Copyright (c) 2023 Samsung R&D Institute Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Eitan Marder-Eppstein +// David V. Lu!! +// Alexey Merzlyakov + +#ifndef NAV2_UTIL__OCC_GRID_UTILS_HPP_ +#define NAV2_UTIL__OCC_GRID_UTILS_HPP_ + +#include "nav_msgs/msg/occupancy_grid.hpp" + +namespace nav2_util +{ + +/** + * @brief Get the data of a cell in the OccupancyGrid map + * @param map OccupancyGrid map to get the data from + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @return The data of the selected cell + */ +inline int8_t getMapData( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + const unsigned int mx, const unsigned int my) +{ + return map->data[my * map->info.width + mx]; +} + +/** + * @brief Set the data of a cell in the OccupancyGrid map to a given value + * @param map OccupancyGrid map to get the data from + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @param value The value to set map cell to + */ +inline void setMapData( + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const unsigned int mx, const unsigned int my, const int8_t value) +{ + map->data[my * map->info.width + mx] = value; +} + + +/** + * @brief: Convert from world coordinates to map coordinates. + Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. + * @param map OccupancyGrid map on which to convert + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ +inline bool worldToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + const double wx, const double wy, unsigned int & mx, unsigned int & my) +{ + const double origin_x = map->info.origin.position.x; + const double origin_y = map->info.origin.position.y; + const double resolution = map->info.resolution; + const unsigned int size_x = map->info.width; + const unsigned int size_y = map->info.height; + + if (wx < origin_x || wy < origin_y) { + return false; + } + + mx = static_cast((wx - origin_x) / resolution); + my = static_cast((wy - origin_y) / resolution); + if (mx >= size_x || my >= size_y) { + return false; + } + + return true; +} + +/** + * @brief Convert from map coordinates to world coordinates + * @param mx The x map coordinate + * @param my The y map coordinate + * @param wx Will be set to the associated world x coordinate + * @param wy Will be set to the associated world y coordinate + */ +inline void mapToWorld( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + const unsigned int mx, const unsigned int my, double & wx, double & wy) +{ + const double origin_x = map->info.origin.position.x; + const double origin_y = map->info.origin.position.y; + const double resolution = map->info.resolution; + + wx = origin_x + (mx + 0.5) * resolution; + wy = origin_y + (my + 0.5) * resolution; +} + +} // namespace nav2_util + +#endif // NAV2_UTIL__OCC_GRID_UTILS_HPP_ diff --git a/nav2_util/include/nav2_util/raytrace_line_2d.hpp b/nav2_util/include/nav2_util/raytrace_line_2d.hpp new file mode 100644 index 00000000000..381846afcb0 --- /dev/null +++ b/nav2_util/include/nav2_util/raytrace_line_2d.hpp @@ -0,0 +1,146 @@ +// Copyright (c) 2008, 2013, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Eitan Marder-Eppstein +// David V. Lu!! + +#ifndef NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ +#define NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ + +#include +#include +#include + +namespace nav2_util +{ + +/** + * @brief get the sign of an int + */ +inline int sign(int x) +{ + return x > 0 ? 1.0 : -1.0; +} + +/** + * @brief A 2D implementation of Bresenham's raytracing algorithm... + * applies an action at each step + */ +template +inline void bresenham2D( + ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, + int offset_a, int offset_b, unsigned int offset, + unsigned int max_length) +{ + unsigned int end = std::min(max_length, abs_da); + for (unsigned int i = 0; i < end; ++i) { + at(offset); + offset += offset_a; + error_b += abs_db; + if ((unsigned int)error_b >= abs_da) { + offset += offset_b; + error_b -= abs_da; + } + } + at(offset); +} + +/** + * @brief Raytrace a line and apply some action at each step + * @param at The action to take... a functor + * @param x0 The starting x coordinate + * @param y0 The starting y coordinate + * @param x1 The ending x coordinate + * @param y1 The ending y coordinate + * @param step_x OX-step on map + * @param max_length The maximum desired length of the segment... + * allows you to not go all the way to the endpoint + * @param min_length The minimum desired length of the segment + */ +template +inline void raytraceLine( + ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, unsigned int step_x, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) +{ + int dx_full = x1 - x0; + int dy_full = y1 - y0; + + // we need to chose how much to scale our dominant dimension, + // based on the maximum length of the line + double dist = std::hypot(dx_full, dy_full); + if (dist < min_length) { + return; + } + + unsigned int min_x0, min_y0; + if (dist > 0.0) { + // Adjust starting point and offset to start from min_length distance + min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); + min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); + } else { + // dist can be 0 if [x0, y0]==[x1, y1]. + // In this case only this cell should be processed. + min_x0 = x0; + min_y0 = y0; + } + unsigned int offset = min_y0 * step_x + min_x0; + + int dx = x1 - min_x0; + int dy = y1 - min_y0; + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + + int offset_dx = sign(dx); + int offset_dy = sign(dy) * step_x; + + double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); + // if x is dominant + if (abs_dx >= abs_dy) { + int error_y = abs_dx / 2; + + bresenham2D( + at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx)); + return; + } + + // otherwise y is dominant + int error_x = abs_dy / 2; + + bresenham2D( + at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); +} + +} // namespace nav2_util + +#endif // NAV2_UTIL__RAYTRACE_LINE_2D_HPP_ diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 4c0f2747210..12b4254ae60 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -1,3 +1,5 @@ +add_subdirectory(regression) + ament_add_gtest(test_execution_timer test_execution_timer.cpp) ament_add_gtest(test_node_utils test_node_utils.cpp) diff --git a/nav2_util/test/regression/CMakeLists.txt b/nav2_util/test/regression/CMakeLists.txt new file mode 100644 index 00000000000..033741a19c4 --- /dev/null +++ b/nav2_util/test/regression/CMakeLists.txt @@ -0,0 +1,2 @@ +# Bresenham2D corner cases test +ament_add_gtest(map_bresenham_2d map_bresenham_2d.cpp) diff --git a/nav2_util/test/regression/map_bresenham_2d.cpp b/nav2_util/test/regression/map_bresenham_2d.cpp new file mode 100644 index 00000000000..684b3429e4f --- /dev/null +++ b/nav2_util/test/regression/map_bresenham_2d.cpp @@ -0,0 +1,167 @@ +// Copyright (c) 2023 Samsung R&D Institute Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Alexey Merzlyakov + +#include + +#include + +#include "nav2_util/raytrace_line_2d.hpp" + +class MapAction +{ +public: + explicit MapAction( + char * map, unsigned int size, char mark_val = 100) + : map_(map), size_(size), mark_val_(mark_val) + { + } + + inline void operator()(unsigned int off) + { + ASSERT_TRUE(off < size_); + map_[off] = mark_val_; + } + + inline unsigned int get(unsigned int off) + { + return map_[off]; + } + +private: + char * map_; + unsigned int size_; + char mark_val_; +}; + +class MapTest +{ +public: + MapTest( + unsigned int size_x, unsigned int size_y, + char default_val = 0) + : size_x_(size_x), size_y_(size_y) + { + map_ = new char[size_x * size_y]; + memset(map_, default_val, size_x * size_y); + } + + char * getMap() + { + return map_; + } + + unsigned int getSize() + { + return size_x_ * size_y_; + } + + void raytraceLine( + MapAction ma, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) + { + nav2_util::raytraceLine(ma, x0, y0, x1, y1, size_x_, max_length, min_length); + } + +private: + char * map_; + unsigned int size_x_, size_y_; +}; + +TEST(map_2d, bresenham2DBoundariesCheck) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 6; + MapTest mt(sz_x, sz_y); + MapAction ma(mt.getMap(), mt.getSize()); + + // Initial point - some assymetrically standing point in order to cover most corner cases + const unsigned int x0 = 2; + const unsigned int y0 = 4; + // (x1, y1) point will move + unsigned int x1, y1; + + // Running on (x, 0) edge + y1 = 0; + for (x1 = 0; x1 < sz_x; x1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (x, sz_y) edge + y1 = sz_y - 1; + for (x1 = 0; x1 < sz_x; x1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (0, y) edge + x1 = 0; + for (y1 = 0; y1 < sz_y; y1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (sz_x, y) edge + x1 = sz_x - 1; + for (y1 = 0; y1 < sz_y; y1++) { + mt.raytraceLine(ma, x0, y0, x1, y1, max_length, min_length); + } +} + +TEST(map_2d, bresenham2DSamePoint) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 0; + MapTest mt(sz_x, sz_y, 0.1); + MapAction ma(mt.getMap(), mt.getSize()); + + // Initial point + const double x0 = 2; + const double y0 = 4; + + unsigned int offset = y0 * sz_x + x0; + char val_before = ma.get(offset); + // Same point to check + mt.raytraceLine(ma, x0, y0, x0, y0, max_length, min_length); + char val_after = ma.get(offset); + ASSERT_FALSE(val_before == val_after); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}