Adding parameter for minimum range for raycasting to clear obstacles (r…

…os-navigation#2126)

* Adding minimum range parameter for raytrace clearing

* Comments in tests

* Uncrustify checks

* Modified default value of raytrace_min_range param and moved computation below plugin layer

* Tests for voxel layer raytracing

* Uncrustify checks

* Addressed pull request ros-navigation#2126 comments

* Using min range parameter for marking also

* Correcting build warnings

* Correct build fails

* Merging changes

* Adding comments

* Added unit tests for obstacle layer and addressed PR comments

* Addressed flake errors

* Flake8 errors

* Correcting W291 Flake8 error

* Variable name and comments changes

* Added raytracing params to the params files

* Modifying parameter names for obstacle marking

doc/parameters/param_list.md

@@ -119,8 +119,10 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | `<data source>`.inf_is_valid | false | Are infinite returns from laser scanners valid measurements |
 | `<data source>`.marking | true | Whether source should mark in costmap |
 | `<data source>`.clearing | false | Whether source should raytrace clear in costmap |
-| `<data source>`.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap |
-| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
+| `<data source>`.obstacle_max_range | 2.5 | Maximum range to mark obstacles in costmap |
+| `<data_source>`.obstacle_min_range | 0.0 | Minimum range to mark obstacles in costmap | 
+| `<data source>`.raytrace_max_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
+| `<data_source>`.raytrace_min_range | 0.0 | Minimum range to raytrace clear obstacles from costmap |
 
 ## range_sensor_layer plugin
 
@@ -168,8 +170,10 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | `<data source>`.inf_is_valid | false | Are infinite returns from laser scanners valid measurements |
 | `<data source>`.marking | true | Whether source should mark in costmap |
 | `<data source>`.clearing | false | Whether source should raytrace clear in costmap |
-| `<data source>`.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap |
-| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
+| `<data source>`.obstacle_max_range | 2.5 | Maximum range to mark obstacles in costmap |
+| `<data_source>`.obstacle_min_range | 0.0 | Minimum range to mark obstacles in costmap |
+| `<data source>`.raytrace_max_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
+| `<data_source>`.raytrace_min_range | 0.0 | Minimum range to raytrace clear obstacles from costmap |
 
 ## keepout filter
 

nav2_bringup/bringup/launch/tb3_simulation_launch.py

@@ -76,7 +76,8 @@ def generate_launch_description():
 
     declare_map_yaml_cmd = DeclareLaunchArgument(
         'map',
-        default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'),
+        default_value=os.path.join(
+            bringup_dir, 'maps', 'turtlebot3_world.yaml'),
         description='Full path to map file to load')
 
     declare_use_sim_time_cmd = DeclareLaunchArgument(
@@ -102,7 +103,8 @@ def generate_launch_description():
 
     declare_rviz_config_file_cmd = DeclareLaunchArgument(
         'rviz_config_file',
-        default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
+        default_value=os.path.join(
+            bringup_dir, 'rviz', 'nav2_default_view.rviz'),
         description='Full path to the RVIZ config file to use')
 
     declare_use_simulator_cmd = DeclareLaunchArgument(
@@ -130,7 +132,7 @@ def generate_launch_description():
         # TODO(orduno) Switch back once ROS argument passing has been fixed upstream
         #              https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
         # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
-        #                            'worlds/turtlebot3_worlds/waffle.model'),
+        # worlds/turtlebot3_worlds/waffle.model')
         default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'),
         description='Full path to world model file to load')
 
@@ -141,7 +143,8 @@ def generate_launch_description():
         cwd=[launch_dir], output='screen')
 
     start_gazebo_client_cmd = ExecuteProcess(
-        condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])),
+        condition=IfCondition(PythonExpression(
+            [use_simulator, ' and not ', headless])),
         cmd=['gzclient'],
         cwd=[launch_dir], output='screen')
 
