Factored out creation of projected footprint

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)
   {
@@ -118,6 +118,19 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
    */
   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

src/path_tracking_pid_local_planner.cpp

@@ -352,29 +352,20 @@ std::vector<tf2::Transform> TrackingPidLocalPlanner::projectionSteps()
   return projected_steps_tf;
 }
 
-uint8_t TrackingPidLocalPlanner::projectedCollisionCost(
-  costmap_2d::Costmap2D * costmap2d, const std::vector<geometry_msgs::Point> & footprint,
+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> collision_footprint_points;
+  std::vector<tf2::Vector3> projected_footprint_points;
   polygon_t previous_footprint_xy;
-  polygon_t collision_polygon;
-  uint8_t max_projected_step_cost = 0;
+  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_proj;
     costmap_2d::transformFootprint(x, y, yaw, footprint, footprint_proj);
@@ -390,17 +381,44 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost(
     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_proj.back();
     for (const auto & point : footprint_proj) {
-      collision_footprint_points.push_back(tf2_convert<tf2::Vector3>(previous_point));
-      collision_footprint_points.push_back(tf2_convert<tf2::Vector3>(point));
+      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);
@@ -452,7 +470,6 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost(
   header.stamp = ros::Time::now();
   header.frame_id = viz_frame;
   viz->publishCollisionObject(header, max_cost, collision_point);
-  viz->publishCollisionFootprint(header, collision_footprint_points);
   viz->publishCollisionPolygon(header, collision_hull_points);
 
   return max_projected_step_cost;