Add raytrace outside the map

costmap_2d/cfg/ObstaclePlugin.cfg

@@ -7,6 +7,7 @@ gen = ParameterGenerator()
 gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True)
 gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True)
 gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50)
+gen.add("raytrace_outside_map", bool_t, 0, "Whether to raytrace when sensor is outside map or not", False)
 
 combo_enum = gen.enum([gen.const("Overwrite", int_t,  0, "Overwrite values"),
                        gen.const("Maximum",   int_t,  1, "Take the maximum of the values"),

costmap_2d/include/costmap_2d/obstacle_layer.h

@@ -56,11 +56,23 @@
 #include <costmap_2d/ObstaclePluginConfig.h>
 #include <costmap_2d/footprint.h>
 
+// boost
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/linestring.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+
 namespace costmap_2d
 {
 
+namespace bg = boost::geometry;
+
 class ObstacleLayer : public CostmapLayer
 {
+private:
+  typedef bg::model::d2::point_xy<double> Point;
+  typedef bg::model::polygon<Point, false, false> Polygon;
+  typedef bg::model::linestring<Point> Linestring;
 public:
   ObstacleLayer()
   {
@@ -141,12 +153,27 @@ class ObstacleLayer : public CostmapLayer
   virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                  double* max_x, double* max_y);
 
+  /**
+   * @brief Adjust the origin of the sensor to be inside the map
+   * When the sensor origin is outside the map, we calculate where the line
+   * between the origin and the point intersects the map boundaries.
+   * This becomes the new start point for the raytrace
+   * @param ox The x coordinate of the origin
+   * @param oy The y coordinate of the origin
+   * @param wx The x coordinate of the point
+   * @param wy The y coordinate of the point
+   * @return bool True if the origin is updated
+   */
+  bool adjustSensorOrigin(double& ox, double& oy, double wx, double wy) const;
+
   void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
                             double* max_x, double* max_y);
 
+  void updateMapPolygon();
+
   std::vector<geometry_msgs::Point> transformed_footprint_;
   bool footprint_clearing_enabled_;
-  void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, 
+  void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                        double* max_x, double* max_y);
 
   std::string global_frame_;  ///< @brief The global frame for the costmap
@@ -167,6 +194,8 @@ class ObstacleLayer : public CostmapLayer
   dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig> *dsrv_;
 
   int combination_method_;
+  bool raytrace_outside_map_;
+  Polygon map_boundary_;
 
 private:
   void reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level);

costmap_2d/plugins/obstacle_layer.cpp

@@ -240,6 +240,7 @@ void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint
   footprint_clearing_enabled_ = config.footprint_clearing_enabled;
   max_obstacle_height_ = config.max_obstacle_height;
   combination_method_ = config.combination_method;
+  raytrace_outside_map_ = config.raytrace_outside_map;
 }
 
 void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
@@ -330,12 +331,27 @@ void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr&
   buffer->unlock();
 }
 
+void ObstacleLayer::updateMapPolygon()
+{
+  map_boundary_.clear();
+  const double origin_x = origin_x_, origin_y = origin_y_;
+  const double map_end_x = origin_x + size_x_ * resolution_;
+  const double map_end_y = origin_y + size_y_ * resolution_;
+  bg::append(map_boundary_.outer(), Point(origin_x, origin_y));
+  bg::append(map_boundary_.outer(), Point(map_end_x, origin_y));
+  bg::append(map_boundary_.outer(), Point(map_end_x, map_end_y));
+  bg::append(map_boundary_.outer(), Point(origin_x, map_end_y));
+  bg::append(map_boundary_.outer(), Point(origin_x, origin_y));
+}
+
+
 void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
 {
   if (rolling_window_)
     updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
   useExtraBounds(min_x, min_y, max_x, max_y);
+  updateMapPolygon();
 
   bool current = true;
   std::vector<Observation> observations, clearing_observations;
@@ -495,8 +511,10 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
   const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
 
   // get the map coordinates of the origin of the sensor
+
   unsigned int x0, y0;
-  if (!worldToMap(ox, oy, x0, y0))
+  const bool origin_valid = worldToMap(ox, oy, x0, y0);
+  if (!raytrace_outside_map_ && !origin_valid)
   {
     ROS_WARN_THROTTLE(
         1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
@@ -521,6 +539,11 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
     double wx = *iter_x;
     double wy = *iter_y;
 
+    if (!origin_valid && !adjustSensorOrigin(ox, oy, wx, wy))
+    {
+      continue;
+    }
+
     // now we also need to make sure that the enpoint we're raytracing
     // to isn't off the costmap and scale if necessary
     double a = wx - ox;
@@ -539,7 +562,6 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
       wx = ox + a * t;
       wy = origin_y;
     }
-
     // the maximum value to raytrace to is the end of the map
     if (wx > map_end_x)
     {
@@ -558,7 +580,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
     unsigned int x1, y1;
 
     // check for legality just in case
-    if (!worldToMap(wx, wy, x1, y1))
+    if (!worldToMap(wx, wy, x1, y1) || !worldToMap(ox, oy, x0, y0))
       continue;
 
     unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
@@ -570,6 +592,41 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
   }
 }
 
+bool ObstacleLayer::adjustSensorOrigin(double &ox, double &oy, double wx, double wy) const
+{
+  // Define the sensor ray as a linestring (from the sensor origin to the endpoint)
+  Linestring sensor_ray;
+  bg::append(sensor_ray, Point(ox, oy));
+  bg::append(sensor_ray, Point(wx, wy));
+
+  std::vector<Point> intersection_points;
+  bg::intersection(sensor_ray, map_boundary_, intersection_points);
+
+  if (intersection_points.size() != 2)
+  {
+    return false;
+  }
+
+  const auto& intersection1 = intersection_points[0];
+  const auto& intersection2 = intersection_points[1];
+  double distance1 = bg::distance(Point(ox, oy), intersection1);
+  double distance2 = bg::distance(Point(ox, oy), intersection2);
+
+  // Choose the closest intersection point as origin
+  if (distance1 < distance2)
+  {
+    ox = intersection1.x();
+    oy = intersection1.y();
+  }
+  else
+  {
+    ox = intersection2.x();
+    oy = intersection2.y();
+  }
+  return true;
+}
+
+
 void ObstacleLayer::activate()
 {
   // if we're stopped we need to re-subscribe to topics