fix origin

costmap_2d/include/costmap_2d/obstacle_layer.h

@@ -158,14 +158,15 @@ class ObstacleLayer : public CostmapLayer
    * 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.
+   * The endpoint is also updated if it is outside the map, as the other intersection point.
    * It returns true if the new origin is updated, within range, and closer than the endpoint
    * @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(const Observation& clearing_observation, double& ox, double& oy, double wx, double wy) const;
+  bool adjustSensorOrigin(const Observation& clearing_observation, 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);

costmap_2d/plugins/obstacle_layer.cpp

@@ -506,19 +506,17 @@ bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_o
 void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
                                               double* max_x, double* max_y)
 {
-  double ox = clearing_observation.origin_.x;
-  double oy = clearing_observation.origin_.y;
   const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
 
   // get the map coordinates of the origin of the sensor
 
   unsigned int x0, y0;
-  const bool origin_valid = worldToMap(ox, oy, x0, y0);
+  const bool origin_valid = worldToMap(clearing_observation.origin_.x, clearing_observation.origin_.y, x0, y0);
   if (!origin_valid && !raytrace_outside_map_)
   {
     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.",
-        ox, oy);
+        clearing_observation.origin_.x, clearing_observation.origin_.y);
     return;
   }
 
@@ -527,14 +525,17 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
   double map_end_x = origin_x + size_x_ * resolution_;
   double map_end_y = origin_y + size_y_ * resolution_;
 
-  touch(ox, oy, min_x, min_y, max_x, max_y);
+  touch(clearing_observation.origin_.x, clearing_observation.origin_.y, min_x, min_y, max_x, max_y);
 
   // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
   sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
   sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
   {
+    double ox = clearing_observation.origin_.x;
+    double oy = clearing_observation.origin_.y;
+
     double wx = *iter_x;
     double wy = *iter_y;
 
@@ -543,37 +544,39 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
     {
       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;
-    double b = wy - oy;
-
-    // the minimum value to raytrace from is the origin
-    if (wx < origin_x)
-    {
-      double t = (origin_x - ox) / a;
-      wx = origin_x;
-      wy = oy + b * t;
-    }
-    if (wy < origin_y)
-    {
-      double t = (origin_y - oy) / b;
-      wx = ox + a * t;
-      wy = origin_y;
-    }
-    // the maximum value to raytrace to is the end of the map
-    if (wx > map_end_x)
-    {
-      double t = (map_end_x - ox) / a;
-      wx = map_end_x - .001;
-      wy = oy + b * t;
-    }
-    if (wy > map_end_y)
+    else
     {
-      double t = (map_end_y - oy) / b;
-      wx = ox + a * t;
-      wy = map_end_y - .001;
+      // 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;
+      double b = wy - oy;
+
+      // the minimum value to raytrace from is the origin
+      if (wx < origin_x)
+      {
+        double t = (origin_x - ox) / a;
+        wx = origin_x;
+        wy = oy + b * t;
+      }
+      if (wy < origin_y)
+      {
+        double t = (origin_y - oy) / b;
+        wx = ox + a * t;
+        wy = origin_y;
+      }
+      // the maximum value to raytrace to is the end of the map
+      if (wx > map_end_x)
+      {
+        double t = (map_end_x - ox) / a;
+        wx = map_end_x - .001;
+        wy = oy + b * t;
+      }
+      if (wy > map_end_y)
+      {
+        double t = (map_end_y - oy) / b;
+        wx = ox + a * t;
+        wy = map_end_y - .001;
+      }
     }
 
     // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
@@ -592,8 +595,8 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
   }
 }
 
-bool ObstacleLayer::adjustSensorOrigin(const Observation& clearing_observation, double& ox, double& oy, double wx,
-                                       double wy) const
+bool ObstacleLayer::adjustSensorOrigin(const Observation& clearing_observation, 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;
@@ -620,17 +623,22 @@ bool ObstacleLayer::adjustSensorOrigin(const Observation& clearing_observation,
   // copy original sensor origin
   const double original_ox = ox;
   const double original_oy = oy;
+  double end_x, end_y;
 
   // Choose the closest intersection point as origin
   if (distance1 < distance2)
   {
     ox = intersection1.x();
     oy = intersection1.y();
+    end_x = intersection2.x();
+    end_y = intersection2.y();
   }
   else
   {
     ox = intersection2.x();
     oy = intersection2.y();
+    end_x = intersection1.x();
+    end_y = intersection1.y();
   }
 
   // check if the distance between new origin and original sensor's origin is within range
@@ -647,6 +655,15 @@ bool ObstacleLayer::adjustSensorOrigin(const Observation& clearing_observation,
     return false;
   }
 
+  // scale the endpoint to the second intersection point 
+  // if the distance between the new origin and endpoint is greater than second intersection point and the new origin
+  // this means the endpoint is outside the map, so we put it on the second intersection point (inside the map)
+  if (std::hypot(end_x - ox, end_y - oy) < std::hypot(wx - ox, wy - oy))
+  {
+    wx = end_x;
+    wy = end_y;
+  }
+
   return true;
 }