From d2cabdf967b10be4170fb46a07766be17309f742 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 22 Feb 2024 13:01:43 +0300 Subject: [PATCH 01/19] Add Vector Object server Signed-off-by: Alexey Merzlyakov --- .../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 | 15 +- 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, 2672 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 04b9f4daa4..0167cddc69 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -416,105 +416,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 d1128b5ccc..e72649b4b9 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 78b761e8d3..834874c2fc 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 f05726a6b3..f8b3f43523 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 3ac785296a..450d1965f2 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 ac30accf24..10082ba922 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 cccae4d393..55ad4d511f 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 8914166a5e..e5d55ccacf 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 { @@ -415,14 +416,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 aafc4d561b..46f978859a 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 a45cb38a7f..0000000000 --- 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 e5416845cc..d7eb87da3a 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 0a43c13696..6102f49201 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -16,6 +16,8 @@ find_package(tf2 REQUIRED) find_package(nav2_util REQUIRED) find_package(GRAPHICSMAGICKCPP REQUIRED) +pkg_search_module(UUID REQUIRED uuid) + nav2_package() include_directories(include) @@ -28,6 +30,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) @@ -40,6 +44,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) @@ -76,6 +85,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}) @@ -94,6 +110,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}) @@ -125,6 +145,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") @@ -137,13 +161,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 0000000000..d28582a110 --- /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 0000000000..f8252cb513 --- /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 0000000000..5065448aa6 --- /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 0000000000..ce0f867e93 --- /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 0000000000..089e8316e3 --- /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 0000000000..72f100383e --- /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 0000000000..b91bacc0f2 --- /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 0000000000..c4b97dbffe --- /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 206ae86322..074bbd01f2 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" @@ -38,6 +41,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ManageLifecycleNodes.srv" "srv/LoadMap.srv" "srv/SaveMap.srv" + "srv/AddShapes.srv" + "srv/RemoveShapes.srv" + "srv/GetShapes.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" @@ -52,7 +58,14 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/DummyBehavior.action" "action/FollowWaypoints.action" "action/FollowGPSWaypoints.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 0000000000..4525d46f94 --- /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 0000000000..c57b69fcd7 --- /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 48e555532b..b5c57aa418 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 0000000000..dda8235de1 --- /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 0000000000..15dcf18c03 --- /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 0000000000..be91b3535a --- /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 0000000000..1e1ebc766b --- /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 0000000000..381846afcb --- /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 fb4b6c107f..4b9e300cf9 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 0000000000..033741a19c --- /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 0000000000..684b3429e4 --- /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(); +} From 726eac866be361048666ff7b8b26224ae0016f62 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 9 Nov 2023 14:51:51 +0300 Subject: [PATCH 02/19] Meet review comments Signed-off-by: Alexey Merzlyakov --- .../nav2_map_server/vector_object_server.hpp | 10 +- .../nav2_map_server/vector_object_shapes.hpp | 34 +-- .../nav2_map_server/vector_object_utils.hpp | 14 + nav2_map_server/package.xml | 1 + .../params/vector_object_server_params.yaml | 2 +- .../src/vo_server/vector_object_server.cpp | 254 +++++++++--------- .../src/vo_server/vector_object_shapes.cpp | 34 ++- nav2_msgs/CMakeLists.txt | 4 +- .../msg/{CircleVO.msg => CircleObject.msg} | 2 +- .../msg/{PolygonVO.msg => PolygonObject.msg} | 2 +- nav2_msgs/srv/AddShapes.srv | 4 +- nav2_msgs/srv/GetShapes.srv | 4 +- nav2_msgs/srv/RemoveShapes.srv | 1 + 13 files changed, 182 insertions(+), 184 deletions(-) rename nav2_msgs/msg/{CircleVO.msg => CircleObject.msg} (100%) rename nav2_msgs/msg/{PolygonVO.msg => PolygonObject.msg} (100%) 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 index d28582a110..3973d5b456 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include "nav_msgs/msg/occupancy_grid.hpp" @@ -178,15 +178,15 @@ class VectorObjectServer : public nav2_util::LifecycleNode * @brief Supporting routine obtaining all ROS-parameters * @return True if all parameters were obtained or false in the failure case */ - bool getROSParameters(); + bool obtainParameters(); /// @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 All {UUID, Shape} vector objects map + std::unordered_map> shapes_; /// @brief Output map resolution double resolution_; @@ -201,7 +201,7 @@ class VectorObjectServer : public nav2_util::LifecycleNode double process_map_; /// @brief Frame of output map - std::string frame_id_; + std::string global_frame_id_; /// @brief Transform tolerance double transform_tolerance_; 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 index f8252cb513..4de44f3006 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -26,8 +26,8 @@ #include "tf2_ros/buffer.h" -#include "nav2_msgs/msg/polygon_vo.hpp" -#include "nav2_msgs/msg/circle_vo.hpp" +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_map_server/vector_object_utils.hpp" @@ -35,7 +35,7 @@ namespace nav2_map_server { -/// @brief Possible VO-shape types +/// @brief Possible vector object shape types enum ShapeType { UNKNOWN = 0, @@ -69,7 +69,7 @@ class Shape * 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); + bool obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid); /** * @brief Supporting routine obtaining ROS-parameters for the given vector object. @@ -77,7 +77,7 @@ class Shape * @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; + virtual bool obtainParameters(const std::string & shape_name) = 0; /** * @brief Gets shape boundaries. @@ -171,20 +171,20 @@ class Polygon : public Shape /* * @brief Polygon class constructor * @param node Vector Object server node pointer - * @param params PolygonVO parameters. In case of nullptr, + * @param params PolygonObject 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); + const nav2_msgs::msg::PolygonObject::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); + bool obtainParameters(const std::string & shape_name); /** * @brief Gets shape boundaries @@ -214,13 +214,13 @@ class Polygon : public Shape * @brief Gets Polygon parameters * @return Polygon parameters */ - nav2_msgs::msg::PolygonVO::SharedPtr getParams() const; + nav2_msgs::msg::PolygonObject::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); + void setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); /** * @brief Gets the value of the shape. @@ -273,7 +273,7 @@ class Polygon : public Shape void checkConsistency(); /// @brief Input polygon parameters (could be in any frame) - nav2_msgs::msg::PolygonVO::SharedPtr params_; + nav2_msgs::msg::PolygonObject::SharedPtr params_; /// @brief Polygon in the map's frame geometry_msgs::msg::Polygon::SharedPtr polygon_; }; @@ -285,20 +285,20 @@ class Circle : public Shape /* * @brief Circle class constructor * @param node Vector Object server node pointer - * @param params CircleVO parameters. In case of nullptr, + * @param params CircleObject 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); + const nav2_msgs::msg::CircleObject::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); + bool obtainParameters(const std::string & shape_name); /** * @brief Gets shape boundaries @@ -328,13 +328,13 @@ class Circle : public Shape * @brief Gets Circle parameters * @return Circle parameters */ - nav2_msgs::msg::CircleVO::SharedPtr getParams() const; + nav2_msgs::msg::CircleObject::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); + void setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); /** * @brief Gets the value of the shape. @@ -412,7 +412,7 @@ class Circle : public Shape const OverlayType overlay_type); /// @brief Input circle parameters (could be in any frame) - nav2_msgs::msg::CircleVO::SharedPtr params_; + nav2_msgs::msg::CircleObject::SharedPtr params_; /// @brief Circle center in the map's frame geometry_msgs::msg::Point32::SharedPtr center_; }; 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 index 5065448aa6..89037bd190 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -28,6 +28,20 @@ namespace nav2_map_server { +// ---------- Working with UUID-s ---------- + +/** + * @beirf Converts input UUID from input array to unparsed string + * @param uuid Input UUID in array format + * @return Unparsed UUID string + */ +inline std::string unparseUUID(const unsigned char * uuid) +{ + char uuid_str[37]; + uuid_unparse(uuid, uuid_str); + return std::string(uuid_str); +} + // ---------- Working with ROS-parameters ---------- /** diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 3f4e287dcd..84779f9fb7 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -25,6 +25,7 @@ nav2_msgs nav2_util graphicsmagick + uuid ament_lint_common ament_lint_auto diff --git a/nav2_map_server/params/vector_object_server_params.yaml b/nav2_map_server/params/vector_object_server_params.yaml index 089e8316e3..31b854c30f 100644 --- a/nav2_map_server/params/vector_object_server_params.yaml +++ b/nav2_map_server/params/vector_object_server_params.yaml @@ -1,7 +1,7 @@ vector_object_server: ros__parameters: map_topic: "vo_map" - frame_id: "map" + global_frame_id: "map" resolution: 0.05 default_value: -1 overlay_type: 0 diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index 72f100383e..9e335d45b6 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -14,7 +14,6 @@ #include "nav2_map_server/vector_object_server.hpp" -#include #include #include #include @@ -47,7 +46,7 @@ VectorObjectServer::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_listener_ = std::make_shared(*tf_buffer_); // Obtaining ROS parameters - if (!getROSParameters()) { + if (!obtainParameters()) { return nav2_util::CallbackReturn::FAILURE; } @@ -55,19 +54,16 @@ VectorObjectServer::on_configure(const rclcpp_lifecycle::State & /*state*/) 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"), + "~/add_shapes", std::bind(&VectorObjectServer::addShapesCallback, this, _1, _2, _3)); get_shapes_service_ = create_service( - service_prefix + std::string("get_shapes"), + "~/get_shapes", std::bind(&VectorObjectServer::getShapesCallback, this, _1, _2, _3)); remove_shapes_service_ = create_service( - service_prefix + std::string("remove_shapes"), + "~/remove_shapes", std::bind(&VectorObjectServer::removeShapesCallback, this, _1, _2, _3)); return nav2_util::CallbackReturn::SUCCESS; @@ -80,13 +76,13 @@ VectorObjectServer::on_activate(const rclcpp_lifecycle::State & /*state*/) map_pub_->on_activate(); - // Creating bond connection - createBond(); - // Trigger map to be published process_map_ = true; switchMapUpdate(); + // Creating bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -95,6 +91,9 @@ VectorObjectServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); + // Destroying bond connection + destroyBond(); + if (map_timer_) { map_timer_->cancel(); map_timer_.reset(); @@ -103,9 +102,6 @@ VectorObjectServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) map_pub_->on_deactivate(); - // Destroying bond connection - destroyBond(); - return nav2_util::CallbackReturn::SUCCESS; } @@ -121,6 +117,11 @@ VectorObjectServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) map_pub_.reset(); map_.reset(); + for (auto shape : shapes_) { + shape.second.reset(); + } + shapes_.clear(); + tf_listener_.reset(); tf_buffer_.reset(); @@ -138,12 +139,12 @@ VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) bool VectorObjectServer::transformVectorObjects() { for (auto shape : shapes_) { - if (shape->getFrameID() != frame_id_ && !shape->getFrameID().empty()) { + if (shape.second->getFrameID() != global_frame_id_ && !shape.second->getFrameID().empty()) { // Shape to be updated dynamically - if (!shape->toFrame(frame_id_, tf_buffer_, transform_tolerance_)) { + if (!shape.second->toFrame(global_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()); + shape.second->getFrameID().c_str(), global_frame_id_.c_str()); return false; } } @@ -163,7 +164,7 @@ void VectorObjectServer::getMapBoundaries( 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); + shape.second->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y); if (min_p_x < min_x) { min_x = min_p_x; } @@ -217,7 +218,7 @@ void VectorObjectServer::updateMap( // 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_->header.frame_id = global_frame_id_; map_->info.resolution = resolution_; map_->info.origin.position.x = min_x; map_->info.origin.position.y = min_y; @@ -227,7 +228,7 @@ void VectorObjectServer::putVectorObjectsOnMap() { // Filling the shapes for (auto shape : shapes_) { - if (shape->isFill()) { + if (shape.second->isFill()) { // Put filled shape on map double wx1 = std::numeric_limits::max(); double wy1 = std::numeric_limits::max(); @@ -238,7 +239,7 @@ void VectorObjectServer::putVectorObjectsOnMap() unsigned int mx2 = 0; unsigned int my2 = 0; - shape->getBoundaries(wx1, wy1, wx2, wy2); + shape.second->getBoundaries(wx1, wy1, wx2, wy2); if ( !nav2_util::worldToMap(map_, wx1, wy1, mx1, my1) || !nav2_util::worldToMap(map_, wx2, wy2, mx2, my2)) @@ -246,7 +247,7 @@ void VectorObjectServer::putVectorObjectsOnMap() RCLCPP_ERROR( get_logger(), "Error to get shape boundaries on map (UUID: %s)", - shape->getUUID().c_str()); + shape.second->getUUID().c_str()); return; } @@ -256,14 +257,14 @@ void VectorObjectServer::putVectorObjectsOnMap() 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_); + if (shape.second->isPointInside(wx, wy)) { + processVal(map_->data[it], shape.second->getValue(), overlay_type_); } } } } else { // Put shape borders on map - shape->putBorders(map_, overlay_type_); + shape.second->putBorders(map_, overlay_type_); } } } @@ -306,7 +307,7 @@ void VectorObjectServer::processMap() bool VectorObjectServer::isMapUpdate() { for (auto shape : shapes_) { - if (shape->getFrameID() != frame_id_ && !shape->getFrameID().empty()) { + if (shape.second->getFrameID() != global_frame_id_ && !shape.second->getFrameID().empty()) { return true; } } @@ -346,49 +347,42 @@ void VectorObjectServer::addShapesCallback( // Process polygons for (auto req_poly : request->polygons) { - nav2_msgs::msg::PolygonVO::SharedPtr new_params = - std::make_shared(req_poly); + nav2_msgs::msg::PolygonObject::SharedPtr new_params = + std::make_shared(req_poly); + + auto shape = shapes_.find(unparseUUID(new_params->uuid.uuid.data())); + if (shape != shapes_.end()) { + // Vector Object with given UUID was found: updating it + // Check that found shape has correct type + if (shape->second->getType() != POLYGON) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a polygon type", + shape->second->getUUID().c_str()); + response->success = false; + // Do not add this shape + continue; + } - 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; - } + std::shared_ptr polygon = std::static_pointer_cast(shape->second); - 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; + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::PolygonObject::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; } - } - - if (new_shape) { + } else { + // Vector Object with given UUID was not found: creating a new one // Creating new polygon try { - polygon = std::make_shared(node, new_params); - shapes_.push_back(polygon); + std::shared_ptr polygon = std::make_shared(node, new_params); + shapes_.insert({polygon->getUUID(), polygon}); } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Can not add polygon: %s", ex.what()); response->success = false; @@ -397,49 +391,43 @@ void VectorObjectServer::addShapesCallback( } // Process circles - std::shared_ptr circle; for (auto req_crcl : request->circles) { - nav2_msgs::msg::CircleVO::SharedPtr new_params = - std::make_shared(req_crcl); + nav2_msgs::msg::CircleObject::SharedPtr new_params = + std::make_shared(req_crcl); + + auto shape = shapes_.find(unparseUUID(new_params->uuid.uuid.data())); + if (shape != shapes_.end()) { + // Vector object with given UUID was found: updating it + // Check that found shape has correct type + if (shape->second->getType() != CIRCLE) { + RCLCPP_ERROR( + get_logger(), + "Shape (UUID: %s) is not a circle type", + shape->second->getUUID().c_str()); + response->success = false; + // Do not add this shape + continue; + } - 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; - } + std::shared_ptr circle = std::static_pointer_cast(shape->second); - 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; + // Preserving old parameters for the case, if new ones to be incorrect + nav2_msgs::msg::CircleObject::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; } - } - - if (new_shape) { + } else { + // Vector Object with given UUID was not found: creating a new one // Creating new circle try { - circle = std::make_shared(node, new_params); - shapes_.push_back(circle); + std::shared_ptr circle = std::make_shared(node, new_params); + shapes_.insert({circle->getUUID(), circle}); } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Can not add circle: %s", ex.what()); response->success = false; @@ -459,17 +447,17 @@ void VectorObjectServer::getShapesCallback( std::shared_ptr circle; for (auto shape : shapes_) { - switch (shape->getType()) { + switch (shape.second->getType()) { case POLYGON: - polygon = std::static_pointer_cast(shape); + polygon = std::static_pointer_cast(shape.second); response->polygons.push_back(*(polygon->getParams())); break; case CIRCLE: - circle = std::static_pointer_cast(shape); + circle = std::static_pointer_cast(shape.second); response->circles.push_back(*(circle->getParams())); break; default: - RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape->getUUID().c_str()); + RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape.second->getUUID().c_str()); } } } @@ -483,43 +471,41 @@ void VectorObjectServer::removeShapesCallback( // 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())) { + if (request->all_objects) { + // Clear all objects + for (auto shape : shapes_) { + shape.second.reset(); + } + shapes_.clear(); + } else { + // Find objects to remove + for (auto req_uuid : request->uuids) { + auto shape = shapes_.find(unparseUUID(req_uuid.uuid.data())); + if (shape != shapes_.end()) { // Polygon with given UUID was found: remove it - (*it).reset(); - shapes_.erase(it); - found = true; - - break; + shape->second.reset(); + shapes_.erase(shape); + } else { + // Required vector object was not found + RCLCPP_ERROR( + get_logger(), + "Can not find shape to remove with UUID: %s", + unparseUUID(req_uuid.uuid.data()).c_str()); + response->success = false; } } - - 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() +bool VectorObjectServer::obtainParameters() { // 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(); + global_frame_id_ = getROSParameter( + shared_from_this(), "global_frame_id", "map").as_string(); resolution_ = getROSParameter( shared_from_this(), "resolution", 0.05).as_double(); default_value_ = getROSParameter( @@ -551,10 +537,10 @@ bool VectorObjectServer::getROSParameters() if (shape_type == "polygon") { try { std::shared_ptr polygon = std::make_shared(shared_from_this()); - if (!polygon->getROSParameters(shape_name)) { + if (!polygon->obtainParameters(shape_name)) { return false; } - shapes_.push_back(polygon); + shapes_.insert({polygon->getUUID(), polygon}); } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Can not create new polygon: %s", ex.what()); return false; @@ -563,10 +549,10 @@ bool VectorObjectServer::getROSParameters() try { std::shared_ptr circle = std::make_shared(shared_from_this()); - if (!circle->getROSParameters(shape_name)) { + if (!circle->obtainParameters(shape_name)) { return false; } - shapes_.push_back(circle); + shapes_.insert({circle->getUUID(), circle}); } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Can not create new circle: %s", ex.what()); return false; diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index c4b97dbffe..2d73a3e005 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -44,7 +44,7 @@ ShapeType Shape::getType() return type_; } -bool Shape::getShapeUUID(const std::string & shape_name, unsigned char * out_uuid) +bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid) { auto node = node_.lock(); if (!node) { @@ -81,13 +81,13 @@ bool Shape::getShapeUUID(const std::string & shape_name, unsigned char * out_uui Polygon::Polygon( const nav2_util::LifecycleNode::WeakPtr & node, - const nav2_msgs::msg::PolygonVO::SharedPtr params) + const nav2_msgs::msg::PolygonObject::SharedPtr params) : Shape::Shape(node) { type_ = POLYGON; if (!params) { - params_ = std::make_shared(); + params_ = std::make_shared(); } else { params_ = params; checkConsistency(); @@ -103,7 +103,7 @@ Polygon::Polygon( } } -bool Polygon::getROSParameters(const std::string & shape_name) +bool Polygon::obtainParameters(const std::string & shape_name) { auto node = node_.lock(); if (!node) { @@ -155,7 +155,7 @@ bool Polygon::getROSParameters(const std::string & shape_name) polygon_->points = params_->points; // Getting shape UUID - return getShapeUUID(shape_name, params_->uuid.uuid.data()); + return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); } void Polygon::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) @@ -248,12 +248,12 @@ void Polygon::putBorders( } } -nav2_msgs::msg::PolygonVO::SharedPtr Polygon::getParams() const +nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const { return params_; } -void Polygon::setParams(const nav2_msgs::msg::PolygonVO::SharedPtr params) +void Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) { params_ = params; checkConsistency(); @@ -275,9 +275,7 @@ std::string Polygon::getFrameID() const std::string Polygon::getUUID() const { - char uuid_str[37]; - uuid_unparse(params_->uuid.uuid.data(), uuid_str); - return std::string(uuid_str); + return unparseUUID(params_->uuid.uuid.data()); } bool Polygon::isUUID(const unsigned char * uuid) const @@ -327,13 +325,13 @@ void Polygon::checkConsistency() Circle::Circle( const nav2_util::LifecycleNode::WeakPtr & node, - const nav2_msgs::msg::CircleVO::SharedPtr params) + const nav2_msgs::msg::CircleObject::SharedPtr params) : Shape::Shape(node) { type_ = CIRCLE; if (!params) { - params_ = std::make_shared(); + params_ = std::make_shared(); } else { params_ = params; } @@ -350,7 +348,7 @@ Circle::Circle( checkConsistency(); } -bool Circle::getROSParameters(const std::string & shape_name) +bool Circle::obtainParameters(const std::string & shape_name) { auto node = node_.lock(); if (!node) { @@ -394,7 +392,7 @@ bool Circle::getROSParameters(const std::string & shape_name) *center_ = params_->center; // Getting shape UUID - return getShapeUUID(shape_name, params_->uuid.uuid.data()); + return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); } // Get/update shape boundaries @@ -503,12 +501,12 @@ void Circle::putBorders( } } -nav2_msgs::msg::CircleVO::SharedPtr Circle::getParams() const +nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const { return params_; } -void Circle::setParams(const nav2_msgs::msg::CircleVO::SharedPtr params) +void Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) { params_ = params; if (!center_) { @@ -530,9 +528,7 @@ std::string Circle::getFrameID() const std::string Circle::getUUID() const { - char uuid_str[37]; - uuid_unparse(params_->uuid.uuid.data(), uuid_str); - return std::string(uuid_str); + return unparseUUID(params_->uuid.uuid.data()); } bool Circle::isUUID(const unsigned char * uuid) const diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 074bbd01f2..60adf7d3df 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -31,8 +31,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" - "msg/PolygonVO.msg" - "msg/CircleVO.msg" + "msg/PolygonObject.msg" + "msg/CircleObject.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/msg/CircleVO.msg b/nav2_msgs/msg/CircleObject.msg similarity index 100% rename from nav2_msgs/msg/CircleVO.msg rename to nav2_msgs/msg/CircleObject.msg index 4525d46f94..2aa64adab6 100644 --- a/nav2_msgs/msg/CircleVO.msg +++ b/nav2_msgs/msg/CircleObject.msg @@ -1,6 +1,6 @@ std_msgs/Header header +unique_identifier_msgs/UUID uuid 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/PolygonObject.msg similarity index 100% rename from nav2_msgs/msg/PolygonVO.msg rename to nav2_msgs/msg/PolygonObject.msg index c57b69fcd7..b5c9721c62 100644 --- a/nav2_msgs/msg/PolygonVO.msg +++ b/nav2_msgs/msg/PolygonObject.msg @@ -1,5 +1,5 @@ std_msgs/Header header +unique_identifier_msgs/UUID uuid geometry_msgs/Point32[] points bool closed int8 value -unique_identifier_msgs/UUID uuid diff --git a/nav2_msgs/srv/AddShapes.srv b/nav2_msgs/srv/AddShapes.srv index dda8235de1..1b3b575387 100644 --- a/nav2_msgs/srv/AddShapes.srv +++ b/nav2_msgs/srv/AddShapes.srv @@ -1,6 +1,6 @@ # Add/update vector objects on map -CircleVO[] circles -PolygonVO[] polygons +CircleObject[] circles +PolygonObject[] polygons --- bool success diff --git a/nav2_msgs/srv/GetShapes.srv b/nav2_msgs/srv/GetShapes.srv index 15dcf18c03..79b7d6c431 100644 --- a/nav2_msgs/srv/GetShapes.srv +++ b/nav2_msgs/srv/GetShapes.srv @@ -1,5 +1,5 @@ # Get vector objects which are now being applied on map --- -CircleVO[] circles -PolygonVO[] polygons +CircleObject[] circles +PolygonObject[] polygons diff --git a/nav2_msgs/srv/RemoveShapes.srv b/nav2_msgs/srv/RemoveShapes.srv index be91b3535a..a001f9360e 100644 --- a/nav2_msgs/srv/RemoveShapes.srv +++ b/nav2_msgs/srv/RemoveShapes.srv @@ -1,5 +1,6 @@ # Remove vector objects from map +bool all_objects unique_identifier_msgs/UUID[] uuids --- bool success From a225fce615d34b133a352c5cb6eba90a3a139cc2 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 10 Nov 2023 11:44:12 +0300 Subject: [PATCH 03/19] Simplify shapes param configuring Signed-off-by: Alexey Merzlyakov --- .../nav2_map_server/vector_object_server.hpp | 2 +- .../nav2_map_server/vector_object_shapes.hpp | 38 ++--- .../src/vo_server/vector_object_server.cpp | 38 ++--- .../src/vo_server/vector_object_shapes.cpp | 143 ++++++++++-------- 4 files changed, 113 insertions(+), 108 deletions(-) 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 index 3973d5b456..5e4160b728 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -178,7 +178,7 @@ class VectorObjectServer : public nav2_util::LifecycleNode * @brief Supporting routine obtaining all ROS-parameters * @return True if all parameters were obtained or false in the failure case */ - bool obtainParameters(); + bool obtainParams(); /// @brief TF buffer std::shared_ptr tf_buffer_; 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 index 4de44f3006..032a5fab44 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -77,7 +77,7 @@ class Shape * @param shape_name Name of the shape * @return True if all parameters were obtained or false in failure case */ - virtual bool obtainParameters(const std::string & shape_name) = 0; + virtual bool obtainParams(const std::string & shape_name) = 0; /** * @brief Gets shape boundaries. @@ -171,20 +171,16 @@ class Polygon : public Shape /* * @brief Polygon class constructor * @param node Vector Object server node pointer - * @param params PolygonObject parameters. In case of nullptr, - * parameters to be read from ROS-parameters. - * @throw std::exception in case of inconsistent shape + * @note setParams()/obtainParams() should be called after to configure the shape */ - Polygon( - const nav2_util::LifecycleNode::WeakPtr & node, - const nav2_msgs::msg::PolygonObject::SharedPtr params = nullptr); + Polygon(const nav2_util::LifecycleNode::WeakPtr & node); /** * @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 obtainParameters(const std::string & shape_name); + bool obtainParams(const std::string & shape_name); /** * @brief Gets shape boundaries @@ -218,9 +214,9 @@ class Polygon : public Shape /** * @brief Tries to update Polygon parameters - * @throw std::exception in case of inconsistent shape + * @return False in case of inconsistent shape */ - void setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); + bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); /** * @brief Gets the value of the shape. @@ -268,9 +264,9 @@ class Polygon : public Shape protected: /** * @brief Checks that shape is consistent for further operation - * @throw std::exception in case of inconsistent shape + * @return False in case of inconsistent shape */ - void checkConsistency(); + bool checkConsistency(); /// @brief Input polygon parameters (could be in any frame) nav2_msgs::msg::PolygonObject::SharedPtr params_; @@ -285,20 +281,16 @@ class Circle : public Shape /* * @brief Circle class constructor * @param node Vector Object server node pointer - * @param params CircleObject parameters. In case of nullptr, - * parameters to be read from ROS-parameters. - * @throw std::exception in case of inconsistent shape + * @note setParams()/obtainParams() should be called after to configure the shape */ - Circle( - const nav2_util::LifecycleNode::WeakPtr & node, - const nav2_msgs::msg::CircleObject::SharedPtr params = nullptr); + Circle(const nav2_util::LifecycleNode::WeakPtr & node); /** * @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 obtainParameters(const std::string & shape_name); + bool obtainParams(const std::string & shape_name); /** * @brief Gets shape boundaries @@ -332,9 +324,9 @@ class Circle : public Shape /** * @brief Tries to update Circle parameters - * @throw std::exception in case of inconsistent shape + * @return False in case of inconsistent shape */ - void setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); + bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); /** * @brief Gets the value of the shape. @@ -382,9 +374,9 @@ class Circle : public Shape protected: /** * @brief Checks that shape is consistent for further operation - * @throw std::exception in case of inconsistent shape + * @return False in case of inconsistent shape */ - void checkConsistency(); + bool checkConsistency(); /** * @brief Converts circle center to map coordinates diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index 9e335d45b6..cff2bdea57 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -46,7 +46,7 @@ VectorObjectServer::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_listener_ = std::make_shared(*tf_buffer_); // Obtaining ROS parameters - if (!obtainParameters()) { + if (!obtainParams()) { return nav2_util::CallbackReturn::FAILURE; } @@ -368,23 +368,18 @@ void VectorObjectServer::addShapesCallback( // Preserving old parameters for the case, if new ones to be incorrect nav2_msgs::msg::PolygonObject::SharedPtr old_params = polygon->getParams(); - try { - polygon->setParams(new_params); - } catch (const std::exception & ex) { + if (!polygon->setParams(new_params)) { // Restore old parameters polygon->setParams(old_params); - // ... and report the problem - RCLCPP_ERROR(get_logger(), "Can not update polygon: %s", ex.what()); + // ... and set the failure to return response->success = false; } } else { // Vector Object with given UUID was not found: creating a new one - // Creating new polygon - try { - std::shared_ptr polygon = std::make_shared(node, new_params); + std::shared_ptr polygon = std::make_shared(node); + if (polygon->setParams(new_params)) { shapes_.insert({polygon->getUUID(), polygon}); - } catch (const std::exception & ex) { - RCLCPP_ERROR(get_logger(), "Can not add polygon: %s", ex.what()); + } else { response->success = false; } } @@ -413,23 +408,18 @@ void VectorObjectServer::addShapesCallback( // Preserving old parameters for the case, if new ones to be incorrect nav2_msgs::msg::CircleObject::SharedPtr old_params = circle->getParams(); - try { - circle->setParams(new_params); - } catch (const std::exception & ex) { + if (!circle->setParams(new_params)) { // Restore old parameters circle->setParams(old_params); - // ... and report the problem - RCLCPP_ERROR(get_logger(), "Can not update circle: %s", ex.what()); + // ... and set the failure to return response->success = false; } } else { // Vector Object with given UUID was not found: creating a new one - // Creating new circle - try { - std::shared_ptr circle = std::make_shared(node, new_params); + std::shared_ptr circle = std::make_shared(node); + if (circle->setParams(new_params)) { shapes_.insert({circle->getUUID(), circle}); - } catch (const std::exception & ex) { - RCLCPP_ERROR(get_logger(), "Can not add circle: %s", ex.what()); + } else { response->success = false; } } @@ -499,7 +489,7 @@ void VectorObjectServer::removeShapesCallback( switchMapUpdate(); } -bool VectorObjectServer::obtainParameters() +bool VectorObjectServer::obtainParams() { // Main ROS-parameters map_topic_ = getROSParameter( @@ -537,7 +527,7 @@ bool VectorObjectServer::obtainParameters() if (shape_type == "polygon") { try { std::shared_ptr polygon = std::make_shared(shared_from_this()); - if (!polygon->obtainParameters(shape_name)) { + if (!polygon->obtainParams(shape_name)) { return false; } shapes_.insert({polygon->getUUID(), polygon}); @@ -549,7 +539,7 @@ bool VectorObjectServer::obtainParameters() try { std::shared_ptr circle = std::make_shared(shared_from_this()); - if (!circle->obtainParameters(shape_name)) { + if (!circle->obtainParams(shape_name)) { return false; } shapes_.insert({circle->getUUID(), circle}); diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index 2d73a3e005..b822e462ed 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -58,7 +58,7 @@ bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_ if (uuid_parse(uuid_str.c_str(), out_uuid)) { RCLCPP_ERROR( node->get_logger(), - "Can not parse UUID string for %s shape: %s", + "[%s] Can not parse UUID string for shape: %s", shape_name.c_str(), uuid_str.c_str()); return false; } @@ -70,7 +70,7 @@ bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_ uuid_unparse(out_uuid, uuid_str); RCLCPP_INFO( node->get_logger(), - "No UUID is specified for %s shape. Generating a new one: %s", + "[%s] No UUID is specified for shape. Generating a new one: %s", shape_name.c_str(), uuid_str); } @@ -80,36 +80,26 @@ bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_ // ---------- Polygon ---------- Polygon::Polygon( - const nav2_util::LifecycleNode::WeakPtr & node, - const nav2_msgs::msg::PolygonObject::SharedPtr params) + const nav2_util::LifecycleNode::WeakPtr & node) : 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::obtainParameters(const std::string & shape_name) +bool Polygon::obtainParams(const std::string & shape_name) { auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; } + if (!params_) { + params_ = std::make_shared(); + } + if (!polygon_) { + polygon_ = std::make_shared(); + } + params_->header.frame_id = getROSParameter( node, shape_name + ".frame_id", "map").as_string(); params_->value = getROSParameter( @@ -125,7 +115,7 @@ bool Polygon::obtainParameters(const std::string & shape_name) } catch (const std::exception & ex) { RCLCPP_ERROR( node->get_logger(), - "Error while getting polygon %s parameters: %s", + "[%s] Error while getting polygon parameters: %s", shape_name.c_str(), ex.what()); return false; } @@ -133,7 +123,7 @@ bool Polygon::obtainParameters(const std::string & shape_name) if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { RCLCPP_ERROR( node->get_logger(), - "Polygon %s has incorrect points description", + "[%s] Polygon has incorrect points description", shape_name.c_str()); return false; } @@ -228,8 +218,8 @@ void Polygon::putBorders( 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); + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), polygon_->points[0].x, polygon_->points[0].y); return; } @@ -240,8 +230,8 @@ void Polygon::putBorders( 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); + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), polygon_->points[i].x, polygon_->points[i].y); return; } nav2_util::raytraceLine(ma, mx0, my0, mx1, my1, map->info.width); @@ -253,14 +243,21 @@ nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const return params_; } -void Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) +bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) { 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()); + } + + return checkConsistency(); } int8_t Polygon::getValue() const @@ -314,47 +311,47 @@ bool Polygon::toFrame( return true; } -void Polygon::checkConsistency() +bool Polygon::checkConsistency() { if (params_->points.size() < 3) { - throw std::runtime_error("Polygon has incorrect number of vertices"); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Polygon has incorrect number of vertices: %li", + getUUID().c_str(), params_->points.size()); + return false; } + + return true; } // ---------- Circle ---------- Circle::Circle( - const nav2_util::LifecycleNode::WeakPtr & node, - const nav2_msgs::msg::CircleObject::SharedPtr params) + const nav2_util::LifecycleNode::WeakPtr & node) : 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::obtainParameters(const std::string & shape_name) +bool Circle::obtainParams(const std::string & shape_name) { auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; } + if (!params_) { + params_ = std::make_shared(); + } + if (!center_) { + center_ = std::make_shared(); + } + params_->header.frame_id = getROSParameter( node, shape_name + ".frame_id", "map").as_string(); params_->value = getROSParameter( @@ -369,10 +366,17 @@ bool Circle::obtainParameters(const std::string & shape_name) node, shape_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY).as_double_array(); params_->radius = getROSParameter( node, shape_name + ".radius", rclcpp::PARAMETER_DOUBLE).as_double(); + if (params_->radius < 0) { + RCLCPP_ERROR( + node->get_logger(), + "[%s] Circle has incorrect radius less than zero", + shape_name.c_str()); + return false; + } } catch (const std::exception & ex) { RCLCPP_ERROR( node->get_logger(), - "Error while getting circle %s parameters: %s", + "[%s] Error while getting circle parameters: %s", shape_name.c_str(), ex.what()); return false; } @@ -380,7 +384,7 @@ bool Circle::obtainParameters(const std::string & shape_name) if (center_row.size() != 2) { RCLCPP_ERROR( node->get_logger(), - "Circle %s has incorrect center description", + "[%s] Circle has incorrect center description", shape_name.c_str()); return false; } @@ -423,8 +427,8 @@ bool Circle::centerToMap( 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); + "[UUID: %s] Can not convert (%f, %f) circle center to map", + getUUID().c_str(), center_->x, center_->y); return false; } // We need the circle center to be always shifted one cell less its logical center @@ -436,7 +440,9 @@ bool Circle::centerToMap( 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); + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), center_->x, center_->y); return false; } @@ -506,14 +512,21 @@ nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const return params_; } -void Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) +bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) { params_ = params; + if (!center_) { center_ = std::make_shared(); } *center_ = params_->center; - checkConsistency(); + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } + + return checkConsistency(); } int8_t Circle::getValue() const @@ -565,11 +578,21 @@ bool Circle::toFrame( return true; } -void Circle::checkConsistency() +bool Circle::checkConsistency() { if (params_->radius < 0.0) { - throw std::runtime_error("Circle has incorrect radius less than zero"); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Circle has incorrect radius less than zero", + getUUID().c_str()); + return false; } + return true; } } // namespace nav2_map_server From 1ba3920c17b868a4778427e5cd12274ef8188866 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 10 Nov 2023 11:54:41 +0300 Subject: [PATCH 04/19] Rename getROSParameter() to getParameter() Signed-off-by: Alexey Merzlyakov --- .../nav2_map_server/vector_object_utils.hpp | 4 ++-- .../src/vo_server/vector_object_server.cpp | 18 ++++++++--------- .../src/vo_server/vector_object_shapes.cpp | 20 +++++++++---------- 3 files changed, 21 insertions(+), 21 deletions(-) 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 index 89037bd190..dd90966638 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -52,7 +52,7 @@ inline std::string unparseUUID(const unsigned char * uuid) * @return Obtained parameter value */ template -inline rclcpp::Parameter getROSParameter( +inline rclcpp::Parameter getParameter( nav2_util::LifecycleNode::SharedPtr node, const std::string & param_name, const ValT & default_val) @@ -71,7 +71,7 @@ inline rclcpp::Parameter getROSParameter( * @throw std::exception if parameter is not set */ template<> -inline rclcpp::Parameter getROSParameter( +inline rclcpp::Parameter getParameter( nav2_util::LifecycleNode::SharedPtr node, const std::string & param_name, const rclcpp::ParameterType & val_type) diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index cff2bdea57..d3a9f0bbe3 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -492,29 +492,29 @@ void VectorObjectServer::removeShapesCallback( bool VectorObjectServer::obtainParams() { // Main ROS-parameters - map_topic_ = getROSParameter( + map_topic_ = getParameter( shared_from_this(), "map_topic", "vo_map").as_string(); - global_frame_id_ = getROSParameter( + global_frame_id_ = getParameter( shared_from_this(), "global_frame_id", "map").as_string(); - resolution_ = getROSParameter( + resolution_ = getParameter( shared_from_this(), "resolution", 0.05).as_double(); - default_value_ = getROSParameter( + default_value_ = getParameter( shared_from_this(), "default_value", nav2_util::OCC_GRID_UNKNOWN).as_int(); - overlay_type_ = static_cast(getROSParameter( + overlay_type_ = static_cast(getParameter( shared_from_this(), "overlay_type", static_cast(OverlayType::OVERLAY_SEQ)).as_int()); - update_frequency_ = getROSParameter( + update_frequency_ = getParameter( shared_from_this(), "update_frequency", 1.0).as_double(); - transform_tolerance_ = getROSParameter( + transform_tolerance_ = getParameter( shared_from_this(), "transform_tolerance", 0.1).as_double(); // Shapes - std::vector shape_names = getROSParameter( + std::vector shape_names = getParameter( shared_from_this(), "shapes", std::vector()).as_string_array(); for (std::string shape_name : shape_names) { std::string shape_type; try { - shape_type = getROSParameter( + shape_type = getParameter( shared_from_this(), shape_name + ".type", rclcpp::PARAMETER_STRING).as_string(); } catch (const std::exception & ex) { RCLCPP_ERROR( diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index b822e462ed..fe87101f6e 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -53,7 +53,7 @@ bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_ try { // Try to get shape UUID from ROS-parameters - std::string uuid_str = getROSParameter( + std::string uuid_str = getParameter( node, shape_name + ".uuid", rclcpp::PARAMETER_STRING).as_string(); if (uuid_parse(uuid_str.c_str(), out_uuid)) { RCLCPP_ERROR( @@ -100,17 +100,17 @@ bool Polygon::obtainParams(const std::string & shape_name) polygon_ = std::make_shared(); } - params_->header.frame_id = getROSParameter( + params_->header.frame_id = getParameter( node, shape_name + ".frame_id", "map").as_string(); - params_->value = getROSParameter( + params_->value = getParameter( node, shape_name + ".value", nav2_util::OCC_GRID_OCCUPIED).as_int(); - params_->closed = getROSParameter( + params_->closed = getParameter( node, shape_name + ".closed", true).as_bool(); std::vector poly_row; try { - poly_row = getROSParameter( + poly_row = getParameter( node, shape_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY).as_double_array(); } catch (const std::exception & ex) { RCLCPP_ERROR( @@ -352,19 +352,19 @@ bool Circle::obtainParams(const std::string & shape_name) center_ = std::make_shared(); } - params_->header.frame_id = getROSParameter( + params_->header.frame_id = getParameter( node, shape_name + ".frame_id", "map").as_string(); - params_->value = getROSParameter( + params_->value = getParameter( node, shape_name + ".value", nav2_util::OCC_GRID_OCCUPIED).as_int(); - params_->fill = getROSParameter( + params_->fill = getParameter( node, shape_name + ".fill", true).as_bool(); std::vector center_row; try { - center_row = getROSParameter( + center_row = getParameter( node, shape_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY).as_double_array(); - params_->radius = getROSParameter( + params_->radius = getParameter( node, shape_name + ".radius", rclcpp::PARAMETER_DOUBLE).as_double(); if (params_->radius < 0) { RCLCPP_ERROR( From 8bd61a3ad4ac615ca707da7055bc5147e92e0aff Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 10 Nov 2023 13:29:03 +0300 Subject: [PATCH 05/19] Return back getMaskData() to nav2_costmap_2d Signed-off-by: Alexey Merzlyakov --- .../costmap_filters/costmap_filter.hpp | 14 +++++++++ .../plugins/costmap_filters/binary_filter.cpp | 2 +- .../plugins/costmap_filters/speed_filter.cpp | 2 +- .../include/nav2_util/occ_grid_utils.hpp | 29 ------------------- 4 files changed, 16 insertions(+), 31 deletions(-) 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 e72649b4b9..72b935e971 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,6 +184,20 @@ class CostmapFilter : public Layer const std::string mask_frame, geometry_msgs::msg::Pose2D & mask_pose) 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 834874c2fc..fe73c2cde1 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -199,7 +199,7 @@ void BinaryFilter::process( } // Getting filter_mask data from cell where the robot placed - int8_t mask_data = nav2_util::getMapData(filter_mask_, mask_robot_i, mask_robot_j); + int8_t mask_data = getMaskData(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/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index 10082ba922..96d78834db 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -202,7 +202,7 @@ void SpeedFilter::process( // Getting filter_mask data from cell where the robot placed and // calculating speed limit value - int8_t speed_mask_data = nav2_util::getMapData(filter_mask_, mask_robot_i, mask_robot_j); + int8_t speed_mask_data = getMaskData(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_util/include/nav2_util/occ_grid_utils.hpp b/nav2_util/include/nav2_util/occ_grid_utils.hpp index 1e1ebc766b..26bb8740f2 100644 --- a/nav2_util/include/nav2_util/occ_grid_utils.hpp +++ b/nav2_util/include/nav2_util/occ_grid_utils.hpp @@ -43,35 +43,6 @@ 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. From 2c0322f828b313900ec6012a49a08753c0586d44 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 10 Nov 2023 15:41:15 +0300 Subject: [PATCH 06/19] Add composition node support Signed-off-by: Alexey Merzlyakov --- nav2_map_server/CMakeLists.txt | 35 ++++-- .../launch/vector_object_server.launch.py | 117 +++++++++++++----- .../src/vo_server/vector_object_server.cpp | 7 ++ 3 files changed, 116 insertions(+), 43 deletions(-) diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 6102f49201..09650f2cf7 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -30,7 +30,7 @@ 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) +set(vo_server_executable vector_object_server) add_executable(${map_server_executable} src/map_server/main.cpp) @@ -44,15 +44,15 @@ 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 +add_executable(${vo_server_executable} src/vo_server/vector_object_server_node.cpp) set(map_io_library_name map_io) set(library_name ${map_server_executable}_core) +set(vo_library_name vector_object_core) + add_library(${map_io_library_name} SHARED src/map_mode.cpp src/map_io.cpp) @@ -62,6 +62,10 @@ add_library(${library_name} SHARED src/map_saver/map_saver.cpp src/costmap_filter_info/costmap_filter_info_server.cpp) +add_library(${vo_library_name} SHARED + src/vo_server/vector_object_shapes.cpp + src/vo_server/vector_object_server.cpp) + set(map_io_dependencies yaml_cpp_vendor nav_msgs @@ -88,6 +92,7 @@ set(map_saver_dependencies set(vo_server_dependencies rclcpp rclcpp_lifecycle + rclcpp_components nav_msgs nav2_msgs nav2_util) @@ -104,15 +109,17 @@ ament_target_dependencies(${map_saver_server_executable} ament_target_dependencies(${costmap_filter_info_server_executable} ${map_saver_dependencies}) +ament_target_dependencies(${vo_server_executable} + ${vo_server_dependencies}) + ament_target_dependencies(${library_name} ${map_server_dependencies}) ament_target_dependencies(${map_io_library_name} ${map_io_dependencies}) -ament_target_dependencies(${vector_object_server_executable} - ${vo_server_dependencies} -) +ament_target_dependencies(${vo_library_name} + ${vo_server_dependencies}) target_link_libraries(${library_name} ${map_io_library_name}) @@ -145,23 +152,26 @@ if(WIN32) YAML_CPP_DLL) endif() -target_link_libraries(${vector_object_server_executable} - ${UUID_LIBRARIES} -) +target_link_libraries(${vo_library_name} + ${UUID_LIBRARIES}) + +target_link_libraries(${vo_server_executable} + ${vo_library_name}) 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") +rclcpp_components_register_nodes(${vo_library_name} "nav2_map_server::VectorObjectServer") install(TARGETS - ${library_name} ${map_io_library_name} + ${library_name} ${map_io_library_name} ${vo_library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) install(TARGETS ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} - ${costmap_filter_info_server_executable} ${vector_object_server_executable} + ${costmap_filter_info_server_executable} ${vo_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ @@ -184,6 +194,7 @@ ament_export_include_directories(include) ament_export_libraries( ${library_name} ${map_io_library_name} + ${vo_library_name} ) ament_export_dependencies(${map_io_dependencies} ${map_server_dependencies}) ament_package() diff --git a/nav2_map_server/launch/vector_object_server.launch.py b/nav2_map_server/launch/vector_object_server.launch.py index ce0f867e93..54c012e46e 100644 --- a/nav2_map_server/launch/vector_object_server.launch.py +++ b/nav2_map_server/launch/vector_object_server.launch.py @@ -19,9 +19,14 @@ 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.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import NotEqualsSubstitution +from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node +from launch_ros.actions import PushROSNamespace +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml @@ -32,12 +37,17 @@ def generate_launch_description(): # Constant parameters lifecycle_nodes = ['vector_object_server'] autostart = True + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] # Launch arguments # 1. Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) # 2. Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( @@ -55,33 +65,76 @@ def generate_launch_description(): 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]) + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True), + allow_substs=True) + + # Declare node launching commands + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_vo_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings), + Node( + package='nav2_map_server', + executable='vector_object_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings) + ] + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushROSNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_vo_server', + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings), + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::VectorObjectServer', + name='vector_object_server', + parameters=[configured_params], + remappings=remappings) + ], + ) + ] + ) ld = LaunchDescription() @@ -89,9 +142,11 @@ def generate_launch_description(): ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) # Node launching commands - ld.add_action(start_lifecycle_manager_cmd) - ld.add_action(start_vector_object_server_cmd) + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) return ld diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index d3a9f0bbe3..6d52b75f70 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -556,3 +556,10 @@ bool VectorObjectServer::obtainParams() } } // namespace nav2_map_server + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::VectorObjectServer) From b6ea965fabbb19629b311125fabb7a93567e49cc Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Mon, 13 Nov 2023 14:49:42 +0300 Subject: [PATCH 07/19] Remove redundant methods Signed-off-by: Alexey Merzlyakov --- .../nav2_map_server/vector_object_shapes.hpp | 22 ------------------- .../src/vo_server/vector_object_shapes.cpp | 10 --------- 2 files changed, 32 deletions(-) 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 index 032a5fab44..2ed47329d7 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -128,14 +128,6 @@ class Shape */ 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 @@ -236,13 +228,6 @@ class Polygon : public Shape */ 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 @@ -346,13 +331,6 @@ class Circle : public Shape */ 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 diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index fe87101f6e..3dc07e424e 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -275,11 +275,6 @@ std::string Polygon::getUUID() const return unparseUUID(params_->uuid.uuid.data()); } -bool Polygon::isUUID(const unsigned char * uuid) const -{ - return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; -} - bool Polygon::isFill() const { return params_->closed; @@ -544,11 +539,6 @@ std::string Circle::getUUID() const return unparseUUID(params_->uuid.uuid.data()); } -bool Circle::isUUID(const unsigned char * uuid) const -{ - return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; -} - bool Circle::isFill() const { return params_->fill; From c987eb400d9ca19b217d58edf1ddda4731957c5d Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Tue, 14 Nov 2023 08:04:56 +0300 Subject: [PATCH 08/19] Update nav2_map_server/src/vo_server/vector_object_server.cpp Co-authored-by: Steve Macenski Signed-off-by: Alexey Merzlyakov --- nav2_map_server/src/vo_server/vector_object_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index 6d52b75f70..e8b8b13a41 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -284,7 +284,7 @@ void VectorObjectServer::processMap() } try { - if (shapes_.size()) { + if (shapes_.size() > 0) { if (!transformVectorObjects()) { return; } From 3cade888dee62e44c991014d79a04d44440ec49d Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Tue, 14 Nov 2023 10:41:35 +0300 Subject: [PATCH 09/19] Avoid shapes clearing Signed-off-by: Alexey Merzlyakov --- nav2_map_server/src/vo_server/vector_object_server.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index e8b8b13a41..509a16301d 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -117,9 +117,6 @@ VectorObjectServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) map_pub_.reset(); map_.reset(); - for (auto shape : shapes_) { - shape.second.reset(); - } shapes_.clear(); tf_listener_.reset(); @@ -463,9 +460,6 @@ void VectorObjectServer::removeShapesCallback( if (request->all_objects) { // Clear all objects - for (auto shape : shapes_) { - shape.second.reset(); - } shapes_.clear(); } else { // Find objects to remove From 0ffb216e8421c33c88c122b68c0df448cae6565c Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Tue, 14 Nov 2023 11:04:17 +0300 Subject: [PATCH 10/19] Optimize switchMapUpdate() method Signed-off-by: Alexey Merzlyakov --- .../nav2_map_server/vector_object_server.hpp | 7 ---- .../src/vo_server/vector_object_server.cpp | 34 +++++++------------ 2 files changed, 13 insertions(+), 28 deletions(-) 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 index 5e4160b728..1752c28e5a 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -122,13 +122,6 @@ class VectorObjectServer : public nav2_util::LifecycleNode */ 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 diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index 509a16301d..15618249e6 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -301,34 +301,26 @@ void VectorObjectServer::processMap() publishMap(); } -bool VectorObjectServer::isMapUpdate() +void VectorObjectServer::switchMapUpdate() { for (auto shape : shapes_) { if (shape.second->getFrameID() != global_frame_id_ && !shape.second->getFrameID().empty()) { - return true; + 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"); + return; } } - 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(); + if (map_timer_) { + map_timer_->cancel(); + map_timer_.reset(); } + RCLCPP_INFO(get_logger(), "Publishing map once"); + processMap(); } void VectorObjectServer::addShapesCallback( From 50d4cac882a04116bda3a26af28f081f5ce52d8b Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 17 Nov 2023 11:04:35 +0300 Subject: [PATCH 11/19] Switch to vector of shapes Signed-off-by: Alexey Merzlyakov --- .../nav2_map_server/vector_object_server.hpp | 13 ++- .../nav2_map_server/vector_object_shapes.hpp | 22 ++++++ .../src/vo_server/vector_object_server.cpp | 79 +++++++++++-------- .../src/vo_server/vector_object_shapes.cpp | 10 +++ 4 files changed, 87 insertions(+), 37 deletions(-) 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 index 1752c28e5a..93dd4b1c52 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include "nav_msgs/msg/occupancy_grid.hpp" @@ -79,6 +79,13 @@ class VectorObjectServer : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief Finds the shape with given UUID + * @param uuid Given UUID to search with + * @return Iterator to the shape, if found. Otherwise past-the-end iterator. + */ + std::vector>::iterator findShape(const unsigned char * uuid); + /** * @brief Transform all vector shapes from their local frame to output map frame */ @@ -178,8 +185,8 @@ class VectorObjectServer : public nav2_util::LifecycleNode /// @brief TF listener std::shared_ptr tf_listener_; - /// @brief All {UUID, Shape} vector objects map - std::unordered_map> shapes_; + /// @brief All shapes vector + std::vector> shapes_; /// @brief Output map resolution double resolution_; 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 index 2ed47329d7..032a5fab44 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -128,6 +128,14 @@ class Shape */ 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 @@ -228,6 +236,13 @@ class Polygon : public Shape */ 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 @@ -331,6 +346,13 @@ class Circle : public Shape */ 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 diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index 15618249e6..fd52b59509 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -133,15 +133,26 @@ VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } +std::vector>::iterator +VectorObjectServer::findShape(const unsigned char * uuid) +{ + for (auto it = shapes_.begin(); it != shapes_.end(); it++) { + if ((*it)->isUUID(uuid)) { + return it; + } + } + return shapes_.end(); +} + bool VectorObjectServer::transformVectorObjects() { for (auto shape : shapes_) { - if (shape.second->getFrameID() != global_frame_id_ && !shape.second->getFrameID().empty()) { + if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) { // Shape to be updated dynamically - if (!shape.second->toFrame(global_frame_id_, tf_buffer_, transform_tolerance_)) { + if (!shape->toFrame(global_frame_id_, tf_buffer_, transform_tolerance_)) { RCLCPP_ERROR( get_logger(), "Can not transform vector object from %s to %s frame", - shape.second->getFrameID().c_str(), global_frame_id_.c_str()); + shape->getFrameID().c_str(), global_frame_id_.c_str()); return false; } } @@ -161,7 +172,7 @@ void VectorObjectServer::getMapBoundaries( double min_p_x, min_p_y, max_p_x, max_p_y; for (auto shape : shapes_) { - shape.second->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y); + 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; } @@ -225,7 +236,7 @@ void VectorObjectServer::putVectorObjectsOnMap() { // Filling the shapes for (auto shape : shapes_) { - if (shape.second->isFill()) { + if (shape->isFill()) { // Put filled shape on map double wx1 = std::numeric_limits::max(); double wy1 = std::numeric_limits::max(); @@ -236,7 +247,7 @@ void VectorObjectServer::putVectorObjectsOnMap() unsigned int mx2 = 0; unsigned int my2 = 0; - shape.second->getBoundaries(wx1, wy1, wx2, wy2); + shape->getBoundaries(wx1, wy1, wx2, wy2); if ( !nav2_util::worldToMap(map_, wx1, wy1, mx1, my1) || !nav2_util::worldToMap(map_, wx2, wy2, mx2, my2)) @@ -244,7 +255,7 @@ void VectorObjectServer::putVectorObjectsOnMap() RCLCPP_ERROR( get_logger(), "Error to get shape boundaries on map (UUID: %s)", - shape.second->getUUID().c_str()); + shape->getUUID().c_str()); return; } @@ -254,14 +265,14 @@ void VectorObjectServer::putVectorObjectsOnMap() it = my * map_->info.width + mx; double wx, wy; nav2_util::mapToWorld(map_, mx, my, wx, wy); - if (shape.second->isPointInside(wx, wy)) { - processVal(map_->data[it], shape.second->getValue(), overlay_type_); + if (shape->isPointInside(wx, wy)) { + processVal(map_->data[it], shape->getValue(), overlay_type_); } } } } else { // Put shape borders on map - shape.second->putBorders(map_, overlay_type_); + shape->putBorders(map_, overlay_type_); } } } @@ -304,7 +315,7 @@ void VectorObjectServer::processMap() void VectorObjectServer::switchMapUpdate() { for (auto shape : shapes_) { - if (shape.second->getFrameID() != global_frame_id_ && !shape.second->getFrameID().empty()) { + if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) { if (!map_timer_) { map_timer_ = this->create_timer( std::chrono::duration(1.0 / update_frequency_), @@ -339,21 +350,21 @@ void VectorObjectServer::addShapesCallback( nav2_msgs::msg::PolygonObject::SharedPtr new_params = std::make_shared(req_poly); - auto shape = shapes_.find(unparseUUID(new_params->uuid.uuid.data())); - if (shape != shapes_.end()) { + auto it = findShape(new_params->uuid.uuid.data()); + if (it != shapes_.end()) { // Vector Object with given UUID was found: updating it // Check that found shape has correct type - if (shape->second->getType() != POLYGON) { + if ((*it)->getType() != POLYGON) { RCLCPP_ERROR( get_logger(), "Shape (UUID: %s) is not a polygon type", - shape->second->getUUID().c_str()); + (*it)->getUUID().c_str()); response->success = false; // Do not add this shape continue; } - std::shared_ptr polygon = std::static_pointer_cast(shape->second); + std::shared_ptr polygon = std::static_pointer_cast(*it); // Preserving old parameters for the case, if new ones to be incorrect nav2_msgs::msg::PolygonObject::SharedPtr old_params = polygon->getParams(); @@ -367,7 +378,7 @@ void VectorObjectServer::addShapesCallback( // Vector Object with given UUID was not found: creating a new one std::shared_ptr polygon = std::make_shared(node); if (polygon->setParams(new_params)) { - shapes_.insert({polygon->getUUID(), polygon}); + shapes_.push_back(polygon); } else { response->success = false; } @@ -379,21 +390,21 @@ void VectorObjectServer::addShapesCallback( nav2_msgs::msg::CircleObject::SharedPtr new_params = std::make_shared(req_crcl); - auto shape = shapes_.find(unparseUUID(new_params->uuid.uuid.data())); - if (shape != shapes_.end()) { + auto it = findShape(new_params->uuid.uuid.data()); + if (it != shapes_.end()) { // Vector object with given UUID was found: updating it // Check that found shape has correct type - if (shape->second->getType() != CIRCLE) { + if ((*it)->getType() != CIRCLE) { RCLCPP_ERROR( get_logger(), "Shape (UUID: %s) is not a circle type", - shape->second->getUUID().c_str()); + (*it)->getUUID().c_str()); response->success = false; // Do not add this shape continue; } - std::shared_ptr circle = std::static_pointer_cast(shape->second); + std::shared_ptr circle = std::static_pointer_cast(*it); // Preserving old parameters for the case, if new ones to be incorrect nav2_msgs::msg::CircleObject::SharedPtr old_params = circle->getParams(); @@ -407,7 +418,7 @@ void VectorObjectServer::addShapesCallback( // Vector Object with given UUID was not found: creating a new one std::shared_ptr circle = std::make_shared(node); if (circle->setParams(new_params)) { - shapes_.insert({circle->getUUID(), circle}); + shapes_.push_back(circle); } else { response->success = false; } @@ -426,17 +437,17 @@ void VectorObjectServer::getShapesCallback( std::shared_ptr circle; for (auto shape : shapes_) { - switch (shape.second->getType()) { + switch (shape->getType()) { case POLYGON: - polygon = std::static_pointer_cast(shape.second); + polygon = std::static_pointer_cast(shape); response->polygons.push_back(*(polygon->getParams())); break; case CIRCLE: - circle = std::static_pointer_cast(shape.second); + circle = std::static_pointer_cast(shape); response->circles.push_back(*(circle->getParams())); break; default: - RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape.second->getUUID().c_str()); + RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape->getUUID().c_str()); } } } @@ -456,11 +467,11 @@ void VectorObjectServer::removeShapesCallback( } else { // Find objects to remove for (auto req_uuid : request->uuids) { - auto shape = shapes_.find(unparseUUID(req_uuid.uuid.data())); - if (shape != shapes_.end()) { - // Polygon with given UUID was found: remove it - shape->second.reset(); - shapes_.erase(shape); + auto it = findShape(req_uuid.uuid.data()); + if (it != shapes_.end()) { + // Shape with given UUID was found: remove it + (*it).reset(); + shapes_.erase(it); } else { // Required vector object was not found RCLCPP_ERROR( @@ -516,7 +527,7 @@ bool VectorObjectServer::obtainParams() if (!polygon->obtainParams(shape_name)) { return false; } - shapes_.insert({polygon->getUUID(), polygon}); + shapes_.push_back(polygon); } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Can not create new polygon: %s", ex.what()); return false; @@ -528,7 +539,7 @@ bool VectorObjectServer::obtainParams() if (!circle->obtainParams(shape_name)) { return false; } - shapes_.insert({circle->getUUID(), circle}); + shapes_.push_back(circle); } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Can not create new circle: %s", ex.what()); return false; diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index 3dc07e424e..fe87101f6e 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -275,6 +275,11 @@ std::string Polygon::getUUID() const return unparseUUID(params_->uuid.uuid.data()); } +bool Polygon::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + bool Polygon::isFill() const { return params_->closed; @@ -539,6 +544,11 @@ std::string Circle::getUUID() const return unparseUUID(params_->uuid.uuid.data()); } +bool Circle::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + bool Circle::isFill() const { return params_->fill; From 2ece7c29bf08f484df5cb663fb8239820bc4592a Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 17 Nov 2023 11:05:40 +0300 Subject: [PATCH 12/19] Minor fixes Signed-off-by: Alexey Merzlyakov --- .../nav2_map_server/vector_object_server.hpp | 2 +- .../src/vo_server/vector_object_server.cpp | 27 +++++++------------ .../src/vo_server/vector_object_shapes.cpp | 2 +- 3 files changed, 11 insertions(+), 20 deletions(-) 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 index 93dd4b1c52..39afec55cf 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -100,7 +100,7 @@ class VectorObjectServer : public nav2_util::LifecycleNode * @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); + double & min_x, double & min_y, double & max_x, double & max_y) const; /** * @brief Creates or updates existing map with required sizes and fills it with default value diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index fd52b59509..10b2370c0d 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -162,7 +162,7 @@ bool VectorObjectServer::transformVectorObjects() } void VectorObjectServer::getMapBoundaries( - double & min_x, double & min_y, double & max_x, double & max_y) + double & min_x, double & min_y, double & max_x, double & max_y) const { min_x = std::numeric_limits::max(); min_y = std::numeric_limits::max(); @@ -305,7 +305,7 @@ void VectorObjectServer::processMap() 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()); + RCLCPP_ERROR(get_logger(), "Can not update map: %s", ex.what()); return; } @@ -522,30 +522,21 @@ bool VectorObjectServer::obtainParams() } if (shape_type == "polygon") { - try { - std::shared_ptr polygon = std::make_shared(shared_from_this()); - if (!polygon->obtainParams(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()); + std::shared_ptr polygon = std::make_shared(shared_from_this()); + if (!polygon->obtainParams(shape_name)) { return false; } + shapes_.push_back(polygon); } else if (shape_type == "circle") { - try { - std::shared_ptr circle = std::make_shared(shared_from_this()); + std::shared_ptr circle = std::make_shared(shared_from_this()); - if (!circle->obtainParams(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()); + if (!circle->obtainParams(shape_name)) { return false; } + shapes_.push_back(circle); } else { RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str()); + return false; } } diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index fe87101f6e..d6df152792 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -120,7 +120,7 @@ bool Polygon::obtainParams(const std::string & shape_name) return false; } // Check for points format correctness - if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { + if (poly_row.size() < 6 || poly_row.size() % 2 != 0) { RCLCPP_ERROR( node->get_logger(), "[%s] Polygon has incorrect points description", From 43bbeca89b63857b3394cbda31b750b2f93718f5 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Mon, 20 Nov 2023 08:38:11 +0300 Subject: [PATCH 13/19] Meet review comments Signed-off-by: Alexey Merzlyakov --- .../nav2_map_server/vector_object_shapes.hpp | 34 ------------------- .../nav2_map_server/vector_object_utils.hpp | 34 +++++++++++++++++++ .../src/vo_server/vector_object_server.cpp | 2 +- .../src/vo_server/vector_object_shapes.cpp | 2 +- 4 files changed, 36 insertions(+), 36 deletions(-) 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 index 032a5fab44..7dff9c7125 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -409,40 +409,6 @@ class Circle : public Shape 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 index dd90966638..8814dc3253 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -141,6 +141,40 @@ inline void fillMap( map->data[offset] = map_val; } +/// @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_UTILS_HPP_ diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index 10b2370c0d..c76de4c49d 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -321,7 +321,7 @@ void VectorObjectServer::switchMapUpdate() std::chrono::duration(1.0 / update_frequency_), std::bind(&VectorObjectServer::processMap, this)); } - RCLCPP_INFO(get_logger(), "Publishing map dynamically"); + RCLCPP_INFO(get_logger(), "Publishing map dynamically at %f Hz rate", update_frequency_); return; } } diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index d6df152792..0078dab64b 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -55,7 +55,7 @@ bool Shape::obtainShapeUUID(const std::string & shape_name, unsigned char * out_ // Try to get shape UUID from ROS-parameters std::string uuid_str = getParameter( node, shape_name + ".uuid", rclcpp::PARAMETER_STRING).as_string(); - if (uuid_parse(uuid_str.c_str(), out_uuid)) { + if (uuid_parse(uuid_str.c_str(), out_uuid) != 0) { RCLCPP_ERROR( node->get_logger(), "[%s] Can not parse UUID string for shape: %s", From d3a832e02ebd420a5a3039ac77ea5b3ed380870a Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Mon, 20 Nov 2023 11:56:06 +0300 Subject: [PATCH 14/19] Move isPointInside algorithm to nav2_util Signed-off-by: Alexey Merzlyakov --- .../nav2_collision_monitor/polygon.hpp | 7 -- nav2_collision_monitor/src/polygon.cpp | 35 +--------- .../src/vo_server/vector_object_shapes.cpp | 32 +-------- nav2_util/include/nav2_util/polygon_utils.hpp | 68 +++++++++++++++++++ 4 files changed, 72 insertions(+), 70 deletions(-) create mode 100644 nav2_util/include/nav2_util/polygon_utils.hpp diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 5e342cdb7f..5dba4b3703 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -203,13 +203,6 @@ class Polygon rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( std::vector parameters); - /** - * @brief Checks if point is inside polygon - * @param point Given point to check - * @return True if given point is inside polygon, otherwise false - */ - bool isPointInside(const Point & point) const; - /** * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...] * @param poly_string Input String containing the verteceis of the polygon diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 77fc1f304c..9070f04d0a 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -21,6 +21,7 @@ #include "geometry_msgs/msg/point32.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/polygon_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/array_parser.hpp" @@ -230,7 +231,7 @@ int Polygon::getPointsInside(const std::vector & points) const { int num = 0; for (const Point & point : points) { - if (isPointInside(point)) { + if (nav2_util::isPointInsidePolygon(point.x, point.y, poly_)) { num++; } } @@ -510,38 +511,6 @@ void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr updatePolygon(msg); } -inline bool Polygon::isPointInside(const Point & point) 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 poly_size = poly_.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 = poly_size - 1; - for (j = 0; j < poly_size; 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 ((point.y <= poly_[i].y) == (point.y > poly_[j].y)) { - // Calculating the intersection coordinate of X+ ray - const double x_inter = poly_[i].x + - (point.y - poly_[i].y) * (poly_[j].x - poly_[i].x) / - (poly_[j].y - poly_[i].y); - // If intersection with checked edge is greater than point.x coordinate, inverting the result - if (x_inter > point.x) { - res = !res; - } - } - i = j; - } - return res; -} - bool Polygon::getPolygonFromString( std::string & poly_string, std::vector & polygon) diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index 0078dab64b..165e199cae 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -24,6 +24,7 @@ #include "nav2_util/occ_grid_utils.hpp" #include "nav2_util/occ_grid_values.hpp" +#include "nav2_util/polygon_utils.hpp" #include "nav2_util/raytrace_line_2d.hpp" #include "nav2_util/robot_utils.hpp" @@ -173,36 +174,7 @@ void Polygon::getBoundaries(double & min_x, double & min_y, double & max_x, doub 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; + return nav2_util::isPointInsidePolygon(px, py, polygon_->points); } void Polygon::putBorders( diff --git a/nav2_util/include/nav2_util/polygon_utils.hpp b/nav2_util/include/nav2_util/polygon_utils.hpp new file mode 100644 index 0000000000..101ce2a815 --- /dev/null +++ b/nav2_util/include/nav2_util/polygon_utils.hpp @@ -0,0 +1,68 @@ +// 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_UTIL__POLYGON_UTILS_HPP_ +#define NAV2_UTIL__POLYGON_UTILS_HPP_ + +#include + +namespace nav2_util +{ + +/** + * @brief Checks if point is inside the polygon + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @param polygon Polygon to check if the point is inside + * @return True if given point is inside polygon, otherwise false + */ +template +inline bool isPointInsidePolygon( + const double px, const double py, const std::vector & polygon) +{ + // 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.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[i].y) == (py > polygon[j].y)) { + // Calculating the intersection coordinate of X+ ray + const double x_inter = polygon[i].x + + (py - polygon[i].y) * + (polygon[j].x - polygon[i].x) / + (polygon[j].y - polygon[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; +} + +} // namespace nav2_util + +#endif // NAV2_UTIL__POLYGON_UTILS_HPP_ From 2b3bf842601799f3d94f2b27dbd94d42d2b772e7 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Mon, 20 Nov 2023 15:31:31 +0300 Subject: [PATCH 15/19] Testcases covering new functionality Signed-off-by: Alexey Merzlyakov --- nav2_map_server/test/unit/CMakeLists.txt | 22 + .../test/unit/test_vector_object_server.cpp | 1281 +++++++++++++++++ .../test/unit/test_vector_object_shapes.cpp | 767 ++++++++++ 3 files changed, 2070 insertions(+) create mode 100644 nav2_map_server/test/unit/test_vector_object_server.cpp create mode 100644 nav2_map_server/test/unit/test_vector_object_shapes.cpp diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt index 936c59ed89..3319fa7f37 100644 --- a/nav2_map_server/test/unit/CMakeLists.txt +++ b/nav2_map_server/test/unit/CMakeLists.txt @@ -19,3 +19,25 @@ ament_target_dependencies(test_costmap_filter_info_server rclcpp) target_link_libraries(test_costmap_filter_info_server ${library_name} ) + +# test_vector_object_shapes unit test +ament_add_gtest(test_vector_object_shapes + test_vector_object_shapes.cpp +) + +ament_target_dependencies(test_vector_object_shapes ${vo_server_dependencies}) + +target_link_libraries(test_vector_object_shapes + ${vo_library_name} +) + +# test_vector_object_server unit test +ament_add_gtest(test_vector_object_server + test_vector_object_server.cpp +) + +ament_target_dependencies(test_vector_object_server ${vo_server_dependencies}) + +target_link_libraries(test_vector_object_server + ${vo_library_name} +) diff --git a/nav2_map_server/test/unit/test_vector_object_server.cpp b/nav2_map_server/test/unit/test_vector_object_server.cpp new file mode 100644 index 0000000000..728ee2deac --- /dev/null +++ b/nav2_map_server/test/unit/test_vector_object_server.cpp @@ -0,0 +1,1281 @@ +// 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 +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "unique_identifier_msgs/msg/uuid.hpp" + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" + +#include "nav2_msgs/srv/add_shapes.hpp" +#include "nav2_msgs/srv/get_shapes.hpp" +#include "nav2_msgs/srv/remove_shapes.hpp" +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/occ_grid_utils.hpp" + +#include "nav2_map_server/vector_object_server.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char POLYGON_NAME[]{"Polygon"}; +static const char CIRCLE_NAME[]{"Circle"}; +static const char GLOBAL_FRAME_ID[]{"map"}; +static const char SHAPE_FRAME_ID[]{"shape"}; +static const double UPDATE_FREQUENCY{10.0}; +static const int8_t POLYGON_VAL{nav2_util::OCC_GRID_OCCUPIED}; +static const int8_t CIRCLE_VAL{nav2_util::OCC_GRID_OCCUPIED / 2}; + +class VOServerWrapper : public nav2_map_server::VectorObjectServer +{ +public: + void start() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_activate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void configureFail() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::FAILURE); + } + + void stop() + { + ASSERT_EQ(on_deactivate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_cleanup(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void cleanup() + { + ASSERT_EQ(on_cleanup(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void setProcessMap(const bool process_map) + { + process_map_ = process_map; + } +}; // VOServerWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + + void setVOServerParams(); + void setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid); + void setPolygonParams(const std::string & uuid); + void setCircleParams(const std::string & uuid); + + nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject( + const std::vector & uuid); + nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject( + const std::vector & uuid); + + void sendTransform(const double frame_shift); + + template + typename T::Response::SharedPtr sendRequest( + typename rclcpp::Client::SharedPtr client, + typename T::Request::SharedPtr request, + const std::chrono::nanoseconds & timeout); + + void mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map); + bool waitMap(const std::chrono::nanoseconds & timeout); + void verifyMap(bool is_circle, double poly_x_end = 1.0, double circle_cx = 3.0); + + void comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2); + void compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2); + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Service clients for calling AddShapes.srv, GetShapes.srv, RemoveShapes.srv + rclcpp::Client::SharedPtr add_shapes_client_; + rclcpp::Client::SharedPtr get_shapes_client_; + rclcpp::Client::SharedPtr remove_shapes_client_; + + // Output map subscriber + rclcpp::Subscription::SharedPtr vo_map_sub_; + // Output map published by VectorObjectServer + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + + // Vector Object server node + std::shared_ptr vo_server_; +}; // Tester + +Tester::Tester() +: map_(nullptr) +{ + vo_server_ = std::make_shared(); + + add_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/add_shapes"); + + get_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/get_shapes"); + + remove_shapes_client_ = vo_server_->create_client( + std::string(vo_server_->get_name()) + "/remove_shapes"); + + vo_map_sub_ = vo_server_->create_subscription( + "vo_map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&Tester::mapCallback, this, std::placeholders::_1)); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(vo_server_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + vo_map_sub_.reset(); + + add_shapes_client_.reset(); + get_shapes_client_.reset(); + remove_shapes_client_.reset(); + + vo_server_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setVOServerParams() +{ + vo_server_->declare_parameter( + "map_topic", rclcpp::ParameterValue("vo_map")); + vo_server_->set_parameter( + rclcpp::Parameter("map_topic", "vo_map")); + + vo_server_->declare_parameter( + "global_frame_id", rclcpp::ParameterValue("map")); + vo_server_->set_parameter( + rclcpp::Parameter("global_frame_id", "map")); + + vo_server_->declare_parameter( + "resolution", rclcpp::ParameterValue(0.1)); + vo_server_->set_parameter( + rclcpp::Parameter("resolution", 0.1)); + + vo_server_->declare_parameter( + "default_value", rclcpp::ParameterValue(nav2_util::OCC_GRID_UNKNOWN)); + vo_server_->set_parameter( + rclcpp::Parameter("default_value", nav2_util::OCC_GRID_UNKNOWN)); + + vo_server_->declare_parameter( + "overlay_type", + rclcpp::ParameterValue(static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + + vo_server_->declare_parameter( + "update_frequency", rclcpp::ParameterValue(UPDATE_FREQUENCY)); + vo_server_->set_parameter( + rclcpp::Parameter("update_frequency", UPDATE_FREQUENCY)); + + vo_server_->declare_parameter( + "transform_tolerance", rclcpp::ParameterValue(0.1)); + vo_server_->set_parameter( + rclcpp::Parameter("transform_tolerance", 0.1)); +} + +void Tester::setShapesParams(const std::string & poly_uuid, const std::string & circle_uuid) +{ + vo_server_->declare_parameter( + "shapes", rclcpp::ParameterValue(std::vector{POLYGON_NAME, CIRCLE_NAME})); + vo_server_->set_parameter( + rclcpp::Parameter("shapes", std::vector{POLYGON_NAME, CIRCLE_NAME})); + + setPolygonParams(poly_uuid); + setCircleParams(circle_uuid); +} + +void Tester::setPolygonParams(const std::string & uuid) +{ + const std::string polygon_name(POLYGON_NAME); + + vo_server_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + vo_server_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + vo_server_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + vo_server_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(POLYGON_VAL)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", POLYGON_VAL)); + + std::vector points{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0}; + vo_server_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + + if (!uuid.empty()) { + vo_server_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue(uuid)); + vo_server_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", uuid)); + } +} + +void Tester::setCircleParams(const std::string & uuid) +{ + const std::string circle_name(CIRCLE_NAME); + + vo_server_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + vo_server_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + vo_server_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + vo_server_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(CIRCLE_VAL)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".value", CIRCLE_VAL)); + + vo_server_->declare_parameter( + circle_name + ".center", rclcpp::ParameterValue(std::vector{3.0, 0.0})); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".center", std::vector{3.0, 0.0})); + + vo_server_->declare_parameter( + circle_name + ".radius", rclcpp::ParameterValue(1.0)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + if (!uuid.empty()) { + vo_server_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue(uuid)); + vo_server_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", uuid)); + } +} + +void Tester::sendTransform(const double frame_shift) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(vo_server_); + + geometry_msgs::msg::TransformStamped transform; + + // global_frame -> shape_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = SHAPE_FRAME_ID; + + transform.header.stamp = vo_server_->now(); + transform.transform.translation.x = frame_shift; + transform.transform.translation.y = 0.0; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject( + const std::vector & uuid) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared(); + po->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + po->uuid.uuid = uuid_array; + } + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = 1.0; + po->points.push_back(p); + p.x = -1.0; + po->points.push_back(p); + p.y = -1.0; + po->points.push_back(p); + p.x = 1.0; + po->points.push_back(p); + po->closed = true; + po->value = POLYGON_VAL; + + return po; +} + +nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject( + const std::vector & uuid) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared(); + co->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + co->uuid.uuid = uuid_array; + } + co->center.x = 3.0; + co->center.y = 0.0; + co->radius = 1.0; + co->fill = true; + co->value = CIRCLE_VAL; + + return co; +} + +template +typename T::Response::SharedPtr Tester::sendRequest( + typename rclcpp::Client::SharedPtr client, + typename T::Request::SharedPtr request, + const std::chrono::nanoseconds & timeout) +{ + auto result_future = client->async_send_request(request).future.share(); + + rclcpp::Time start_time = vo_server_->now(); + while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return result_future.get(); + } + rclcpp::spin_some(vo_server_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return nullptr; +} + +void Tester::mapCallback(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + map_ = map; +} + +bool Tester::waitMap(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = vo_server_->now(); + while (rclcpp::ok() && vo_server_->now() - start_time <= rclcpp::Duration(timeout)) { + if (map_) { + return true; + } + rclcpp::spin_some(vo_server_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::verifyMap(bool is_circle, double poly_x_end, double circle_cx) +{ + // Wait for map to be received + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // Map should contain polygon and circle and will look like: + // + // polygon {x1, y1} (and map's origin): shuld be always {-1.0, -1.0} + // V + // xxxxxx....xxx. + // xxxxxx...xxxxx < circle is optionally placed on map + // xxxxxx...xxxxx + // xxxxxx....xxx. + // ^ + // polygon {x2, y2}. x2 = poly_x_end; y2 - is always == 1.0 + + // Polygon {x2, y2} in map coordinates + const unsigned int m_poly_x2 = 9 + poly_x_end * 10; + const unsigned int m_poly_y2 = 19; + + // Center of the circle in map coordinates (expressed in floating-point for best precision) + const double m_circle_cx = 9 + circle_cx * 10 + 0.5; // cell's origin + 0.5 center of cell + const double m_circle_cy = 9 + 0.5; // cell's origin + 0.5 center of cell + + // Radius of the circle in map coordinates + const double m_circle_rad = 10.0; + + for (unsigned int my = 0; my < map_->info.height; my++) { + for (unsigned int mx = 0; mx < map_->info.width; mx++) { + if (mx <= m_poly_x2 && my <= m_poly_y2) { + // Point belongs to the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + } else if (is_circle && std::hypot(m_circle_cx - mx, m_circle_cy - my) <= m_circle_rad) { + // Point belongs to the circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + } else { + // Point does not belong to any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + } + } + } +} + +void Tester::comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2) +{ + ASSERT_EQ(p1->header.frame_id, p2->header.frame_id); + ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid); + ASSERT_EQ(p1->points.size(), p2->points.size()); + for (unsigned int i = 0; i < p1->points.size(); i++) { + ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON); + ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON); + } + ASSERT_EQ(p1->closed, p2->closed); + ASSERT_EQ(p1->value, p2->value); +} + +void Tester::compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2) +{ + ASSERT_EQ(c1->header.frame_id, c2->header.frame_id); + ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid); + ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON); + ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON); + ASSERT_NEAR(c1->radius, c2->radius, EPSILON); + ASSERT_EQ(c1->fill, c2->fill); + ASSERT_EQ(c1->value, c2->value); +} + +// ---------- ROS-parameters tests ---------- +TEST_F(Tester, testObtainParams) +{ + setVOServerParams(); + setShapesParams( + "01010101-0101-0101-0101-010101010101", + "01010101-0101-0101-0101-010101010102"); + vo_server_->start(); + + verifyMap(true); + + vo_server_->stop(); +} + +TEST_F(Tester, testObtainNoShapes) +{ + setVOServerParams(); + // Set shapes array, but does not set shapes themselves + vo_server_->declare_parameter( + "shapes", rclcpp::ParameterValue(std::vector{POLYGON_NAME, CIRCLE_NAME})); + vo_server_->set_parameter( + rclcpp::Parameter("shapes", std::vector{POLYGON_NAME, CIRCLE_NAME})); + + // Configured that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectShapeType) +{ + setVOServerParams(); + setShapesParams("", ""); + // Setting incorrect type of circle + vo_server_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".type", "incorrect_type")); + + // Configured that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectPolygonParams) +{ + setVOServerParams(); + // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test + setShapesParams( + "incorrect_polygon_uuid", + "01010101-0101-0101-0101-010101010102"); + + // Set that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +TEST_F(Tester, testIncorrectCircleParams) +{ + setVOServerParams(); + // Setting incorrect UUID. Other incorrect params to be checked inside Vector Object Shapes test + setShapesParams( + "01010101-0101-0101-0101-010101010101", + "incorrect_circle_uuid"); + + // Set that way, vo_server_ should give the failure on configuration stage + vo_server_->configureFail(); + + // Cleaning-up vo_server_ w/o deactivation + vo_server_->cleanup(); +} + +// ---------- Service call tests ---------- +TEST_F(Tester, testAddShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Now move X-coordinate of polygon's border and + // add a new circle fully plaecd inside first one + // through AddShapes.srv: + // Update polygon x2-coordinate to 1.5 + po_msg->points[0].x = 1.5; + po_msg->points[3].x = 1.5; + // Prepare second circle object (fully placed inside first circle) + auto co2_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3}); + co2_msg->radius = 0.5; + + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_msg->circles.push_back(*co2_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check the map has been updated correctly + verifyMap(true, 1.5); + + // Check that getShapes.srv will return correct shapes + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circles + ASSERT_EQ(get_shapes_result->circles.size(), 2u); + c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + c_check = std::make_shared(get_shapes_result->circles[1]); + compareCircleObjects(c_check, co2_msg); + + vo_server_->stop(); +} + +TEST_F(Tester, testRemoveShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Now remove circle from map + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = false; + unique_identifier_msgs::msg::UUID uuid; + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}; + remove_shapes_msg->uuids.push_back(uuid); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Check that there is only polygon remained on map + verifyMap(false); + + // Check that getShapes.srv will return only polygon + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + // Then call RemoveShapes.srv with enabled all_objects parameter + remove_shapes_msg->all_objects = true; + remove_shapes_msg->uuids.clear(); + map_.reset(); // Resetting the map to ensure that updated one is received later + remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Check that map is empty + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN); + + // getShapes.srv should return empty vectors + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + ASSERT_EQ(get_shapes_result->polygons.size(), 0u); + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + vo_server_->stop(); +} + +TEST_F(Tester, testAddIncorrectShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Then try to update polygon with circle's uuid + po_msg->uuid.uuid[15] = 2; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + po_msg->uuid.uuid[15] = 1; // Restoring back for further usage + + // Then try to update circle with polygon's uuid + co_msg->uuid.uuid[15] = 1; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + co_msg->uuid.uuid[15] = 2; // Restoring back for further usage + + // Try to update polygon with incorrect number of points + po_msg->points.resize(2); + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + + // Try to add new incorrect polygon + po_msg->uuid.uuid[15] = 100; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + // Restoring back for further usage + po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + + // Try to update circle with incorrect radius + co_msg->radius = -1.0; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + + // Try to add new incorrect circle + co_msg->uuid.uuid[15] = 100; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.clear(); + add_shapes_msg->circles.push_back(*co_msg); + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_FALSE(add_shapes_result->success); + co_msg->radius = 1.0; // Restoring back for further usage + co_msg->uuid.uuid[15] = 2; // Restoring back for further usage + + // Verify that map did not corrupted after all false-manipulations + verifyMap(true); + + // Check that getShapes.srv will return correct shapes after all false-manipulations + get_shapes_msg = std::make_shared(); + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + vo_server_->stop(); +} + +TEST_F(Tester, testRemoveIncorrectShapes) +{ + setVOServerParams(); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that getShapes.srv will return correct shapes + auto get_shapes_msg = std::make_shared(); + auto get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + auto p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 1u); + auto c_check = std::make_shared(get_shapes_result->circles[0]); + compareCircleObjects(c_check, co_msg); + + // Then try to remove two shapes: non-existing shape and circle + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = false; + unique_identifier_msgs::msg::UUID uuid; + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 100}; + remove_shapes_msg->uuids.push_back(uuid); + uuid.uuid = std::array{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}; + remove_shapes_msg->uuids.push_back(uuid); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_FALSE(remove_shapes_result->success); + + // Check that despite on the error, the circle was removed from map + verifyMap(false); + + // Check that getShapes.srv will return only polygon + get_shapes_result = + sendRequest(get_shapes_client_, get_shapes_msg, 2s); + ASSERT_NE(get_shapes_result, nullptr); + // Verify obtained polygon + ASSERT_EQ(get_shapes_result->polygons.size(), 1u); + p_check = std::make_shared(get_shapes_result->polygons[0]); + comparePolygonObjects(p_check, po_msg); + // Verify obtained circle + ASSERT_EQ(get_shapes_result->circles.size(), 0u); + + vo_server_->stop(); +} + +// ---------- Overlay tests ---------- +TEST_F(Tester, testOverlayMax) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_MAX))); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that ovelrap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 14; // in the overlapping area + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 20; // inside circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlayMin) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_MIN))); + vo_server_->start(); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that ovelrap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 14; // in the overlapping area + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + mx = 20; // inside circle + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlaySeq) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter( + "overlay_type", + static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); + vo_server_->start(); + + // Sequentially add polygon and then circle overlapped with it on map + auto add_shapes_msg = std::make_shared(); + + // Prepare first polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + add_shapes_msg->polygons.push_back(*po_msg); + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Then prepare circle object to add over the polygon + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + // Make circle to be overlapped with polygon + co_msg->center.x = 1.0; + // Also check that putBorders part works correctly + co_msg->fill = false; + add_shapes_msg->polygons.clear(); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Verify that ovelrap on map generated correctly + double my = 9; + double mx = 1; // inside polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 10; // on the circle border laying over the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + mx = 14; // inside circle and polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], POLYGON_VAL); + mx = 24; // inside circle only + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + mx = 29; // on the circle border laying outside the polygon + ASSERT_EQ(map_->data[my * map_->info.width + mx], CIRCLE_VAL); + my = 18; // outside any shape + mx = 28; // outside any shape + ASSERT_EQ(map_->data[my * map_->info.width + mx], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +TEST_F(Tester, testOverlayUnknown) +{ + setVOServerParams(); + vo_server_->set_parameter( + rclcpp::Parameter("overlay_type", static_cast(1000))); + vo_server_->start(); + + // Try to add polygon on map + auto add_shapes_msg = std::make_shared(); + auto po_msg = makePolygonObject(std::vector()); + add_shapes_msg->polygons.push_back(*po_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Wait for the map + waitMap(500ms); + + // Check that polygon was not added on map: map is empty + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + ASSERT_EQ(map_->data[0], nav2_util::OCC_GRID_UNKNOWN); + + vo_server_->stop(); +} + +// ---------- Transform/dynamic tests ---------- +TEST_F(Tester, testShapesMove) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // No shift between polygon and map + sendTransform(0.0); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Polygon and circle to be in another moving frame + po_msg->header.frame_id = SHAPE_FRAME_ID; + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that polygon and circle are in correct positions on map + verifyMap(true); + + // Move frame with polygon + sendTransform(0.5); + + // Map is being pusblished dynamically. Wait for the map to be published one more time + map_.reset(); + // Check that polygon and circle are in correct positions on map + verifyMap(true, 1.0, 2.5); // Polygon is moved 0.5m towards to the circle + // Check that map's origin was updated in accordance to polygon movement + ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON); // -1.0 + 0.5 + ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON); + + // Then try to publish polygon in incorrect frame + add_shapes_msg->polygons[0].header.frame_id = "incorrect_frame"; + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Having incorrect transform, map should not be published + ASSERT_FALSE(waitMap(timeout)); + + vo_server_->stop(); +} + +TEST_F(Tester, testSwitchDynamicStatic) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // 0.5m shift between polygon and map + sendTransform(0.5); + + // Add polygon and circle on map + auto add_shapes_msg = std::make_shared(); + // Prepare polygon object to add + auto po_msg = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}); + // Polygon to be in different frame + po_msg->header.frame_id = SHAPE_FRAME_ID; + // Prepare circle object to add + auto co_msg = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2}); + + add_shapes_msg->polygons.push_back(*po_msg); + add_shapes_msg->circles.push_back(*co_msg); + map_.reset(); // Resetting the map to ensure that updated one is received later + auto add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + // Check that polygon and circle are in correct positions on map + verifyMap(true, 1.0, 2.5); // Polygon is moved 0.5m towards to the circle + // Check that map's origin was updated in accordance to polygon movement + ASSERT_NEAR(map_->info.origin.position.x, -0.5, EPSILON); // -1.0 + 0.5 + ASSERT_NEAR(map_->info.origin.position.y, -1.0, EPSILON); + + // Then return to the static model and ensure the everything works correctly + add_shapes_msg->polygons[0].header.frame_id = GLOBAL_FRAME_ID; + map_.reset(); // Resetting the map to ensure that updated one is received later + add_shapes_result = + sendRequest(add_shapes_client_, add_shapes_msg, 2s); + ASSERT_NE(add_shapes_result, nullptr); + ASSERT_TRUE(add_shapes_result->success); + + verifyMap(true); + + vo_server_->stop(); +} + +// ---------- Corner cases ---------- +TEST_F(Tester, switchProcessMap) +{ + setVOServerParams(); + vo_server_->start(); + + // Wait for the initial map to be received to not get it in later updates + const std::chrono::nanoseconds timeout = + std::chrono::duration_cast(1s * 1.5 / UPDATE_FREQUENCY); + ASSERT_TRUE(waitMap(timeout)); + + // Check that received default map is empty + ASSERT_EQ(map_->header.frame_id, "map"); + ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON); + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + + // Disable map processing and trying to obtain the map + vo_server_->setProcessMap(false); + + // Switching map update by calling remove service + auto remove_shapes_msg = std::make_shared(); + remove_shapes_msg->all_objects = true; + map_.reset(); // Resetting the map to ensure that updated one is received later + auto remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Having process_map_ disabled, map should not be published + ASSERT_FALSE(waitMap(timeout)); + + // Then enable map processing and trying to obtain the map + vo_server_->setProcessMap(true); + + // Switching map update by calling remove service + map_.reset(); // Resetting the map to ensure that updated one is received later + remove_shapes_result = + sendRequest(remove_shapes_client_, remove_shapes_msg, 2s); + ASSERT_NE(remove_shapes_result, nullptr); + ASSERT_TRUE(remove_shapes_result->success); + + // Ensure that map is being updated + ASSERT_TRUE(waitMap(timeout)); + + // ... and it is empty + ASSERT_EQ(map_->header.frame_id, "map"); + ASSERT_NEAR(map_->info.resolution, 0.1, EPSILON); + ASSERT_EQ(map_->info.width, 1); + ASSERT_EQ(map_->info.height, 1); + + vo_server_->stop(); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_map_server/test/unit/test_vector_object_shapes.cpp b/nav2_map_server/test/unit/test_vector_object_shapes.cpp new file mode 100644 index 0000000000..85ee9b908c --- /dev/null +++ b/nav2_map_server/test/unit/test_vector_object_shapes.cpp @@ -0,0 +1,767 @@ +// 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 +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" + +#include "nav2_msgs/msg/polygon_object.hpp" +#include "nav2_msgs/msg/circle_object.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/occ_grid_utils.hpp" + +#include "nav2_map_server/vector_object_shapes.hpp" +#include "nav2_map_server/vector_object_utils.hpp" + +static constexpr double EPSILON = std::numeric_limits::epsilon(); + +static const char POLYGON_NAME[]{"Polygon"}; +static const char CIRCLE_NAME[]{"Circle"}; +static const char GLOBAL_FRAME_ID[]{"map"}; +static const char SHAPE_FRAME_ID[]{"shape"}; +static double FRAME_SHIFT = 0.1; +static std::vector POINTS{1.0, 1.0, -1.0, 1.0, -1.0, -1.0, 1.0, -1.0}; +static std::vector CENTER{0.0, 0.0}; + +class PolygonWrapper : public nav2_map_server::Polygon +{ +public: + explicit PolygonWrapper(const nav2_util::LifecycleNode::WeakPtr & node) + : Polygon(node) + {} + + geometry_msgs::msg::Polygon::SharedPtr getPoly() + { + return polygon_; + } +}; // PolygonWrapper + +class CircleWrapper : public nav2_map_server::Circle +{ +public: + explicit CircleWrapper(const nav2_util::LifecycleNode::WeakPtr & node) + : Circle(node) + {} + + geometry_msgs::msg::Point32::SharedPtr getCenter() + { + return center_; + } +}; // CircleWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + +protected: + void setPolygonParams(const std::string & uuid); + void setCircleParams(const std::string & uuid); + nav2_msgs::msg::PolygonObject::SharedPtr makePolygonObject( + const std::vector & uuid); + nav2_msgs::msg::CircleObject::SharedPtr makeCircleObject( + const std::vector & uuid); + + void sendTransform(); + + nav_msgs::msg::OccupancyGrid::SharedPtr makeMap(); + void verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map); + void verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map); + void verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map); + + void comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2); + void compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2); + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::shared_ptr polygon_; + std::shared_ptr circle_; + + nav2_util::LifecycleNode::SharedPtr node_; +}; // Tester + +Tester::Tester() +{ + node_ = std::make_shared("test_node"); + + // Create shapes + polygon_ = std::make_shared(node_); + circle_ = std::make_shared(node_); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + polygon_.reset(); + circle_.reset(); + + node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::setPolygonParams(const std::string & uuid) +{ + const std::string polygon_name(POLYGON_NAME); + + node_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + node_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + node_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(POINTS)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", POINTS)); + + if (!uuid.empty()) { + node_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue(uuid)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", uuid)); + } +} + +void Tester::setCircleParams(const std::string & uuid) +{ + const std::string circle_name(CIRCLE_NAME); + + node_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + node_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + node_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + circle_name + ".center", rclcpp::ParameterValue(CENTER)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + node_->declare_parameter( + circle_name + ".radius", rclcpp::ParameterValue(1.0)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + if (!uuid.empty()) { + node_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue(uuid)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", uuid)); + } +} + +nav2_msgs::msg::PolygonObject::SharedPtr Tester::makePolygonObject( + const std::vector & uuid) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = std::make_shared(); + po->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + po->uuid.uuid = uuid_array; + } + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = 1.0; + po->points.push_back(p); + p.x = -1.0; + po->points.push_back(p); + p.y = -1.0; + po->points.push_back(p); + p.x = 1.0; + po->points.push_back(p); + po->closed = true; + po->value = nav2_util::OCC_GRID_OCCUPIED; + + return po; +} + +nav2_msgs::msg::CircleObject::SharedPtr Tester::makeCircleObject( + const std::vector & uuid) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = std::make_shared(); + co->header.frame_id = GLOBAL_FRAME_ID; + if (uuid.size() == 16) { + // uuid could be optionally specified + std::array uuid_array; + std::copy(uuid.begin(), uuid.end(), uuid_array.begin()); + co->uuid.uuid = uuid_array; + } + co->center.x = 0.0; + co->center.y = 0.0; + co->radius = 1.0; + co->fill = true; + co->value = nav2_util::OCC_GRID_OCCUPIED; + + return co; +} + +void Tester::sendTransform() +{ + std::shared_ptr tf_broadcaster = + std::make_shared(node_); + + geometry_msgs::msg::TransformStamped transform; + + // global_frame -> shape_frame transform + transform.header.frame_id = GLOBAL_FRAME_ID; + transform.child_frame_id = SHAPE_FRAME_ID; + + transform.header.stamp = node_->now(); + transform.transform.translation.x = FRAME_SHIFT; + transform.transform.translation.y = FRAME_SHIFT; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +nav_msgs::msg::OccupancyGrid::SharedPtr Tester::makeMap() +{ + nav_msgs::msg::OccupancyGrid::SharedPtr map = std::make_shared(); + map->header.frame_id = GLOBAL_FRAME_ID; + map->info.resolution = 0.1; + map->info.width = 40; + map->info.height = 40; + map->info.origin.position.x = -2.0; + map->info.origin.position.y = -2.0; + map->data = std::vector(400 * 400, nav2_util::OCC_GRID_FREE); + + return map; +} + +void Tester::verifyPolygonBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + // Map is expected to be as follows: + // 0 0 0 0 0 0 0 0 + // 0 0 x x x x 0 0 + // 0 0 x 0 0 0 0 0 + // 0 0 x 0 0 0 0 0 + // 0 0 x x x x 0 0 + // 0 0 0 0 0 0 0 0 + const unsigned int map_center_x = 19; + const unsigned int map_center_y = 19; + + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + if (my == map_center_y - 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) { + // 1st border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else if (mx == map_center_x - 10 && my >= map_center_y - 10 && my <= map_center_y + 10) { + // 2nd border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else if (my == map_center_y + 10 && mx >= map_center_x - 10 && mx <= map_center_x + 10) { + // 3rd border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_OCCUPIED); + } else { + // Does not belong to any border + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE); + } + } + } +} + +void Tester::verifyCircleBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + // Circle center in cell coordinates + const double circle_center_x = 19.5; // 19 cell's origin + 0.5 center of cell + const double circle_center_y = 19.5; // 19 cell's origin + 0.5 center of cell + + double radius; + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + if (map->data[my * map->info.width + mx] == nav2_util::OCC_GRID_OCCUPIED) { + radius = std::hypot(circle_center_x - mx, circle_center_y - my); + ASSERT_NEAR(radius, 10.0, 1.0); // Border drift no more than once cell + } + } + } +} + +void Tester::verifyMapEmpty(nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + for (unsigned int my = 0; my < map->info.height; my++) { + for (unsigned int mx = 0; mx < map->info.width; mx++) { + ASSERT_EQ(map->data[my * map->info.width + mx], nav2_util::OCC_GRID_FREE); + } + } +} + +void Tester::comparePolygonObjects( + nav2_msgs::msg::PolygonObject::SharedPtr p1, + nav2_msgs::msg::PolygonObject::SharedPtr p2) +{ + ASSERT_EQ(p1->header.frame_id, p2->header.frame_id); + ASSERT_EQ(p1->uuid.uuid, p2->uuid.uuid); + ASSERT_EQ(p1->points.size(), p2->points.size()); + for (unsigned int i = 0; i < p1->points.size(); i++) { + ASSERT_NEAR(p1->points[i].x, p2->points[i].x, EPSILON); + ASSERT_NEAR(p1->points[i].y, p2->points[i].y, EPSILON); + } + ASSERT_EQ(p1->closed, p2->closed); + ASSERT_EQ(p1->value, p2->value); +} + +void Tester::compareCircleObjects( + nav2_msgs::msg::CircleObject::SharedPtr c1, + nav2_msgs::msg::CircleObject::SharedPtr c2) +{ + ASSERT_EQ(c1->header.frame_id, c2->header.frame_id); + ASSERT_EQ(c1->uuid.uuid, c2->uuid.uuid); + ASSERT_NEAR(c1->center.x, c2->center.x, EPSILON); + ASSERT_NEAR(c1->center.y, c2->center.y, EPSILON); + ASSERT_NEAR(c1->radius, c2->radius, EPSILON); + ASSERT_EQ(c1->fill, c2->fill); + ASSERT_EQ(c1->value, c2->value); +} + +//---------- Polygon testcases ---------- + +TEST_F(Tester, testPolygonObtainParams) +{ + setPolygonParams("01010101-0101-0101-0101-010101010101"); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON); + ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(polygon_->isFill(), true); + ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-010101010101"); +} + +TEST_F(Tester, testPolygonObtainIncorrectParams) +{ + // Setting polygon parameters w/o points + const std::string polygon_name(POLYGON_NAME); + + node_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + node_->declare_parameter( + polygon_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + polygon_name + ".closed", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".closed", true)); + + node_->declare_parameter( + polygon_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + polygon_name + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + + // Check that points is mandatory parameter for the polygon + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); + + // Setting incorrect number of points + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", std::vector{1.0, 2.0, 3.0})); + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", POINTS)); + + // Setting incorrect UUID + node_->declare_parameter( + polygon_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid")); + node_->set_parameter( + rclcpp::Parameter(polygon_name + ".uuid", "incorrect-uuid")); + ASSERT_FALSE(polygon_->obtainParams(polygon_name)); +} + +TEST_F(Tester, testPolygonSetParams) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2}); + + // Check that parameters were set correctly + ASSERT_TRUE(polygon_->setParams(po)); + ASSERT_EQ(polygon_->getType(), nav2_map_server::POLYGON); + ASSERT_EQ(polygon_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(polygon_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(polygon_->isFill(), true); + ASSERT_EQ(polygon_->getUUID(), "01010101-0101-0101-0101-020202020202"); + + // Verify that getting the parameters also works correctly + nav2_msgs::msg::PolygonObject::SharedPtr params = polygon_->getParams(); + comparePolygonObjects(params, po); +} + +TEST_F(Tester, testPolygonSetIncorrectParams) +{ + nav2_msgs::msg::PolygonObject::SharedPtr po = makePolygonObject( + std::vector()); + // Setting incorrect number of vertices + po->points.resize(2); + + // And check that it is failed to set these parameters + ASSERT_FALSE(polygon_->setParams(po)); +} + +TEST_F(Tester, testPolygonBoundaries) +{ + setPolygonParams(""); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + double min_x, min_y, max_x, max_y; + polygon_->getBoundaries(min_x, min_y, max_x, max_y); + + ASSERT_NEAR(min_x, -1.0, EPSILON); + ASSERT_NEAR(min_y, -1.0, EPSILON); + ASSERT_NEAR(max_x, 1.0, EPSILON); + ASSERT_NEAR(max_y, 1.0, EPSILON); +} + +TEST_F(Tester, testPolygonPoints) +{ + setPolygonParams(""); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + ASSERT_TRUE(polygon_->isPointInside(0.0, 0.0)); + ASSERT_FALSE(polygon_->isPointInside(-2.0, -2.0)); + ASSERT_FALSE(polygon_->isPointInside(2.0, 2.0)); +} + +TEST_F(Tester, testPolygonBorders) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyPolygonBorders(map); +} + +TEST_F(Tester, testPolygonBordersOutOfBoundaries) +{ + setPolygonParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".closed", false)); + // Set polygon to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(POLYGON_NAME) + ".points", + std::vector{5.0, 5.0, 6.0, 5.0, 5.0, 6.0})); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + polygon_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testPolygonDifferentFrame) +{ + setPolygonParams(""); + // Change shape's frame ID to differ from global one + node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".frame_id", SHAPE_FRAME_ID)); + ASSERT_TRUE(polygon_->obtainParams(POLYGON_NAME)); + + sendTransform(); + + // Check that shape coordinates are not transformed + geometry_msgs::msg::Polygon::SharedPtr poly = polygon_->getPoly(); + ASSERT_NEAR(poly->points[0].x, 1.0, EPSILON); + ASSERT_NEAR(poly->points[0].y, 1.0, EPSILON); + ASSERT_NEAR(poly->points[1].x, -1.0, EPSILON); + ASSERT_NEAR(poly->points[1].y, 1.0, EPSILON); + ASSERT_NEAR(poly->points[2].x, -1.0, EPSILON); + ASSERT_NEAR(poly->points[2].y, -1.0, EPSILON); + ASSERT_NEAR(poly->points[3].x, 1.0, EPSILON); + ASSERT_NEAR(poly->points[3].y, -1.0, EPSILON); + + // Transform shape coordinates to global frame + ASSERT_TRUE(polygon_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0)); + + // Verify that shape coordinates were transformed to global frame successfully + poly = polygon_->getPoly(); + ASSERT_NEAR(poly->points[0].x, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[0].y, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[1].x, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[1].y, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[2].x, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[2].y, -1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[3].x, 1.0 + FRAME_SHIFT, EPSILON); + ASSERT_NEAR(poly->points[3].y, -1.0 + FRAME_SHIFT, EPSILON); + + // Try to transform to incorrect frame + ASSERT_FALSE(polygon_->toFrame("incorrect_frame", tf_buffer_, 0.1)); +} + +//---------- Circles testcases ---------- + +TEST_F(Tester, testCircleObtainParams) +{ + setCircleParams("01010101-0101-0101-0101-010101010101"); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE); + ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(circle_->isFill(), true); + ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-010101010101"); +} + +TEST_F(Tester, testCircleObtainIncorrectParams) +{ + const std::string circle_name(CIRCLE_NAME); + + // Setting circle parameters w/o center and radius + node_->declare_parameter( + circle_name + ".type", rclcpp::ParameterValue("circle")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".type", "circle")); + + node_->declare_parameter( + circle_name + ".frame_id", rclcpp::ParameterValue(GLOBAL_FRAME_ID)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".frame_id", GLOBAL_FRAME_ID)); + + node_->declare_parameter( + circle_name + ".fill", rclcpp::ParameterValue(true)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".fill", true)); + + node_->declare_parameter( + circle_name + ".value", rclcpp::ParameterValue(nav2_util::OCC_GRID_OCCUPIED)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".value", nav2_util::OCC_GRID_OCCUPIED)); + + node_->declare_parameter( + circle_name + ".center", rclcpp::PARAMETER_DOUBLE_ARRAY); + node_->declare_parameter( + circle_name + ".radius", rclcpp::PARAMETER_DOUBLE); + + // Check that center and radius are mandatory parameter for the circle + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + // Setting incorrect radius + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", -1.0)); + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".radius", 1.0)); + + // Setting incorrect center format + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", std::vector{0.0})); + ASSERT_FALSE(circle_->obtainParams(circle_name)); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".center", CENTER)); + + // Setting incorrect UUID + node_->declare_parameter( + circle_name + ".uuid", rclcpp::ParameterValue("incorrect-uuid")); + node_->set_parameter( + rclcpp::Parameter(circle_name + ".uuid", "incorrect-uuid")); + ASSERT_FALSE(circle_->obtainParams(circle_name)); +} + +TEST_F(Tester, testCircleSetParams) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject( + std::vector{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2}); + + // Check that parameters were set correctly + ASSERT_TRUE(circle_->setParams(co)); + ASSERT_EQ(circle_->getType(), nav2_map_server::CIRCLE); + ASSERT_EQ(circle_->getValue(), nav2_util::OCC_GRID_OCCUPIED); + ASSERT_EQ(circle_->getFrameID(), GLOBAL_FRAME_ID); + ASSERT_EQ(circle_->isFill(), true); + ASSERT_EQ(circle_->getUUID(), "01010101-0101-0101-0101-020202020202"); + + // Verify that getting the parameters also works correctly + nav2_msgs::msg::CircleObject::SharedPtr params = circle_->getParams(); + compareCircleObjects(params, co); +} + +TEST_F(Tester, testCircleSetIncorrectParams) +{ + nav2_msgs::msg::CircleObject::SharedPtr co = makeCircleObject( + std::vector()); + // Setting incorrect radius + co->radius = -1.0; + + // And check that it is failed to set these parameters + ASSERT_FALSE(circle_->setParams(co)); +} + +TEST_F(Tester, testCircleBoundaries) +{ + setCircleParams(""); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + double min_x, min_y, max_x, max_y; + circle_->getBoundaries(min_x, min_y, max_x, max_y); + + ASSERT_NEAR(min_x, -1.0, EPSILON); + ASSERT_NEAR(min_y, -1.0, EPSILON); + ASSERT_NEAR(max_x, 1.0, EPSILON); + ASSERT_NEAR(max_y, 1.0, EPSILON); +} + +TEST_F(Tester, testCirclePoints) +{ + setCircleParams(""); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + ASSERT_TRUE(circle_->isPointInside(0.0, 0.0)); + ASSERT_FALSE(circle_->isPointInside(-1.0, -1.0)); + ASSERT_FALSE(circle_->isPointInside(1.0, 1.0)); +} + +TEST_F(Tester, testCircleBorders) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyCircleBorders(map); +} + +TEST_F(Tester, testCircleBordersOutOfBoundaries) +{ + setCircleParams(""); + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".fill", false)); + // Set circle to be out of map + node_->set_parameter( + rclcpp::Parameter( + std::string(CIRCLE_NAME) + ".center", + std::vector{5.0, 5.0})); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + nav_msgs::msg::OccupancyGrid::SharedPtr map = makeMap(); + + circle_->putBorders(map, nav2_map_server::OverlayType::OVERLAY_SEQ); + + verifyMapEmpty(map); +} + +TEST_F(Tester, testCircleDifferentFrame) +{ + setCircleParams(""); + // Change shape's frame ID to differ from global one + node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".frame_id", SHAPE_FRAME_ID)); + ASSERT_TRUE(circle_->obtainParams(CIRCLE_NAME)); + + sendTransform(); + + // Check that shape coordinates are not transformed + geometry_msgs::msg::Point32::SharedPtr center = circle_->getCenter(); + ASSERT_NEAR(center->x, 0.0, EPSILON); + ASSERT_NEAR(center->y, 0.0, EPSILON); + // Transform shape coordinates to global frame + ASSERT_TRUE(circle_->toFrame(GLOBAL_FRAME_ID, tf_buffer_, 1.0)); + + // Verify that shape coordinates were transformed to global frame successfully + center = circle_->getCenter(); + ASSERT_NEAR(center->x, FRAME_SHIFT, EPSILON); + ASSERT_NEAR(center->y, FRAME_SHIFT, EPSILON); + + // Try to transform to incorrect frame + ASSERT_FALSE(circle_->toFrame("incorrect_frame", tf_buffer_, 0.1)); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} From 2bf3fb715ea3280dbcb0199430b0d0f063f7bd98 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Mon, 20 Nov 2023 16:15:27 +0300 Subject: [PATCH 16/19] Fix linting issues Signed-off-by: Alexey Merzlyakov --- .../include/nav2_map_server/vector_object_shapes.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 index 7dff9c7125..d7fabec4f9 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -173,7 +173,7 @@ class Polygon : public Shape * @param node Vector Object server node pointer * @note setParams()/obtainParams() should be called after to configure the shape */ - Polygon(const nav2_util::LifecycleNode::WeakPtr & node); + explicit Polygon(const nav2_util::LifecycleNode::WeakPtr & node); /** * @brief Supporting routine obtaining ROS-parameters for the given vector object. @@ -283,7 +283,7 @@ class Circle : public Shape * @param node Vector Object server node pointer * @note setParams()/obtainParams() should be called after to configure the shape */ - Circle(const nav2_util::LifecycleNode::WeakPtr & node); + explicit Circle(const nav2_util::LifecycleNode::WeakPtr & node); /** * @brief Supporting routine obtaining ROS-parameters for the given vector object. From 3197856a6fa27f6fb66e2d2d923f8acc639707d0 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 30 Nov 2023 07:45:39 +0300 Subject: [PATCH 17/19] Adjust for Vector Objects demonstration Signed-off-by: Alexey Merzlyakov --- .../launch/vector_object_server.launch.py | 27 ++++++++++++++++--- .../params/vector_object_server_params.yaml | 27 +++++++++---------- 2 files changed, 36 insertions(+), 18 deletions(-) diff --git a/nav2_map_server/launch/vector_object_server.launch.py b/nav2_map_server/launch/vector_object_server.launch.py index 54c012e46e..c07c26f93e 100644 --- a/nav2_map_server/launch/vector_object_server.launch.py +++ b/nav2_map_server/launch/vector_object_server.launch.py @@ -35,8 +35,7 @@ def generate_launch_description(): package_dir = get_package_share_directory('nav2_map_server') # Constant parameters - lifecycle_nodes = ['vector_object_server'] - autostart = True + lifecycle_nodes = ['vector_object_server', 'costmap_filter_info_server'] remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -45,9 +44,9 @@ def generate_launch_description(): namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') container_name = LaunchConfiguration('container_name') - container_name_full = (namespace, '/', container_name) # 2. Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( @@ -65,6 +64,10 @@ def generate_launch_description(): 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') + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='True', + description='Automatically startup Vector Object server') + declare_use_composition_cmd = DeclareLaunchArgument( 'use_composition', default_value='True', description='Use composed bringup if True') @@ -101,6 +104,15 @@ def generate_launch_description(): Node( package='nav2_map_server', executable='vector_object_server', + name='vector_object_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 parameters=[configured_params], @@ -116,7 +128,7 @@ def generate_launch_description(): condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), namespace=namespace), LoadComposableNodes( - target_container=container_name_full, + target_container=container_name, composable_node_descriptions=[ ComposableNode( package='nav2_lifecycle_manager', @@ -130,6 +142,12 @@ def generate_launch_description(): plugin='nav2_map_server::VectorObjectServer', name='vector_object_server', parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::CostmapFilterInfoServer', + name='costmap_filter_info_server', + parameters=[configured_params], remappings=remappings) ], ) @@ -142,6 +160,7 @@ def generate_launch_description(): ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_composition_cmd) ld.add_action(declare_container_name_cmd) diff --git a/nav2_map_server/params/vector_object_server_params.yaml b/nav2_map_server/params/vector_object_server_params.yaml index 31b854c30f..2b9e912f03 100644 --- a/nav2_map_server/params/vector_object_server_params.yaml +++ b/nav2_map_server/params/vector_object_server_params.yaml @@ -7,25 +7,24 @@ vector_object_server: overlay_type: 0 update_frequency: 1.0 transform_tolerance: 0.1 - shapes: ["Poly", "CircleA", "CircleB"] + shapes: ["Poly", "Circle"] 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: + points: [0.3, 0.5, -0.4, 1.2, -0.4, -0.2] + Circle: 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 + value: 100 + center: [1.5, 0.5] + radius: 0.7 +costmap_filter_info_server: + ros__parameters: + type: 0 + filter_info_topic: "costmap_filter_info" + mask_topic: "vo_map" + base: 0.0 + multiplier: 1.0 From e2883b6c16a1e5e814819443124eff404afcdbbf Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 30 Nov 2023 15:55:23 +0300 Subject: [PATCH 18/19] Code clean-up * Corrected headers * Functions ordering * Comment fixes Signed-off-by: Alexey Merzlyakov --- .../nav2_map_server/vector_object_server.hpp | 19 +- .../nav2_map_server/vector_object_shapes.hpp | 229 ++++++----- .../nav2_map_server/vector_object_utils.hpp | 1 + .../src/vo_server/vector_object_server.cpp | 121 +++--- .../src/vo_server/vector_object_shapes.cpp | 373 +++++++++--------- 5 files changed, 374 insertions(+), 369 deletions(-) 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 index 39afec55cf..7e94072c8e 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -19,6 +19,7 @@ #include #include +#include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "tf2_ros/buffer.h" @@ -28,7 +29,6 @@ #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" @@ -79,6 +79,12 @@ class VectorObjectServer : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief Supporting routine obtaining all ROS-parameters + * @return True if all parameters were obtained or false in the failure case + */ + bool obtainParams(); + /** * @brief Finds the shape with given UUID * @param uuid Given UUID to search with @@ -88,6 +94,7 @@ class VectorObjectServer : public nav2_util::LifecycleNode /** * @brief Transform all vector shapes from their local frame to output map frame + * @return Whether all vector objects were transformed successfully */ bool transformVectorObjects(); @@ -124,8 +131,8 @@ class VectorObjectServer : public nav2_util::LifecycleNode void publishMap(); /** - * @brief Calculates new map sizes, updates map, process all vector objects on it - * and publish output map one time + * @brief Calculates new map sizes, updates map, processes all vector objects on it + * and publishes output map one time */ void processMap(); @@ -174,12 +181,6 @@ class VectorObjectServer : public nav2_util::LifecycleNode 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 obtainParams(); - /// @brief TF buffer std::shared_ptr tf_buffer_; /// @brief TF listener 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 index d7fabec4f9..aeae185553 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -15,11 +15,10 @@ #ifndef NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ #define NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ -#include #include #include -#include +#include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon.hpp" #include "geometry_msgs/msg/point32.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -71,42 +70,6 @@ class Shape */ bool obtainShapeUUID(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 obtainParams(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 @@ -143,6 +106,14 @@ class Shape */ virtual bool isFill() const = 0; + /** + * @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 obtainParams(const std::string & shape_name) = 0; + /** * @brief Transforms shape coordinates to a new frame. * Empty virtual method intended to be used in child implementations @@ -156,67 +127,52 @@ class Shape 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 - * @note setParams()/obtainParams() should be called after to configure the shape - */ - explicit Polygon(const nav2_util::LifecycleNode::WeakPtr & node); - - /** - * @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 obtainParams(const std::string & shape_name); - /** - * @brief Gets shape boundaries + * @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 */ - void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + 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 */ - bool isPointInside(const double px, const double py) const; + 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 */ - void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + virtual void putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0; - /** - * @brief Gets Polygon parameters - * @return Polygon parameters - */ - nav2_msgs::msg::PolygonObject::SharedPtr getParams() const; +protected: + /// @brief Type of shape + ShapeType type_; - /** - * @brief Tries to update Polygon parameters - * @return False in case of inconsistent shape + /// @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 + * @note setParams()/obtainParams() should be called after to configure the shape */ - bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); + explicit Polygon(const nav2_util::LifecycleNode::WeakPtr & node); /** * @brief Gets the value of the shape. @@ -249,6 +205,25 @@ class Polygon : public Shape */ bool isFill() const; + /** + * @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 obtainParams(const std::string & shape_name); + + /** + * @brief Gets Polygon parameters + * @return Polygon parameters + */ + nav2_msgs::msg::PolygonObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Polygon parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); + /** * @brief Transforms shape coordinates to a new frame. * @param to_frame Frame ID to transform to @@ -261,37 +236,6 @@ class Polygon : public Shape const std::shared_ptr tf_buffer, const double transform_tolerance); -protected: - /** - * @brief Checks that shape is consistent for further operation - * @return False in case of inconsistent shape - */ - bool checkConsistency(); - - /// @brief Input polygon parameters (could be in any frame) - nav2_msgs::msg::PolygonObject::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 - * @note setParams()/obtainParams() should be called after to configure the shape - */ - explicit Circle(const nav2_util::LifecycleNode::WeakPtr & node); - - /** - * @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 obtainParams(const std::string & shape_name); - /** * @brief Gets shape boundaries * @param min_x output min X-boundary of shape @@ -316,17 +260,29 @@ class Circle : public Shape */ void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); +protected: /** - * @brief Gets Circle parameters - * @return Circle parameters + * @brief Checks that shape is consistent for further operation + * @return False in case of inconsistent shape */ - nav2_msgs::msg::CircleObject::SharedPtr getParams() const; + bool checkConsistency(); - /** - * @brief Tries to update Circle parameters - * @return False in case of inconsistent shape + /// @brief Input polygon parameters (could be in any frame) + nav2_msgs::msg::PolygonObject::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 + * @note setParams()/obtainParams() should be called after to configure the shape */ - bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); + explicit Circle(const nav2_util::LifecycleNode::WeakPtr & node); /** * @brief Gets the value of the shape. @@ -359,6 +315,25 @@ class Circle : public Shape */ bool isFill() const; + /** + * @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 obtainParams(const std::string & shape_name); + + /** + * @brief Gets Circle parameters + * @return Circle parameters + */ + nav2_msgs::msg::CircleObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Circle parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); + /** * @brief Transforms shape coordinates to a new frame. * @param to_frame Frame ID to transform to @@ -371,6 +346,30 @@ class Circle : public Shape const std::shared_ptr tf_buffer, const double transform_tolerance); + /** + * @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); + protected: /** * @brief Checks that shape is consistent for further operation 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 index 8814dc3253..1903bc5d00 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -19,6 +19,7 @@ #include #include +#include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index c76de4c49d..291b462200 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -14,14 +14,19 @@ #include "nav2_map_server/vector_object_server.hpp" -#include +#include +#include #include #include #include +#include + +#include "rclcpp/create_timer.hpp" #include "tf2_ros/create_timer_ros.h" #include "nav2_util/occ_grid_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" using namespace std::placeholders; @@ -133,6 +138,63 @@ VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } +bool VectorObjectServer::obtainParams() +{ + // Main ROS-parameters + map_topic_ = getParameter( + shared_from_this(), "map_topic", "vo_map").as_string(); + global_frame_id_ = getParameter( + shared_from_this(), "global_frame_id", "map").as_string(); + resolution_ = getParameter( + shared_from_this(), "resolution", 0.05).as_double(); + default_value_ = getParameter( + shared_from_this(), "default_value", nav2_util::OCC_GRID_UNKNOWN).as_int(); + overlay_type_ = static_cast(getParameter( + shared_from_this(), "overlay_type", static_cast(OverlayType::OVERLAY_SEQ)).as_int()); + update_frequency_ = getParameter( + shared_from_this(), "update_frequency", 1.0).as_double(); + transform_tolerance_ = getParameter( + shared_from_this(), "transform_tolerance", 0.1).as_double(); + + // Shapes + std::vector shape_names = getParameter( + shared_from_this(), "shapes", std::vector()).as_string_array(); + for (std::string shape_name : shape_names) { + std::string shape_type; + + try { + shape_type = getParameter( + 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") { + std::shared_ptr polygon = std::make_shared(shared_from_this()); + if (!polygon->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(polygon); + } else if (shape_type == "circle") { + std::shared_ptr circle = std::make_shared(shared_from_this()); + + if (!circle->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(circle); + } else { + RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str()); + return false; + } + } + + return true; +} + std::vector>::iterator VectorObjectServer::findShape(const unsigned char * uuid) { @@ -486,63 +548,6 @@ void VectorObjectServer::removeShapesCallback( switchMapUpdate(); } -bool VectorObjectServer::obtainParams() -{ - // Main ROS-parameters - map_topic_ = getParameter( - shared_from_this(), "map_topic", "vo_map").as_string(); - global_frame_id_ = getParameter( - shared_from_this(), "global_frame_id", "map").as_string(); - resolution_ = getParameter( - shared_from_this(), "resolution", 0.05).as_double(); - default_value_ = getParameter( - shared_from_this(), "default_value", nav2_util::OCC_GRID_UNKNOWN).as_int(); - overlay_type_ = static_cast(getParameter( - shared_from_this(), "overlay_type", static_cast(OverlayType::OVERLAY_SEQ)).as_int()); - update_frequency_ = getParameter( - shared_from_this(), "update_frequency", 1.0).as_double(); - transform_tolerance_ = getParameter( - shared_from_this(), "transform_tolerance", 0.1).as_double(); - - // Shapes - std::vector shape_names = getParameter( - shared_from_this(), "shapes", std::vector()).as_string_array(); - for (std::string shape_name : shape_names) { - std::string shape_type; - - try { - shape_type = getParameter( - 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") { - std::shared_ptr polygon = std::make_shared(shared_from_this()); - if (!polygon->obtainParams(shape_name)) { - return false; - } - shapes_.push_back(polygon); - } else if (shape_type == "circle") { - std::shared_ptr circle = std::make_shared(shared_from_this()); - - if (!circle->obtainParams(shape_name)) { - return false; - } - shapes_.push_back(circle); - } else { - RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str()); - return false; - } - } - - return true; -} - } // namespace nav2_map_server #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index 165e199cae..44022bc061 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -15,10 +15,11 @@ #include "nav2_map_server/vector_object_shapes.hpp" #include -#include #include +#include #include #include +#include #include "geometry_msgs/msg/pose_stamped.hpp" @@ -87,6 +88,31 @@ Polygon::Polygon( type_ = POLYGON; } +int8_t Polygon::getValue() const +{ + return params_->value; +} + +std::string Polygon::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Polygon::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +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::obtainParams(const std::string & shape_name) { auto node = node_.lock(); @@ -149,6 +175,54 @@ bool Polygon::obtainParams(const std::string & shape_name) return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); } +nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const +{ + return params_; +} + +bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) +{ + params_ = params; + + 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()); + } + + return checkConsistency(); +} + +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::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) { min_x = std::numeric_limits::max(); @@ -210,79 +284,6 @@ void Polygon::putBorders( } } -nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const -{ - return params_; -} - -bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) -{ - params_ = params; - - 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()); - } - - return checkConsistency(); -} - -int8_t Polygon::getValue() const -{ - return params_->value; -} - -std::string Polygon::getFrameID() const -{ - return params_->header.frame_id; -} - -std::string Polygon::getUUID() const -{ - return unparseUUID(params_->uuid.uuid.data()); -} - -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; -} - bool Polygon::checkConsistency() { if (params_->points.size() < 3) { @@ -310,6 +311,31 @@ Circle::Circle( type_ = CIRCLE; } +int8_t Circle::getValue() const +{ + return params_->value; +} + +std::string Circle::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Circle::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +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::obtainParams(const std::string & shape_name) { auto node = node_.lock(); @@ -371,65 +397,66 @@ bool Circle::obtainParams(const std::string & shape_name) return obtainShapeUUID(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) +nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const { - min_x = center_->x - params_->radius; - min_y = center_->y - params_->radius; - max_x = center_->x + params_->radius; - max_y = center_->y + params_->radius; + return params_; } -bool Circle::isPointInside(const double px, const double py) const +bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) { - return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <= - params_->radius * params_->radius; -} + params_ = params; -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"}; + if (!center_) { + center_ = std::make_shared(); } + *center_ = params_->center; - // 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(), - "[UUID: %s] Can not convert (%f, %f) circle center to map", - getUUID().c_str(), center_->x, center_->y); - return false; + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); } - // 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(), - "[UUID: %s] Can not convert (%f, %f) point to map", - getUUID().c_str(), center_->x, center_->y); + + return checkConsistency(); +} + +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; } -inline void Circle::putPoint( - unsigned int mx, unsigned int my, - nav_msgs::msg::OccupancyGrid::SharedPtr map, - const OverlayType overlay_type) +void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) { - fillMap(map, my * map->info.width + mx, params_->value, overlay_type); + 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; } -// Put params_gons line borders on map void Circle::putBorders( nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) { @@ -479,77 +506,6 @@ void Circle::putBorders( } } -nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const -{ - return params_; -} - -bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) -{ - 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()); - } - - return checkConsistency(); -} - -int8_t Circle::getValue() const -{ - return params_->value; -} - -std::string Circle::getFrameID() const -{ - return params_->header.frame_id; -} - -std::string Circle::getUUID() const -{ - return unparseUUID(params_->uuid.uuid.data()); -} - -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; -} - bool Circle::checkConsistency() { if (params_->radius < 0.0) { @@ -567,4 +523,47 @@ bool Circle::checkConsistency() return true; } +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(), + "[UUID: %s] Can not convert (%f, %f) circle center to map", + getUUID().c_str(), 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(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), 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); +} + } // namespace nav2_map_server From 67a16b0f2c298c5f853bf81185629abedea40598 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 1 Dec 2023 14:14:20 +0300 Subject: [PATCH 19/19] Additional code facelift * Correct licensing years * Fix Vector Object server dependencies * Funcion rename for better readability * Improve/fix comments Signed-off-by: Alexey Merzlyakov --- nav2_map_server/CMakeLists.txt | 4 ++++ .../nav2_map_server/vector_object_server.hpp | 2 +- .../nav2_map_server/vector_object_shapes.hpp | 6 ++--- .../nav2_map_server/vector_object_utils.hpp | 24 +++++++++---------- nav2_map_server/package.xml | 2 ++ .../src/vo_server/vector_object_shapes.cpp | 2 +- nav2_util/include/nav2_util/polygon_utils.hpp | 2 +- .../test/regression/map_bresenham_2d.cpp | 5 +++- 8 files changed, 28 insertions(+), 19 deletions(-) diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 09650f2cf7..73aed874b1 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) @@ -93,6 +95,8 @@ set(vo_server_dependencies rclcpp rclcpp_lifecycle rclcpp_components + geometry_msgs + tf2_ros nav_msgs nav2_msgs nav2_util) 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 index 7e94072c8e..5bde7a3370 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -170,7 +170,7 @@ class VectorObjectServer : public nav2_util::LifecycleNode /** * @brief Callback for RemoveShapes service call. - * Try to remove all requested objects and switches map processing/publishing routine + * Try to remove requested vector objects and switches map processing/publishing routine * @param request_header Service request header * @param request Service request * @param response Service response 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 index aeae185553..b70f1579fd 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -128,7 +128,7 @@ class Shape const double transform_tolerance) = 0; /** - * @brief Gets shape boundaries. + * @brief Gets shape box-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 @@ -237,7 +237,7 @@ class Polygon : public Shape const double transform_tolerance); /** - * @brief Gets shape boundaries + * @brief Gets shape box-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 @@ -347,7 +347,7 @@ class Circle : public Shape const double transform_tolerance); /** - * @brief Gets shape boundaries + * @brief Gets shape box-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 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 index 1903bc5d00..68b632e7c0 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -16,8 +16,8 @@ #define NAV2_MAP_SERVER__VECTOR_OBJECT_UTILS_HPP_ #include -#include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -32,7 +32,7 @@ namespace nav2_map_server // ---------- Working with UUID-s ---------- /** - * @beirf Converts input UUID from input array to unparsed string + * @beirf Converts UUID from input array to unparsed string * @param uuid Input UUID in array format * @return Unparsed UUID string */ @@ -49,7 +49,7 @@ inline std::string unparseUUID(const unsigned char * uuid) * @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) + * @param default_val Default value of the parameter (for the case if parameter is not set) * @return Obtained parameter value */ template @@ -94,7 +94,7 @@ enum class OverlayType : uint8_t /** * @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 map_val Map value. To be updated with new value if overlay is involved * @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 @@ -125,13 +125,13 @@ inline void processVal( } /** - * @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 + * @brief Updates the cell on the map with given shape value according to the given overlay type + * @param map Output map to be updated with + * @param offset Offset to the cell to be updated + * @param shape_val Vector object value to be updated map with * @param overlay_type Type of overlay */ -inline void fillMap( +inline void processCell( nav_msgs::msg::OccupancyGrid::SharedPtr map, const unsigned int offset, const int8_t shape_val, @@ -148,7 +148,7 @@ class MapAction public: /** * @brief MapAction constructor - * @param map Output map pointer + * @param map Pointer to output map * @param value Value to put on map * @param overlay_type Overlay type */ @@ -159,12 +159,12 @@ class MapAction {} /** - * @brief Map filling operator + * @brief Map' cell updating 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_); + processCell(map_, offset, value_, overlay_type_); } protected: diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 84779f9fb7..ce79bcf0e2 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -21,7 +21,9 @@ yaml_cpp_vendor launch_ros launch_testing + geometry_msgs tf2 + tf2_ros nav2_msgs nav2_util graphicsmagick diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index 44022bc061..21dd17abf0 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -563,7 +563,7 @@ inline void Circle::putPoint( nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) { - fillMap(map, my * map->info.width + mx, params_->value, overlay_type); + processCell(map, my * map->info.width + mx, params_->value, overlay_type); } } // namespace nav2_map_server diff --git a/nav2_util/include/nav2_util/polygon_utils.hpp b/nav2_util/include/nav2_util/polygon_utils.hpp index 101ce2a815..28e9186c3e 100644 --- a/nav2_util/include/nav2_util/polygon_utils.hpp +++ b/nav2_util/include/nav2_util/polygon_utils.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Samsung R&D Institute Russia +// Copyright (c) 2022 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. diff --git a/nav2_util/test/regression/map_bresenham_2d.cpp b/nav2_util/test/regression/map_bresenham_2d.cpp index 684b3429e4..90ecb5110c 100644 --- a/nav2_util/test/regression/map_bresenham_2d.cpp +++ b/nav2_util/test/regression/map_bresenham_2d.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Samsung R&D Institute Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // All rights reserved. // // Software License Agreement (BSD License 2.0) @@ -38,6 +38,9 @@ #include "nav2_util/raytrace_line_2d.hpp" +// MapAction - is a functor class used to cover raytraceLine algorithm. +// It contains char map inside, which is an abstract one and not related +// to any concrete representation (like Costmap2D or OccupancyGrid). class MapAction { public: