Fix small bugs and improvements

CMakeLists.txt

@@ -7,11 +7,13 @@ find_package(catkin REQUIRED
   COMPONENTS
     actionlib
     actionlib_msgs
+    base_local_planner
     dynamic_reconfigure
     geometry_msgs
     mbf_costmap_core
     mbf_msgs
     message_generation
+    nav_core
     nav_msgs
     pluginlib
     roscpp
@@ -46,11 +48,13 @@ catkin_package(
   CATKIN_DEPENDS
     actionlib
     actionlib_msgs
+    base_local_planner
     dynamic_reconfigure
     geometry_msgs
     mbf_costmap_core
     mbf_msgs
     message_runtime
+    nav_core
     nav_msgs
     pluginlib
     roscpp

cfg/Pid.cfg

@@ -26,13 +26,13 @@ gen.add("init_vel_method", int_t, 0, "Initial velocity method", 1, 0, 3, edit_me
     ], "Initial velocity method enum"))
 gen.add("init_vel_max_diff", double_t, 0, "Maximum vel-diff allowed when starting a path (-1 to ignore, only active upon 'init_vel_method'==InternalSetpoint)", 0.5, -1, 10)
 
-gen.add("Kp_lat", double_t, 0, "Kp Lateral", 1, 0, 10)
-gen.add("Ki_lat", double_t, 0, "Ki Lateral", 0, 0, 2)
-gen.add("Kd_lat", double_t, 0, "Kd Lateral", 0.3, 0, 10)
+gen.add("Kp_lat", double_t, 0, "Kp Lateral", 1, 0, 1000)
+gen.add("Ki_lat", double_t, 0, "Ki Lateral", 0, 0, 200)
+gen.add("Kd_lat", double_t, 0, "Kd Lateral", 0.3, 0, 1000)
 
-gen.add("Kp_ang", double_t, 0, "Kp Angular", 1, 0, 10)
-gen.add("Ki_ang", double_t, 0, "Ki Angular", 0, 0, 2)
-gen.add("Kd_ang", double_t, 0, "Kd Angular", 0.3, 0, 10)
+gen.add("Kp_ang", double_t, 0, "Kp Angular", 1, 0, 1000)
+gen.add("Ki_ang", double_t, 0, "Ki Angular", 0, 0, 200)
+gen.add("Kd_ang", double_t, 0, "Kd Angular", 0.3, 0, 1000)
 
 gen.add("lowpass_cutoff", double_t, 0, "Lowpass cutoff (Hz), 0 disables the filter", 0, 0, 1000)
 gen.add("lowpass_damping", double_t, 0, "Lowpass damping", sqrt(2), 0, 10)
@@ -64,6 +64,6 @@ gen.add("anti_collision", bool_t, 0, "Stop on lethal obstacles", False)
 collision_group = gen.add_group("collision_group", type="hide")
 collision_group.add("obstacle_speed_reduction", bool_t, 0, "Slow down on near obstacles", True)
 collision_group.add("collision_look_ahead_length_offset", double_t, 0, "Offset in length to project rectangle collision along path", 1.0, 0, 5)
-collision_group.add("collision_look_ahead_resolution", double_t, 0, "Spatial resolution to project rectangle collision along path", 1.0, 1e-9, 10)
+collision_group.add("collision_look_ahead_time", double_t, 0, "The time (s) to project a velocity command forward to check for collisions", 3.0, 0, 20)
 
 exit(gen.generate(PACKAGE, "path_tracking_pid", "Pid"))

include/path_tracking_pid/path_tracking_pid_local_planner.hpp

@@ -1,9 +1,13 @@
 #pragma once
 
+#include <base_local_planner/costmap_model.h>
+#include <base_local_planner/odometry_helper_ros.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <dynamic_reconfigure/server.h>
 #include <geometry_msgs/Point.h>
+#include <geometry_msgs/Pose2D.h>
 #include <mbf_costmap_core/costmap_controller.h>