@@ -159,14 +162,16 @@ def generate_launch_description():
         arguments=[urdf])
 
     rviz_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')),
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_dir, 'rviz_launch.py')),
         condition=IfCondition(use_rviz),
         launch_arguments={'namespace': '',
                           'use_namespace': 'False',
                           'rviz_config': rviz_config_file}.items())
 
     bringup_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')),
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_dir, 'bringup_launch.py')),
         launch_arguments={'namespace': namespace,
                           'use_namespace': use_namespace,
                           'slam': slam,

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -170,6 +170,10 @@ local_costmap:
           max_obstacle_height: 2.0
           clearing: True
           marking: True
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True
@@ -193,6 +197,10 @@ global_costmap:
           max_obstacle_height: 2.0
           clearing: True
           marking: True
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -170,6 +170,10 @@ local_costmap:
           max_obstacle_height: 2.0
           clearing: True
           marking: True
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True
@@ -193,6 +197,10 @@ global_costmap:
           max_obstacle_height: 2.0
           clearing: True
           marking: True
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True

nav2_bringup/bringup/params/nav2_params.yaml

@@ -188,6 +188,10 @@ local_costmap:
           clearing: True
           marking: True
           data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True
@@ -220,6 +224,10 @@ global_costmap:
           clearing: True
           marking: True
           data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True

nav2_common/nav2_common/launch/replace_string.py

@@ -51,7 +51,7 @@ def perform(self, context: launch.LaunchContext) -> Text:
     try:
       input_file = open(launch.utilities.perform_substitutions(context, self.name), 'r')
       self.replace(input_file, output_file, replacements)
-    except Exception as err:
+    except Exception as err:  # noqa: B902
       print('ReplaceString substitution error: ', err)
     finally:
       input_file.close()

nav2_costmap_2d/cfg/ObstaclePlugin.cfg

@@ -15,6 +15,8 @@ combo_enum = gen.enum([gen.const("Overwrite", int_t,  0, "Overwrite values"),
 gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum)
 
 
-#gen.add("max_obstacle_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50)
-#gen.add("raytrace_range", double_t, 0, "The default range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50)
+# gen.add("obstacle_max_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50)
+# gen.add("obstacle_min_range", double_t, 0, "The default minimum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 0.0, 0, 50)
+# gen.add("raytrace_max_range", double_t, 0, "The default maximum range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50)
+# gen.add("raytrace_min_range", double_t, 0, "The default minimum range in meters from which to raytrace out obstacles from the map using sensor data.", 0.0, 0, 50)
 exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin"))

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

@@ -391,12 +391,13 @@ class Costmap2D
    * @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 max_length = UINT_MAX, unsigned int min_length = 0)
   {
     int dx = x1 - x0;
     int dy = y1 - y0;
@@ -407,27 +408,39 @@ class Costmap2D
     int offset_dx = sign(dx);
     int offset_dy = sign(dy) * size_x_;
 
-    unsigned int offset = y0 * size_x_ + x0;
-
     // we need to chose how much to scale our dominant dimension,
     // based on the maximum length of the line
     double dist = std::hypot(dx, dy);
-    double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
+    if (dist < min_length) {
+      return;
+    }
+
+    // Adjust starting point and offset to start from min_length distance
+    unsigned int min_x0 = (unsigned int)(x0 + dx / dist * min_length);
+    unsigned int min_y0 = (unsigned int)(y0 + dy / dist * min_length);
+    unsigned int offset = min_y0 * size_x_ + min_x0;
 
+    double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
+    unsigned int length;
     // if x is dominant
     if (abs_dx >= abs_dy) {
       int error_y = abs_dx / 2;
+
+      // Subtract min_length from total length since initial point (x0, y0) has been adjusted by min Z
+      length = (unsigned int)(scale * abs_dx) - min_length;
+
       bresenham2D(
-        at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset,
-        (unsigned int)(scale * abs_dx));
+        at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, length);
       return;
     }
 
     // otherwise y is dominant
     int error_x = abs_dy / 2;
+
+    // Subtract min_length from total length since initial point (x0, y0) has been adjusted by min Z
+    length = (unsigned int)(scale * abs_dy) - min_length;
     bresenham2D(
-      at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset,
-      (unsigned int)(scale * abs_dy));
+      at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, length);
   }
 
 private:

nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp

@@ -50,7 +50,9 @@ class Observation
    * @brief  Creates an empty observation
    */
   Observation()
-  : cloud_(new sensor_msgs::msg::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
+  : cloud_(new sensor_msgs::msg::PointCloud2()), obstacle_max_range_(0.0), obstacle_min_range_(0.0),
+    raytrace_max_range_(0.0),
+    raytrace_min_range_(0.0)
   {
   }
 
@@ -68,14 +70,19 @@ class Observation
    * @brief  Creates an observation from an origin point and a point cloud
    * @param origin The origin point of the observation
    * @param cloud The point cloud of the observation
-   * @param obstacle_range The range out to which an observation should be able to insert obstacles
-   * @param raytrace_range The range out to which an observation should be able to clear via raytracing
+   * @param obstacle_max_range The range out to which an observation should be able to insert obstacles
+   * @param obstacle_min_range The range from which an observation should be able to insert obstacles
+   * @param raytrace_max_range The range out to which an observation should be able to clear via raytracing
+   * @param raytrace_min_range The range from which an observation should be able to clear via raytracing
    */
   Observation(
     geometry_msgs::msg::Point & origin, const sensor_msgs::msg::PointCloud2 & cloud,
-    double obstacle_range, double raytrace_range)
+    double obstacle_max_range, double obstacle_min_range, double raytrace_max_range,
+    double raytrace_min_range)
   : origin_(origin), cloud_(new sensor_msgs::msg::PointCloud2(cloud)),
-    obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
+    obstacle_max_range_(obstacle_max_range), obstacle_min_range_(obstacle_min_range),
+    raytrace_max_range_(raytrace_max_range), raytrace_min_range_(
+      raytrace_min_range)
   {
   }
 
@@ -85,24 +92,30 @@ class Observation
    */
   Observation(const Observation & obs)
   : origin_(obs.origin_), cloud_(new sensor_msgs::msg::PointCloud2(*(obs.cloud_))),
-    obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
+    obstacle_max_range_(obs.obstacle_max_range_), obstacle_min_range_(obs.obstacle_min_range_),
+    raytrace_max_range_(obs.raytrace_max_range_),
+    raytrace_min_range_(obs.raytrace_min_range_)
   {
   }
 
   /**
    * @brief  Creates an observation from a point cloud
    * @param cloud The point cloud of the observation
-   * @param obstacle_range The range out to which an observation should be able to insert obstacles
+   * @param obstacle_max_range The range out to which an observation should be able to insert obstacles
+   * @param obstacle_min_range The range from which an observation should be able to insert obstacles
    */
-  Observation(const sensor_msgs::msg::PointCloud2 & cloud, double obstacle_range)
-  : cloud_(new sensor_msgs::msg::PointCloud2(cloud)), obstacle_range_(obstacle_range),
-    raytrace_range_(0.0)
+  Observation(
+    const sensor_msgs::msg::PointCloud2 & cloud, double obstacle_max_range,
+    double obstacle_min_range)
+  : cloud_(new sensor_msgs::msg::PointCloud2(cloud)), obstacle_max_range_(obstacle_max_range),
+    obstacle_min_range_(obstacle_min_range),
+    raytrace_max_range_(0.0), raytrace_min_range_(0.0)
   {
   }
 
   geometry_msgs::msg::Point origin_;
   sensor_msgs::msg::PointCloud2 * cloud_;
-  double obstacle_range_, raytrace_range_;
+  double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp

@@ -66,8 +66,10 @@ class ObservationBuffer
    * @param  expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
    * @param  min_obstacle_height The minimum height of a hitpoint to be considered legal
    * @param  max_obstacle_height The minimum height of a hitpoint to be considered legal
-   * @param  obstacle_range The range to which the sensor should be trusted for inserting obstacles
-   * @param  raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
+   * @param  obstacle_max_range The range to which the sensor should be trusted for inserting obstacles
+   * @param  obstacle_min_range The range from which the sensor should be trusted for inserting obstacles
+   * @param  raytrace_max_range The range to which the sensor should be trusted for raytracing to clear out space
+   * @param  raytrace_min_range The range from which the sensor should be trusted for raytracing to clear out space
    * @param  tf2_buffer A reference to a tf2 Buffer
    * @param  global_frame The frame to transform PointClouds into
    * @param  sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
@@ -78,8 +80,10 @@ class ObservationBuffer
     std::string topic_name,
     double observation_keep_time,
     double expected_update_rate,
-    double min_obstacle_height, double max_obstacle_height, double obstacle_range,
-    double raytrace_range, tf2_ros::Buffer & tf2_buffer, std::string global_frame,
+    double min_obstacle_height, double max_obstacle_height, double obstacle_max_range,
+    double obstacle_min_range,
+    double raytrace_max_range, double raytrace_min_range, tf2_ros::Buffer & tf2_buffer,
+    std::string global_frame,
     std::string sensor_frame,
     tf2::Duration tf_tolerance);
 
@@ -146,7 +150,7 @@ class ObservationBuffer
   std::string topic_name_;
   double min_obstacle_height_, max_obstacle_height_;
   std::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
-  double obstacle_range_, raytrace_range_;
+  double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_;
   tf2::Duration tf_tolerance_;
 };
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -153,7 +153,7 @@ class ObstacleLayer : public CostmapLayer
     double * max_y);
 
   void updateRaytraceBounds(
-    double ox, double oy, double wx, double wy, double range,
+    double ox, double oy, double wx, double wy, double max_range, double min_range,
     double * min_x, double * min_y,
     double * max_x,
     double * max_y);

nav2_costmap_2d/launch/example_params.yaml

@@ -26,9 +26,12 @@ mark_threshold: 0
 #END VOXEL STUFF
 
 transform_tolerance: 0.3
-obstacle_range: 2.5
+obstacle_max_range: 2.5
+obstacle_min_range: 0.0
 max_obstacle_height: 2.0
-raytrace_range: 3.0
+raytrace_max_range: 3.0
+raytrace_min_range: 0.0
+
 footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]
 #robot_radius: 0.46
 footprint_padding: 0.01