Feature/vector object server (ros-navigation#5479)

* Add Vector Object server

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Meet review comments

Signed-off-by: Alexey Merzlyakov <[email protected]>

* Simplify shapes param configuring

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Rename getROSParameter() to getParameter()

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Return back getMaskData() to nav2_costmap_2d

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Add composition node support

Signed-off-by: Alexey Merzlyakov <[email protected]>

* Remove redundant methods

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Update nav2_map_server/src/vo_server/vector_object_server.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Avoid shapes clearing

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Optimize switchMapUpdate() method

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Switch to vector of shapes

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Minor fixes

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Meet review comments

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Move isPointInside algorithm to nav2_util

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Testcases covering new functionality

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Fix linting issues

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Adjust for Vector Objects demonstration

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Code clean-up
* Corrected headers
* Functions ordering
* Comment fixes

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

* Additional code facelift
* Correct licensing years
* Fix Vector Object server dependencies
* Funcion rename for better readability
* Improve/fix comments

Signed-off-by: Alexey Merzlyakov <[email protected]>

* Minor fixing after rebase

Signed-off-by: Alberto Tudela <[email protected]>

* Rename vector object server

Signed-off-by: Alberto Tudela <[email protected]>

* Minor changes

Signed-off-by: Alberto Tudela <[email protected]>

* Update tests

Signed-off-by: Alberto Tudela <[email protected]>

* Merge branch 'main' into feature/vector_object_server

Signed-off-by: Sushant Chavan <[email protected]>

* Fix merge issues and pre-commit checks

Signed-off-by: Sushant Chavan <[email protected]>

* Change tf2_ros headers from `.h` to `.hpp`

Signed-off-by: Sushant Chavan <[email protected]>

* Fix race condition in pub-sub of VO map

Signed-off-by: Sushant Chavan <[email protected]>

* Cleanup

Signed-off-by: Sushant Chavan <[email protected]>

* Remove use of ament_target_dependencies

Signed-off-by: Sushant Chavan <[email protected]>

* Fix review comments

Signed-off-by: Sushant Chavan <[email protected]>

* Fix linter errors

Signed-off-by: Sushant Chavan <[email protected]>

* Fix exception handling

Signed-off-by: Sushant Chavan <[email protected]>

* Update nav2_map_server/include/nav2_map_server/vector_object_utils.hpp

Co-authored-by: Copilot <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_util/include/nav2_util/raytrace_line_2d.hpp

Co-authored-by: Copilot <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>

* Add error logs

Signed-off-by: Sushant Chavan <[email protected]>

* Fix cpplint

Signed-off-by: Sushant Chavan <[email protected]>

---------

Signed-off-by: Alexey Merzlyakov <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>
Signed-off-by: Sushant Chavan <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Co-authored-by: Alberto Tudela <[email protected]>
Co-authored-by: Copilot <[email protected]>

Author: 5 people

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -239,13 +239,6 @@ class Polygon
   rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
     std::vector<rclcpp::Parameter> 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

nav2_collision_monitor/src/polygon.cpp

@@ -22,6 +22,7 @@
 #include "tf2/transform_datatypes.hpp"
 
 #include "nav2_ros_common/node_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/array_parser.hpp"
 
@@ -229,7 +230,7 @@ int Polygon::getPointsInside(const std::vector<Point> & points) const
 {
   int num = 0;
   for (const Point & point : points) {
-    if (isPointInside(point)) {
+    if (nav2_util::geometry_utils::isPointInsidePolygon(point.x, point.y, poly_)) {
       num++;
     }
   }
@@ -602,38 +603,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<Point> & polygon)

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

@@ -461,105 +461,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<class ActionType>
-  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<class ActionType>
-  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:

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp

@@ -185,20 +185,6 @@ class CostmapFilter : public Layer
     const std::string mask_frame,
     geometry_msgs::msg::Pose & 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

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
 {
@@ -190,8 +191,8 @@ 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.position.x, mask_pose.position.y, mask_robot_i,
-    mask_robot_j))
+  if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y,
+    mask_robot_i, mask_robot_j))
   {
     // Robot went out of mask range. Set "false" state by-default
     RCLCPP_WARN(

nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

@@ -193,29 +193,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<unsigned int>((wx - origin_x) / resolution);
-  my = static_cast<unsigned int>((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

nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

@@ -35,15 +35,17 @@
  * Author: Alexey Merzlyakov
  *********************************************************************/
 
+#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
+
 #include <string>
 #include <memory>
 #include <algorithm>
 #include "tf2/convert.hpp"
 #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/geometry_utils.hpp"
+#include "nav2_util/occ_grid_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -200,8 +202,8 @@ void KeepoutFilter::updateBounds(
     geometry_msgs::msg::Pose mask_pose;
     if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
       unsigned int mask_robot_i, mask_robot_j;
-      if (worldToMask(filter_mask_, mask_pose.position.x, mask_pose.position.y, mask_robot_i,
-        mask_robot_j))
+      if (nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y,
+        mask_robot_i, mask_robot_j))
       {
         auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j);
         is_pose_lethal_ = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE);
@@ -366,7 +368,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) {

nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

@@ -43,6 +43,7 @@
 #include <string>
 
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+#include "nav2_util/occ_grid_utils.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -198,8 +199,8 @@ 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.position.x, mask_pose.position.y, mask_robot_i,
-    mask_robot_j))
+  if (!nav2_util::worldToMap(filter_mask_, mask_pose.position.x, mask_pose.position.y,
+    mask_robot_i, mask_robot_j))
   {
     return;
   }

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -45,7 +45,9 @@
 
 #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"
+#include "nav2_ros_common/node_utils.hpp"
 #include "rclcpp/version.h"
 
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer)
@@ -768,7 +770,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_,

nav2_costmap_2d/src/costmap_2d.cpp

@@ -43,6 +43,7 @@
 #include <vector>
 #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
 {
@@ -462,14 +463,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_);
   }
 }