+#include <nav_core/base_local_planner.h>
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/Path.h>
 #include <path_tracking_pid/PidConfig.h>
@@ -25,8 +29,9 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(geometry_msgs::Point, double, cs::cartesian, x,
 
 namespace path_tracking_pid
 {
-class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
-                                private boost::noncopyable
+class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController
+                              , public nav_core::BaseLocalPlanner
+                              , private boost::noncopyable
 {
 private:
   using polygon_t = boost::geometry::model::ring<geometry_msgs::Point>;
@@ -68,6 +73,14 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
     const geometry_msgs::PoseStamped & pose, const geometry_msgs::TwistStamped & velocity,
     geometry_msgs::TwistStamped & cmd_vel, std::string & message) override;
 
+  /**
+   * @brief  Given the current position, orientation, and velocity of the robot,
+   * compute velocity commands to send to the base
+   * @param cmd_vel Will be filled with the velocity command to be passed to the robot base
+   * @return True if a valid trajectory was found, false otherwise
+   */
+  bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) override;
+
   /**
    * @brief Returns true, if the goal is reached. Currently does not respect the parameters given.
    * @param dist_tolerance Tolerance in distance to the goal
@@ -77,7 +90,7 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
   bool isGoalReached(double dist_tolerance, double angle_tolerance) override;
 
   /**
-   * @brief Canceles the planner.
+   * @brief Cancels the planner.
    * @return True on cancel success.
    */
   bool cancel() override;
@@ -98,7 +111,7 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
    * @brief Returns true, if the goal is reached.
    * @return true, if the goal is reached
    */
-  bool isGoalReached() const;
+  bool isGoalReached() override;
 
   /**
    * Accept a new configuration for the PID controller
@@ -112,12 +125,6 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
 
   void velMaxExternalCallback(const std_msgs::Float64 & msg);
 
-  /**
-   * @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
@@ -131,31 +138,33 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
     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);
+  bool inCollision( const double & x, const double & y,
+                    const double & theta);
+
+  bool isCollisionImminent(
+      const geometry_msgs::PoseStamped & robot_pose,
+      const double & linear_vel, const double & angular_vel);
+
+  tf2::Quaternion createQuaternionFromYaw(double yaw)
+  {
+    tf2::Quaternion q;
+    q.setRPY(0, 0, yaw);
+    return q;
+  }
 
   nav_msgs::Odometry latest_odom_;
   ros::Time prev_time_;
   ros::Duration prev_dt_;
   path_tracking_pid::Controller pid_controller_;
 
   // Obstacle collision detection
-  costmap_2d::Costmap2DROS * costmap_ = nullptr;
+  costmap_2d::Costmap2D * costmap_ = nullptr;
+  costmap_2d::Costmap2DROS * costmap_ros_ = nullptr;
+  // For retrieving robot footprint cost
+  base_local_planner::CostmapModel* costmap_model_ = nullptr;
 
   // Cancel flags (multi threaded, so atomic bools)
   std::atomic<bool> active_goal_{false};
-  std::atomic<bool> cancel_requested_{false};
   std::atomic<bool> cancel_in_progress_{false};
 
   // dynamic reconfiguration
@@ -171,7 +180,6 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
   std::unique_ptr<Visualization> visualization_;
   ros::Publisher path_pub_;
 
-  ros::Subscriber sub_odom_;
   ros::Publisher feedback_pub_;
 
   ros::Subscriber sub_vel_max_external_;
@@ -187,6 +195,14 @@ class TrackingPidLocalPlanner : public mbf_costmap_core::CostmapController,
 
   // Controller logic
   bool controller_debug_enabled_ = false;
+
+  std::vector<geometry_msgs::PoseStamped> global_plan_map_frame_;
+
+  // Odometry helper class
+  base_local_planner::OdometryHelperRos odom_helper_;
+
+  // Projected footprints for collision detection
+  std::vector<tf2::Transform> projection_steps_;
 };
 
 }  // namespace path_tracking_pid

package.xml

@@ -18,10 +18,12 @@
   <build_export_depend>message_runtime</build_export_depend>
   <depend>actionlib</depend>
   <depend>actionlib_msgs</depend>
+  <depend>base_local_planner</depend>
   <depend>dynamic_reconfigure</depend>
   <depend>geometry_msgs</depend>
   <depend>mbf_costmap_core</depend>
   <depend>mbf_msgs</depend>
+  <depend>nav_core</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>roscpp</depend>

path_tracking_pid_plugin.xml

@@ -4,4 +4,9 @@
             Local planner that tries to stay on the global path as well as it can, using multiple SISO PID controllers for steering calculation and an open-loop strategy for forward velocity
         </description>
     </class>
+    <class name="path_tracking_pid/TrackingPidLocalPlanner" type="path_tracking_pid::TrackingPidLocalPlanner" base_class_type="nav_core::BaseLocalPlanner">
+        <description>
+            Local planner that tries to stay on the global path as well as it can, using multiple SISO PID controllers for steering calculation and an open-loop strategy for forward velocity
+        </description>
+    </class>
 </library>

src/controller.cpp

@@ -40,7 +40,7 @@ bool is_pose_angle_obtuse(
 /**
  * Checks the given plan. The first and last poses are always accepted (if they exist). Intermediate
  * poses are only accepted if the angle (with respect to the previous and next poses) is obtuse.
- * 
+ *
  * @param[in] plan Plan to check.
  * @return True if all poses in the plan are accepted. False otherwise.
  */
@@ -206,13 +206,6 @@ bool Controller::setPlan(
       break;
   }
 
-  // When velocity error is too big reset current_x_vel
-  if (fabs(odom_twist.linear.x - controller_state_.current_x_vel) > config_.max_error_x_vel) {
-    // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here
-    ROS_WARN(
-      "Large control error. Current_x_vel %f / odometry %f", controller_state_.current_x_vel,
-      odom_twist.linear.x);
-  }
   controller_state_.end_phase_enabled = false;
   controller_state_.end_reached = false;
 
@@ -331,7 +324,7 @@ Controller::FindPoseOnPlanResult Controller::findPoseOnPlan(
 
 Controller::UpdateResult Controller::update(
   double target_x_vel, double target_end_x_vel, const tf2::Transform & current_tf,
-  const geometry_msgs::Twist & odom_twist, ros::Duration dt)
+  const geometry_msgs::Twist & /*odom_twist*/, ros::Duration dt)
 {
   UpdateResult result;
 
@@ -448,7 +441,7 @@ Controller::UpdateResult Controller::update(
   // However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase.
   // This is to avoid skipping paths that start with opposite velocity.
   if ((distance_to_goal <= fabs(d_end_phase)) && in_direction_of_goal) {
-    // This state will be remebered to avoid jittering on target_x_vel
+    // This state will be remembered to avoid jittering on target_x_vel
     controller_state_.end_phase_enabled = true;
   }
 
@@ -499,15 +492,6 @@ Controller::UpdateResult Controller::update(
     new_x_vel = min_vel;
   }
 
-  // When velocity error is too big reset current_x_vel
-  if (
-    fabs(odom_twist.linear.x) < fabs(current_target_x_vel_) &&
-    fabs(odom_twist.linear.x - new_x_vel) > config_.max_error_x_vel) {
-    // TODO(clopez/mcfurry/nobleo): Give feedback to higher level software here
-    ROS_WARN_THROTTLE(
-      1.0, "Large tracking error. Current_x_vel %f / odometry %f", new_x_vel, odom_twist.linear.x);
-  }
-
   // Force target_end_x_vel at the very end of the path
   // Or when the end velocity is reached.
   // Warning! If target_end_x_vel == 0 and min_vel = 0 then the robot might not reach end pose