add footprint inflation for higher speeds (#32)

* add a feature to inflate the footprint at higher speeds
* only update max_vel_x once per call

graceful_controller_ros/cfg/GracefulController.cfg

@@ -38,4 +38,9 @@ gen.add("yaw_gap_tolerance", double_t, 0, "Maximum distance between poses in the
 # Goal tolerance latch
 gen.add("latch_xy_goal_tolerance", bool_t, 0, "When goal has been reached, just fix heading", False)
 
+# Footprint scaling at higher speeds
+gen.add("scaling_vel_x", double_t, 0, "Above this velocity, the footprint will be scaled up", 0.5, 0.0);
+gen.add("scaling_factor", double_t, 0, "Amount to scale footprint when at max velocity", 0.0, 0.0);
+gen.add("scaling_step", double_t, 0, "Amount to reduce x velocity when iteratively reducing velocity", 0.1, 0.01, 1.0);
+
 exit(gen.generate("graceful_controller", "graceful_controller", "GracefulController"))

graceful_controller_ros/include/graceful_controller_ros/graceful_controller_ros.hpp

@@ -111,6 +111,14 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
 private:
   void velocityCallback(const std_msgs::Float32::ConstPtr& max_vel_x);
 
+  /**
+   * @brief Simulate a path.
+   * @param target_pose Pose to simulate towards.
+   * @param cmd_vel The returned command to execute.
+   * @returns True if the path is valid.
+   */
+  bool simulate(const geometry_msgs::PoseStamped& target_pose, geometry_msgs::Twist& cmd_vel);
+
   ros::Publisher global_plan_pub_, local_plan_pub_, target_pose_pub_;
   ros::Subscriber max_vel_sub_;
 
@@ -119,6 +127,7 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
 
   tf2_ros::Buffer* buffer_;
   costmap_2d::Costmap2DROS* costmap_ros_;
+  geometry_msgs::TransformStamped robot_to_costmap_transform_;
   base_local_planner::LocalPlannerUtil planner_util_;
   base_local_planner::OdometryHelperRos odom_helper_;
 
@@ -131,6 +140,9 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
   double min_in_place_vel_theta_;
   double acc_lim_x_;
   double acc_lim_theta_;
+  double scaling_vel_x_;
+  double scaling_factor_;
+  double scaling_step_;
   double xy_goal_tolerance_;
   double yaw_goal_tolerance_;
   double min_lookahead_;