Prevent a possible segmentation fault (ros-navigation#4141) (ros-navi…

…gation#4147)

* Prevent a possible segmentation fault

ros-navigation#4141
Signed-off-by: Joni Pöllänen <[email protected]>

* Cleanup

Signed-off-by: Joni Pöllänen <[email protected]>

---------

Signed-off-by: Joni Pöllänen <[email protected]>
Signed-off-by: Alberto Tudela <[email protected]>

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

@@ -426,105 +426,7 @@ class Costmap2D
    */
   virtual void initMaps(unsigned int size_x, unsigned int size_y);
 
-  /**
-   * @brief  Raytrace a line and apply some action at each step
-   * @param  at The action to take... a functor
-   * @param  x0 The starting x coordinate
-   * @param  y0 The starting y coordinate
-   * @param  x1 The ending x coordinate
-   * @param  y1 The ending y coordinate
-   * @param  max_length The maximum desired length of the segment...
-   * allows you to not go all the way to the endpoint
-   * @param  min_length The minimum desired length of the segment
-   */
-  template<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

@@ -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

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.

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<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,14 +35,16 @@
  * Author: Alexey Merzlyakov
  *********************************************************************/
 
+#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
+
 #include <string>
 #include <memory>
 #include <algorithm>
 #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) {

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
 {
@@ -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.

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_,

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
 {
@@ -430,14 +431,15 @@ void Costmap2D::polygonOutlineCells(
 {
   PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
   for (unsigned int i = 0; i < polygon.size() - 1; ++i) {
-    raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
+    nav2_util::raytraceLine(
+      cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y, size_x_);
   }
   if (!polygon.empty()) {
     unsigned int last_index = polygon.size() - 1;
     // we also need to close the polygon by going from the last point to the first
-    raytraceLine(
+    nav2_util::raytraceLine(
       cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x,
-      polygon[0].y);
+      polygon[0].y, size_x_);
   }
 }
 

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)