Add Backward/Forward recovery plugin

CMakeLists.txt

@@ -68,14 +68,16 @@ catkin_package(
 
 add_library(ftc_local_planner 
 	    src/ftc_planner.cpp
-        src/recovery_behaviors.cpp)
+        src/oscillation_detector.cpp)
 target_link_libraries(ftc_local_planner ${catkin_LIBRARIES})
 add_dependencies(ftc_local_planner ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
 add_dependencies(ftc_local_planner ftc_local_planner_gencfg)
 add_dependencies(ftc_local_planner ftc_local_planner_generate_messages_cpp)
 
+add_library(backward_forward_recovery src/backward_forward_recovery.cpp)
+target_link_libraries(backward_forward_recovery ${catkin_LIBRARIES})
 
-install(TARGETS ftc_local_planner
+install(TARGETS ftc_local_planner backward_forward_recovery
        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
        )

cfg/FTCPlanner.cfg

@@ -20,7 +20,7 @@ grp_pid.add("kd_lon", double_t, 0, "KD for speed controller lon", 0.0, 0.0, 1.0)
 grp_pid.add("ki_lat", double_t, 0, "KI for speed controller lat", 0.0, 0.0, 5.0)
 grp_pid.add("ki_lat_max", double_t, 0, "KI windup for speed controller lat", 10.0, 0.0, 500.0)
 grp_pid.add("kp_lat", double_t, 0, "KP for speed controller lat", 1.0, 0.0, 250.0)
-grp_pid.add("kd_lat", double_t, 0, "KD for speed controller lat", 0.0, 0.0, 5.0)
+grp_pid.add("kd_lat", double_t, 0, "KD for speed controller lat", 0.0, 0.0, 25.0)
 grp_pid.add("kp_ang", double_t, 0, "KP for angle controller", 1.0, 0.0, 50.0)
 grp_pid.add("ki_ang", double_t, 0, "KI for angle controller", 0.0, 0.0, 5.0)
 grp_pid.add("ki_ang_max", double_t, 0, "KI windup for speed controller angle", 10.0, 0.0, 500.0)

include/ftc_local_planner/backward_forward_recovery.h

@@ -0,0 +1,41 @@
+#ifndef BACKWARD_FORWARD_RECOVERY_H
+#define BACKWARD_FORWARD_RECOVERY_H
+
+#include <nav_core/recovery_behavior.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <tf2_ros/buffer.h>
+#include <base_local_planner/costmap_model.h>
+#include <geometry_msgs/Twist.h>
+#include <string>
+
+namespace ftc_local_planner
+{
+
+class BackwardForwardRecovery : public nav_core::RecoveryBehavior
+{
+public:
+  BackwardForwardRecovery();
+  void initialize(std::string name, tf2_ros::Buffer* tf,
+                  costmap_2d::Costmap2DROS* global_costmap,
+                  costmap_2d::Costmap2DROS* local_costmap);
+  void runBehavior();
+
+private:
+  bool attemptMove(double distance, bool forward);
+  bool isPositionValid(double x, double y);
+
+  std::string name_;
+  bool initialized_;     
+  tf2_ros::Buffer* tf_;
+  costmap_2d::Costmap2DROS* global_costmap_, *local_costmap_;
+  ros::Publisher cmd_vel_pub_;
+  double max_distance_;
+  double linear_vel_;
+  double check_frequency_;
+  unsigned char max_cost_threshold_;
+  ros::Duration timeout_;
+};
+
+}  // namespace ftc_local_planner
+
+#endif  // BACKWARD_FORWARD_RECOVERY_H

include/ftc_local_planner/ftc_planner.h

@@ -4,7 +4,7 @@
 
 #include <ros/ros.h>
 #include "ftc_local_planner/PlannerGetProgress.h"
-#include "ftc_local_planner/recovery_behaviors.h"
+#include "ftc_local_planner/oscillation_detector.h"
 
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/Path.h>

include/ftc_local_planner/recovery_behaviors.h → include/ftc_local_planner/oscillation_detector.h

@@ -36,8 +36,8 @@
  * Author: Christoph Rösmann
  *********************************************************************/
 
-#ifndef RECOVERY_BEHAVIORS_H__
-#define RECOVERY_BEHAVIORS_H__
+#ifndef OSCILLATION_DETECTOR_H__
+#define OSCILLATION_DETECTOR_H__
 
 
 #include <boost/circular_buffer.hpp>
@@ -132,6 +132,6 @@ class FailureDetector
 };
 
 
-} // namespace teb_local_planner
+} // namespace ftc_local_planner
 
-#endif /* RECOVERY_BEHAVIORS_H__ */
+#endif /* OSCILLATION_DETECTOR_H__ */

package.xml

@@ -40,6 +40,8 @@
     <run_depend>std_msgs</run_depend>
     <run_depend>tf2_eigen</run_depend>
 
-    <export><mbf_costmap_core plugin="${prefix}/mbf_plugin.xml"/></export>
-
+    <export>
+        <mbf_costmap_core plugin="${prefix}/mbf_plugin.xml"/>
+        <nav_core plugin="${prefix}/recovery_plugin.xml" />
+    </export>
 </package>

recovery_plugin.xml

@@ -0,0 +1,7 @@
+<library path="lib/libbackward_forward_recovery">
+  <class name="ftc_local_planner/BackwardForwardRecovery" type="ftc_local_planner::BackwardForwardRecovery" base_class_type="nav_core::RecoveryBehavior">
+    <description>
+      A simple recovery behavior plugin to try driving directly backwards and forwards.
+    </description>
+  </class>
+</library>

