Merge pull request SharedAutonomyToolkit#1 from furushchev/allow-back

add param to plan safe trajectory on backwards
furushchev · Nov 26, 2019 · 10f60fa · 10f60fa
2 parents 03afa1f + 5a196ff
commit 10f60fa
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 1 deletion.
diff --git a/safe_teleop_base/include/safe_teleop_base/safe_trajectory_planner_ros.h b/safe_teleop_base/include/safe_teleop_base/safe_trajectory_planner_ros.h
@@ -126,6 +126,7 @@ typedef tf::TransformListener TFListener;
 
       boost::recursive_mutex odom_lock_;
       double max_vel_th_, min_vel_th_;
+      bool safe_backwards_;
   };
 
 }

diff --git a/safe_teleop_base/src/safe_trajectory_planner_ros.cpp b/safe_teleop_base/src/safe_trajectory_planner_ros.cpp
@@ -78,6 +78,7 @@ namespace safe_teleop {
     private_nh.param("world_model", world_model_type, string("costmap"));
     private_nh.param("holonomic_robot", holonomic_robot, true);
     private_nh.param("dwa", dwa, true);
+    private_nh.param("safe_backwards", safe_backwards_, false);
 
     //parameters for using the freespace controller
     double min_pt_separation, max_obstacle_height, grid_resolution;
@@ -159,7 +160,8 @@ namespace safe_teleop {
   }
 
   void SafeTrajectoryPlannerROS::cmdCallback(const geometry_msgs::Twist::ConstPtr& vel) {
-    if ((vel->linear.x > 0) || (fabs(vel->linear.y) > 0)) {
+    if ((safe_backwards_  && ((fabs(vel->linear.x) > 0) || (fabs(vel->linear.y) > 0))) ||
+        (!safe_backwards_ && ((vel->linear.x > 0) || (fabs(vel->linear.y) > 0))))
       geometry_msgs::Twist safe_vel;
       if (computeVelocityCommands(vel, safe_vel)) {
         cmd_pub_.publish(safe_vel);
-Original file line number
+Diff line change
@@ Expand Up / @@ -126,6 +126,7 @@ typedef tf::TransformListener TFListener; @@
           boost::recursive_mutex odom_lock_;
           double max_vel_th_, min_vel_th_;
+          bool safe_backwards_;
       };
     }
@@ Expand Down @@