From 11e44095dc02f7eaad6fb7c8e3f92ebf11b13415 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Thu, 13 Jul 2023 09:11:18 +0200
Subject: [PATCH] Added a bypass parameter to be able to disable its operation
when necessary.
---
cfg/SafeCmd.cfg | 1 +
src/safe_cmd_alg_node.cpp | 27 ++++++++++++++++-----------
2 files changed, 17 insertions(+), 11 deletions(-)
diff --git a/cfg/SafeCmd.cfg b/cfg/SafeCmd.cfg
index 33d3535..8d13bb9 100755
--- a/cfg/SafeCmd.cfg
+++ b/cfg/SafeCmd.cfg
@@ -40,6 +40,7 @@ ackermann = gen.add_group("ackermann")
# Name Type Reconfiguration level Description Default Min Max
gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
+gen.add("bypass", bool_t, 0, "Whether or not to bypassa the safety fwature", False)
gen.add("safety_distance", double_t, 0, "Safety distance to possible obstacles",0.5, 0.0, 10.0)
gen.add("traj_sim_distance", double_t, 0, "Distance for the trajectory computation", 1.0, 0.1, 5.0)
gen.add("traj_sim_time", double_t, 0, "Time for the trajectory computation", 1.0, 0.1, 5.0)
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index d7ff458..dd85a25 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -171,20 +171,25 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
this->alg_.lock();
//cmd_vel_mutex_.enter();
- this->Twist_msg_=*msg;
- // compute the robot trajectory depending on the architecture
- if(this->config.architecture==0)// ackermann using steering angle
- this->alg_.compute_trajectory_ackermann_steer_angle(msg->linear.x,msg->angular.z,this->traj);
- else if(this->config.architecture==1)// ackermann using angular velocity
- this->alg_.compute_trajectory_ackermann_angular_speed(msg->linear.x,msg->angular.z,this->traj);
- else if(this->config.architecture==2)// differential
- this->alg_.compute_trajectory_differential(msg->linear.x,msg->angular.z,this->traj);
+ if(this->config.bypass)
+ cmd_vel_safe_publisher_.publish(msg);
else
{
- ROS_ERROR("SafeCmdAlgNode: unknown robot architecture");
- return;
+ this->Twist_msg_=*msg;
+ // compute the robot trajectory depending on the architecture
+ if(this->config.architecture==0)// ackermann using steering angle
+ this->alg_.compute_trajectory_ackermann_steer_angle(msg->linear.x,msg->angular.z,this->traj);
+ else if(this->config.architecture==1)// ackermann using angular velocity
+ this->alg_.compute_trajectory_ackermann_angular_speed(msg->linear.x,msg->angular.z,this->traj);
+ else if(this->config.architecture==2)// differential
+ this->alg_.compute_trajectory_differential(msg->linear.x,msg->angular.z,this->traj);
+ else
+ {
+ ROS_ERROR("SafeCmdAlgNode: unknown robot architecture");
+ return;
+ }
+ this->new_cmd_vel=true;
}
- this->new_cmd_vel=true;
//cmd_vel_mutex_.exit();
this->alg_.unlock();
--
GitLab