Refactor/projected collision cost

include/path_tracking_pid/path_tracking_pid_local_planner.hpp

@@ -29,7 +29,7 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
                                 private boost::noncopyable
 {
 private:
-  typedef boost::geometry::model::ring<geometry_msgs::Point> polygon_t;
+  using polygon_t = boost::geometry::model::ring<geometry_msgs::Point>;
 
   static inline polygon_t union_(const polygon_t & polygon1, const polygon_t & polygon2)
   {
@@ -112,7 +112,38 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
 
   void velMaxExternalCallback(const std_msgs::Float64 & msg);
 
-  uint8_t projectedCollisionCost();
+  /**
+   * @brief Project an amount of steps in the direction of movement based on velocity
+   * @return Projected steps
+   */
+  std::vector<tf2::Transform> projectionSteps();
+
+  /**
+   * @brief Expand the footprint with the projected steps
+   * @param footprint
+   * @param projected_steps
+   * @param viz Used for marker publishing
+   * @param viz_frame Used for marker publishing
+   * @return Projected footprint
+   */
+  static polygon_t projectionFootprint(
+    const std::vector<geometry_msgs::Point> & footprint,
+    const std::vector<tf2::Transform> & projected_steps, std::unique_ptr<Visualization> & viz,
+    const std::string viz_frame);
+
+  /**
+   * @brief Projects the footprint along the projected steps and determines maximum cost in that area
+   * @param costmap2d
+   * @param footprint
+   * @param projected_steps
+   * @param viz Used for marker publishing
+   * @param viz_frame Used for marker publishing
+   * @return Maximum cost
+   */
+  static uint8_t projectedCollisionCost(
+    costmap_2d::Costmap2D * costmap2d, const std::vector<geometry_msgs::Point> & footprint,
+    const std::vector<tf2::Transform> & projected_steps, std::unique_ptr<Visualization> & viz,
+    const std::string viz_frame);
 
   nav_msgs::Odometry latest_odom_;
   ros::Time prev_time_;

src/path_tracking_pid_local_planner.cpp

@@ -32,7 +32,7 @@ constexpr double DT_MAX = 1.5;
 
 /**
  * Convert the plan from geometry message format to tf2 format.
- * 
+ *
  * @param[in] plan Plan to convert.
  * @return Converted plan.
  */
@@ -241,7 +241,9 @@ std::optional<geometry_msgs::Twist> TrackingPidLocalPlanner::computeVelocityComm
 
   // Handle obstacles
   if (pid_controller_.getConfig().anti_collision) {
-    auto cost = projectedCollisionCost();
+    const std::vector<geometry_msgs::Point> footprint = costmap_->getRobotFootprint();
+    auto cost = projectedCollisionCost(
+      costmap_->getCostmap(), footprint, projectionSteps(), visualization_, map_frame_);
 
     if (cost >= costmap_2d::LETHAL_OBSTACLE) {
       pid_controller_.setVelMaxObstacle(0.0);
@@ -300,7 +302,7 @@ std::optional<geometry_msgs::Twist> TrackingPidLocalPlanner::computeVelocityComm
   return update_result.velocity_command;
 }
 
-uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
+std::vector<tf2::Transform> TrackingPidLocalPlanner::projectionSteps()
 {
   // Check how far we should check forward
   double x_vel = pid_controller_.getCurrentForwardVelocity();
@@ -340,51 +342,83 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
     poses_on_path_points.push_back(projected_step_tf.getOrigin());
   }
 
-  costmap_2d::Costmap2D * costmap2d = costmap_->getCostmap();
-  std::vector<tf2::Vector3> collision_footprint_points;
+  // Visualize
+  std_msgs::Header header;
+  header.stamp = ros::Time::now();
+  header.frame_id = map_frame_;
+  visualization_->publishExtrapolatedPoses(header, step_points);
+  visualization_->publishgGoalPosesOnPath(header, poses_on_path_points);
+
+  return projected_steps_tf;
+}
+
+boost::geometry::model::ring<geometry_msgs::Point> TrackingPidLocalPlanner::projectionFootprint(
+  const std::vector<geometry_msgs::Point> & footprint,
+  const std::vector<tf2::Transform> & projected_steps, std::unique_ptr<Visualization> & viz,
+  const std::string viz_frame)
+{
+  std::vector<tf2::Vector3> projected_footprint_points;
   polygon_t previous_footprint_xy;
-  polygon_t collision_polygon;
-  uint8_t max_projected_step_cost = 0;
-  for (const auto & projection_tf : projected_steps_tf) {
+  polygon_t projected_polygon;
+  for (const auto & projection_tf : projected_steps) {
     // Project footprint forward
     double x = projection_tf.getOrigin().x();
     double y = projection_tf.getOrigin().y();
     double yaw = tf2::getYaw(projection_tf.getRotation());
 
-    // Calculate cost by checking base link location in costmap
-    int map_x, map_y;
-    costmap2d->worldToMapEnforceBounds(x, y, map_x, map_y);
-    uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y);
-    if (projected_step_cost > max_projected_step_cost) {
-      max_projected_step_cost = projected_step_cost;
-    }
-
     // Project footprint forward
-    std::vector<geometry_msgs::Point> footprint;
-    costmap_2d::transformFootprint(x, y, yaw, costmap_->getRobotFootprint(), footprint);
+    std::vector<geometry_msgs::Point> footprint_proj;
+    costmap_2d::transformFootprint(x, y, yaw, footprint, footprint_proj);
 
     // Append footprint to polygon
     polygon_t two_footprints = previous_footprint_xy;
     previous_footprint_xy.clear();
-    for (const auto & point : footprint) {
+    for (const auto & point : footprint_proj) {
       boost::geometry::append(two_footprints, point);
       boost::geometry::append(previous_footprint_xy, point);
     }
 
     boost::geometry::correct(two_footprints);
     polygon_t two_footprint_hull;
     boost::geometry::convex_hull(two_footprints, two_footprint_hull);
-    collision_polygon = union_(collision_polygon, two_footprint_hull);
+    projected_polygon = union_(projected_polygon, two_footprint_hull);
 
     // Add footprint to marker
-    geometry_msgs::Point previous_point = footprint.back();
-    for (const auto & point : footprint) {
-      collision_footprint_points.push_back(tf2_convert<tf2::Vector3>(previous_point));
-      collision_footprint_points.push_back(tf2_convert<tf2::Vector3>(point));
+    geometry_msgs::Point previous_point = footprint_proj.back();
+    for (const auto & point : footprint_proj) {
+      projected_footprint_points.push_back(tf2_convert<tf2::Vector3>(previous_point));
+      projected_footprint_points.push_back(tf2_convert<tf2::Vector3>(point));
       previous_point = point;
     }
   }
 
+  std_msgs::Header header;
+  header.stamp = ros::Time::now();
+  header.frame_id = viz_frame;
+  viz->publishCollisionFootprint(header, projected_footprint_points);
+
+  return projected_polygon;
+}
+
+uint8_t TrackingPidLocalPlanner::projectedCollisionCost(
+  costmap_2d::Costmap2D * costmap2d, const std::vector<geometry_msgs::Point> & footprint,
+  const std::vector<tf2::Transform> & projected_steps, std::unique_ptr<Visualization> & viz,
+  const std::string viz_frame)
+{
+  auto collision_polygon = projectionFootprint(footprint, projected_steps, viz, viz_frame);
+
+  // Calculate cost by checking base link location in costmap
+  uint8_t max_projected_step_cost = 0;
+  for (const auto & projection_tf : projected_steps) {
+    int map_x, map_y;
+    costmap2d->worldToMapEnforceBounds(
+      projection_tf.getOrigin().x(), projection_tf.getOrigin().y(), map_x, map_y);
+    uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y);
+    if (projected_step_cost > max_projected_step_cost) {
+      max_projected_step_cost = projected_step_cost;
+    }
+  }
+
   // Create a convex hull so we can use costmap2d->convexFillCells
   polygon_t collision_polygon_hull;
   boost::geometry::convex_hull(collision_polygon, collision_polygon_hull);
@@ -434,12 +468,9 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
   }
   std_msgs::Header header;
   header.stamp = ros::Time::now();
-  header.frame_id = map_frame_;
-  visualization_->publishCollisionObject(header, max_cost, collision_point);
-  visualization_->publishExtrapolatedPoses(header, step_points);
-  visualization_->publishgGoalPosesOnPath(header, poses_on_path_points);
-  visualization_->publishCollisionFootprint(header, collision_footprint_points);
-  visualization_->publishCollisionPolygon(header, collision_hull_points);
+  header.frame_id = viz_frame;
+  viz->publishCollisionObject(header, max_cost, collision_point);
+  viz->publishCollisionPolygon(header, collision_hull_points);
 
   return max_projected_step_cost;
 }