Discount the scaling of future footprints when scoring a trajectory (#90

)

base_local_planner/include/base_local_planner/obstacle_cost_function.h

@@ -61,9 +61,10 @@ class ObstacleCostFunction : public TrajectoryCostFunction {
 
   void setSumScores(bool score_sums){ sum_scores_=score_sums; }
 
-  void setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, double scaling_speed, bool occdist_use_footprint = true);
+  void setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation,
+                 double scaling_speed, double scaling_discount_factor = 1.0, bool occdist_use_footprint = true);
   void setFootprint(std::vector<geometry_msgs::Point> footprint_spec);
-  std::vector<geometry_msgs::Point> getScaledFootprint(const Trajectory& traj) const;
+  std::vector<geometry_msgs::Point> getScaledFootprint(const Trajectory& traj, unsigned int index = 0) const;
 
   // helper functions, made static for easy unit testing
   static double getScalingFactor(const Trajectory &traj, double scaling_speed, double max_trans_vel);
@@ -87,6 +88,7 @@ class ObstacleCostFunction : public TrajectoryCostFunction {
   ros::Subscriber sideward_inflation_scale_sub_;
   std::atomic<double> sideward_inflation_scale_;
   bool occdist_use_footprint_;
+  double scaling_discount_factor_;
 };
 
 } /* namespace base_local_planner */

base_local_planner/src/obstacle_cost_function.cpp

@@ -62,23 +62,31 @@ ObstacleCostFunction::~ObstacleCostFunction() {
   }
 }
 
-
-void ObstacleCostFunction::setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, double scaling_speed, bool occdist_use_footprint) {
+void ObstacleCostFunction::setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation,
+                                     double scaling_speed, double scaling_discount_factor, bool occdist_use_footprint) {
   // TODO: move this to prepare if possible
   max_trans_vel_ = max_trans_vel;
   max_forward_inflation_ = max_forward_inflation;
   max_sideward_inflation_ = max_sideward_inflation;
   scaling_speed_ = scaling_speed;
   occdist_use_footprint_ = occdist_use_footprint;
+  scaling_discount_factor_ = scaling_discount_factor;
 }
 
 void ObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
   footprint_spec_ = footprint_spec;
 }
 
