diff --git a/safe_teleop_base/src/safe_trajectory_planner_ros.cpp b/safe_teleop_base/src/safe_trajectory_planner_ros.cpp index 9f5a9d8..c91aff6 100644 --- a/safe_teleop_base/src/safe_trajectory_planner_ros.cpp +++ b/safe_teleop_base/src/safe_trajectory_planner_ros.cpp @@ -162,6 +162,7 @@ namespace safe_teleop { void SafeTrajectoryPlannerROS::cmdCallback(const geometry_msgs::Twist::ConstPtr& vel) { 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);