src/backward_forward_recovery.cpp

@@ -0,0 +1,130 @@
+#include <ftc_local_planner/backward_forward_recovery.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+#include <tf2/utils.h>
+#include <geometry_msgs/PoseStamped.h>
+
+namespace ftc_local_planner
+{
+
+BackwardForwardRecovery::BackwardForwardRecovery() 
+  : initialized_(false), 
+    max_distance_(0.5),
+    linear_vel_(0.3), 
+    check_frequency_(10.0), 
+    max_cost_threshold_(costmap_2d::INSCRIBED_INFLATED_OBSTACLE-10),
+    timeout_(ros::Duration(3.0)) {}
+
+void BackwardForwardRecovery::initialize(std::string name, tf2_ros::Buffer* tf,
+                                costmap_2d::Costmap2DROS* global_costmap,
+                                costmap_2d::Costmap2DROS* local_costmap)
+{
+  if(!initialized_){
+    name_ = name;
+    tf_ = tf;
+    global_costmap_ = global_costmap;
+    local_costmap_ = local_costmap;
+
+    ros::NodeHandle private_nh("~/" + name_);
+    cmd_vel_pub_ = private_nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+
+    private_nh.param("max_distance", max_distance_, 0.5);
+    private_nh.param("linear_vel", linear_vel_, 0.3);
+    private_nh.param("check_frequency", check_frequency_, 10.0);
+    int temp_threshold;
+    private_nh.param("max_cost_threshold", temp_threshold, static_cast<int>(costmap_2d::INSCRIBED_INFLATED_OBSTACLE-10));
+    max_cost_threshold_ = static_cast<unsigned char>(temp_threshold);
+
+    double timeout_seconds;
+    private_nh.param("timeout", timeout_seconds, 3.0);
+    timeout_ = ros::Duration(timeout_seconds);
+
+    initialized_ = true;
+  }
+  else{
+    ROS_ERROR("You should not call initialize twice on this object, doing nothing");
+  }
+}
+
+void BackwardForwardRecovery::runBehavior()
+{
+  if (!initialized_)
+  {
+    ROS_ERROR("This object must be initialized before runBehavior is called");
+    return;
+  }
+
+  ROS_WARN("Running Backward/Forward recovery behavior");
+
+  if (attemptMove(max_distance_, false)) {
+    ROS_INFO("Successfully moved backwards");
+    return;
+  }
+
+  if (attemptMove(max_distance_, true)) {
+    ROS_INFO("Successfully moved forwards");
+    return;
+  }
+
+  ROS_WARN("Backward/Forward recovery behavior failed to move in either direction");
+}
+
+bool BackwardForwardRecovery::attemptMove(double distance, bool forward)
+{
+  geometry_msgs::PoseStamped start_pose;
+  local_costmap_->getRobotPose(start_pose);
+
+  ros::Rate rate(check_frequency_);
+  geometry_msgs::Twist cmd_vel;
+  cmd_vel.linear.x = forward ? linear_vel_ : -linear_vel_;
+
+  double moved_distance = 0.0;
+  ros::Time start_time = ros::Time::now();
+  while (moved_distance < distance && (ros::Time::now() - start_time) < timeout_)
+  {
+    geometry_msgs::PoseStamped current_pose;
+    local_costmap_->getRobotPose(current_pose);
+
+    moved_distance = std::hypot(
+      current_pose.pose.position.x - start_pose.pose.position.x,
+      current_pose.pose.position.y - start_pose.pose.position.y
+    );
+
+    if (!isPositionValid(current_pose.pose.position.x, current_pose.pose.position.y))
+    {
+      ROS_WARN("Reached maximum allowed cost after moving %.2f meters", moved_distance);
+      cmd_vel.linear.x = 0;
+      cmd_vel_pub_.publish(cmd_vel);
+      return false;
+    }
+
+    cmd_vel_pub_.publish(cmd_vel);
+    rate.sleep();
+  }
+
+  cmd_vel.linear.x = 0;
+  cmd_vel_pub_.publish(cmd_vel);
+
+  if (moved_distance >= distance) {
+    ROS_INFO("%s movement completed successfully", forward ? "Forward" : "Backward");
+    return true;
+  } else {
+    ROS_WARN("%s movement timed out after %.2f seconds", forward ? "Forward" : "Backward", timeout_.toSec());
+    return false;
+  }
+}
+
+bool BackwardForwardRecovery::isPositionValid(double x, double y)
+{
+  unsigned int mx, my;
+  if (local_costmap_->getCostmap()->worldToMap(x, y, mx, my))
+  {
+    unsigned char cost = local_costmap_->getCostmap()->getCost(mx, my);
+    return (cost <= max_cost_threshold_);
+  }
+  return false;
+}
+
+}  // namespace ftc_local_planner
+
+PLUGINLIB_EXPORT_CLASS(ftc_local_planner::BackwardForwardRecovery, nav_core::RecoveryBehavior)

src/recovery_behaviors.cpp → src/oscillation_detector.cpp

@@ -36,7 +36,7 @@
  * Author: Christoph Rösmann
  *********************************************************************/
 
-#include <ftc_local_planner/recovery_behaviors.h>
+#include <ftc_local_planner/oscillation_detector.h>
 #include <functional>
 
 namespace ftc_local_planner