-std::vector<geometry_msgs::Point> ObstacleCostFunction::getScaledFootprint(const Trajectory& traj) const {
+std::vector<geometry_msgs::Point> ObstacleCostFunction::getScaledFootprint(const Trajectory& traj, unsigned int index) const {
+  index = std::min(index, traj.getPointsSize() - 1);
   std::vector<geometry_msgs::Point> scaled_footprint = footprint_spec_;
-  const double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_);
+
+  // scaling_discount_factor is the discount factor applied to the last point in the trajectory
+  // so we need to compute the discount factor gamma to apply to the given index
+  const double max_index = std::max(traj.getPointsSize() - 1, 1u);
+  const double gamma = std::pow(scaling_discount_factor_, index / max_index);
+
+  const double scale = gamma * getScalingFactor(traj, scaling_speed_, max_trans_vel_);
   if (scale != 0.0) {
     const bool fwd = traj.xv_ > 0;
     const double forward_inflation = scale * max_forward_inflation_;
@@ -110,12 +118,11 @@ double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
     return -9;
   }
 
-  std::vector<geometry_msgs::Point> scaled_footprint = getScaledFootprint(traj);
-
    const unsigned int point_size = traj.getPointsSize();
   // ignore first trajectory point since it is the same for all trajectories,
   // unless trajectory only contains one point
   for (unsigned int i = point_size > 1 ? 1 : 0; i < point_size; ++i) {
+    const auto scaled_footprint = getScaledFootprint(traj, i);
     traj.getPoint(i, px, py, pth);
     double f_cost = footprintCost(px, py, pth,
         scaled_footprint,

dwa_local_planner/cfg/DWAPlanner.cfg

@@ -25,6 +25,7 @@ gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn b
 
 gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325)
 
+gen.add("scaling_discount_factor", double_t, 0, "Discount factor to apply to the scaling of the footprint in the last point in the trajectory. Discount factor for other footprints is automatically computed", 1.0, 0.0, 1.0)
 gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0)
 gen.add("max_forward_inflation", double_t, 0, "The maximum length the robot's footprint is extended forward by", 0.2, 0)
 gen.add("max_sideward_inflation", double_t, 0, "The maximum length the robot's footprint is extended sidewards by", 0.2, 0)

dwa_local_planner/include/dwa_local_planner/dwa_planner.h

@@ -161,9 +161,10 @@ namespace dwa_local_planner {
       /**
        * @brief Gets the footprint of the robot scaled by the given trajectory
        * @param traj Trajectory to evaluate
+       * @param index Index of the point in the trajectory to evaluate
        * @return Scaled footprint
        */
-      std::vector<geometry_msgs::Point> getScaledFootprint(const base_local_planner::Trajectory &traj) const;
+      std::vector<geometry_msgs::Point> getScaledFootprint(const base_local_planner::Trajectory& traj, unsigned int index = 0) const;
 
     private:
       /**

dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h

@@ -179,7 +179,7 @@ namespace dwa_local_planner {
 
       void publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path);
 
-      void publishScaledFootprint(const geometry_msgs::PoseStamped& pose, const base_local_planner::Trajectory &traj) const;
+      void publishScaledFootprint(const base_local_planner::Trajectory& traj) const;
 
       bool finishedBestEffort();
 

dwa_local_planner/src/dwa_planner.cpp

@@ -81,7 +81,8 @@ namespace dwa_local_planner {
     alignment_costs_.setXShift(forward_point_distance_);
 
     // obstacle costs can vary due to scaling footprint feature
-    obstacle_costs_.setParams(config.max_vel_trans, config.max_forward_inflation, config.max_sideward_inflation, config.scaling_speed, config.occdist_use_footprint);
+    obstacle_costs_.setParams(config.max_vel_trans, config.max_forward_inflation, config.max_sideward_inflation,
+                              config.scaling_speed, config.scaling_discount_factor, config.occdist_use_footprint);
 
     twirling_costs_.setScale(config.twirling_scale);
 
@@ -225,8 +226,8 @@ namespace dwa_local_planner {
     return planner_util_->setPlan(orig_global_plan);
   }
 
-  std::vector<geometry_msgs::Point> DWAPlanner::getScaledFootprint(const base_local_planner::Trajectory &traj) const {
-    return obstacle_costs_.getScaledFootprint(traj);
+  std::vector<geometry_msgs::Point> DWAPlanner::getScaledFootprint(const base_local_planner::Trajectory& traj, unsigned int index) const {
+    return obstacle_costs_.getScaledFootprint(traj, index);
   }
 
   /**

dwa_local_planner/src/dwa_planner_ros.cpp

@@ -46,6 +46,7 @@
 #include <base_local_planner/goal_functions.h>
 #include <nav_msgs/Path.h>
 #include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
 #include <tf2/utils.h>
 #include <numeric>
 
@@ -121,7 +122,7 @@ namespace dwa_local_planner {
       ros::NodeHandle private_nh("~/" + name);
       g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
       l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
-      scaled_fp_pub_ = private_nh.advertise<visualization_msgs::Marker>("scaled_footprint", 1);
+      scaled_fp_pub_ = private_nh.advertise<visualization_msgs::MarkerArray>("scaled_footprint", 1);
       tf_ = tf;
       costmap_ros_ = costmap_ros;
       costmap_ros_->getRobotPose(current_pose_);
@@ -244,18 +245,43 @@ namespace dwa_local_planner {
     base_local_planner::publishPlan(path, g_plan_pub_);
   }
 
-  void DWAPlannerROS::publishScaledFootprint(const geometry_msgs::PoseStamped& pose, const base_local_planner::Trajectory &traj) const {
-    visualization_msgs::Marker marker;
-    marker.header.frame_id = pose.header.frame_id;
-    marker.header.stamp = ros::Time::now();
-    marker.lifetime = ros::Duration(2 * dp_->getSimPeriod()); // double the sim period to avoid flickering
-    marker.type = visualization_msgs::Marker::LINE_STRIP;
-    marker.pose = pose.pose;
-    marker.scale.x = 0.01;
-    marker.color.g = marker.color.a = 1.0; // green
-    marker.points = dp_->getScaledFootprint(traj);
-    marker.points.push_back(marker.points.front()); // close the polygon
-    scaled_fp_pub_.publish(marker);
+  void DWAPlannerROS::publishScaledFootprint(const base_local_planner::Trajectory& traj) const {
+    if (scaled_fp_pub_.getNumSubscribers() == 0) {
+      return;
+    }
+
+    visualization_msgs::MarkerArray markers;
+
+    for (size_t i = 0; i < traj.getPointsSize(); ++i) {
+      visualization_msgs::Marker marker;
+      marker.header.frame_id = costmap_ros_->getGlobalFrameID();
+      marker.header.stamp = ros::Time::now();
+      marker.ns = i == 0 ? "current_footprint" : "future_footprints";
+      marker.id = i;
+      marker.lifetime = ros::Duration(2 * dp_->getSimPeriod());  // double the sim period to avoid flickering
+      marker.type = visualization_msgs::Marker::LINE_STRIP;
+
+      double yaw = 0;
+      traj.getPoint(i, marker.pose.position.x, marker.pose.position.y, yaw);
+      tf2::Quaternion quat_tf;
+      quat_tf.setRPY(0, 0, yaw);
+      marker.pose.orientation = tf2::toMsg(quat_tf);
+
+      // gradient: green -> yellow -> red
+      // https://stackoverflow.com/a/7947812
+      const double ratio = i / static_cast<double>(traj.getPointsSize() - 1);
+      marker.color.a = 1.0;
+      marker.color.r = std::min(2 * ratio, 1.0);
+      marker.color.g = std::min(2 * (1.0 - ratio), 1.0);
+      marker.scale.x = 0.01;
+
+      marker.points = dp_->getScaledFootprint(traj, i);
+      marker.points.push_back(marker.points.front());  // close the polygon
+
+      markers.markers.push_back(marker);
+    }
+
+    scaled_fp_pub_.publish(markers);
   }
 
   DWAPlannerROS::~DWAPlannerROS(){
@@ -386,7 +412,7 @@ namespace dwa_local_planner {
     }
 
     //publish information to the visualizer
-    publishScaledFootprint(global_pose, path);
+    publishScaledFootprint(path);
     publishLocalPlan(local_plan);
     return mbf_msgs::ExePathResult::SUCCESS;
   }