From cf0b6dd8f982dd809a4fd6444df4f906d894860d Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Wed, 9 Feb 2022 13:15:35 +0100
Subject: [PATCH] Add bool param to invert cmd_vel steering value. Add bool
param to enable/disable inplace steering change before each maneuver
---
cfg/AckermannLocalPlanner.cfg | 2 ++
include/ackermann_planner_ros.h | 2 ++
src/ackermann_planner_ros.cpp | 9 +++++++--
3 files changed, 11 insertions(+), 2 deletions(-)
diff --git a/cfg/AckermannLocalPlanner.cfg b/cfg/AckermannLocalPlanner.cfg
index a22f7c5..6592062 100755
--- a/cfg/AckermannLocalPlanner.cfg
+++ b/cfg/AckermannLocalPlanner.cfg
@@ -63,6 +63,8 @@ gen.add("trans_vel_samples", int_t, 0, "The number of samples to use w
gen.add("steer_angle_samples", int_t, 0, "The number of samples to use when exploring the steer angle space", 10, 1)
gen.add("angular_vel_samples", int_t, 0, "The number of samples to use when exploring the angular velocity space", 10, 1)
gen.add("use_steer_angle_cmd", bool_t, 0, "Whether to use the steer angle or the angular velocity.", False)
+gen.add("stopped_steering", bool_t, 0, "Whether to allow steering while stopped or not.", True)
+gen.add("invert_steer_angle_cmd", bool_t, 0, "Whether to use the invert the steer angle command or not.", False)
gen.add("prune_plan", bool_t, 0, "Start following closest point of global plan, not first point (if different).", False)
gen.add("xy_goal_tolerance", double_t, 0, "Within what maximum distance we consider the robot to be in goal", 0.1)
diff --git a/include/ackermann_planner_ros.h b/include/ackermann_planner_ros.h
index 5b45993..5a34c8c 100644
--- a/include/ackermann_planner_ros.h
+++ b/include/ackermann_planner_ros.h
@@ -148,5 +148,7 @@ class AckermannPlannerROS : public nav_core::BaseLocalPlanner
bool stucked;
bool first;
bool use_steer_angle_cmd;
+ bool stopped_steering;
+ bool invert_steer_angle_cmd;
};
#endif
diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index 7d08e2f..9c43ed8 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/src/ackermann_planner_ros.cpp
@@ -100,6 +100,8 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke
planner_util_.reconfigure_callback(limits, config.restore_defaults);
this->last_cmds.resize(config.cmd_vel_avg);
this->use_steer_angle_cmd=config.use_steer_angle_cmd;
+ this->invert_steer_angle_cmd=config.invert_steer_angle_cmd;
+ this->stopped_steering=config.stopped_steering;
odom_helper_.set_average_samples(config.odom_avg);
patience_=config.planner_patience;
@@ -216,7 +218,10 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
//pass along drive commands
cmd_vel.linear.x = drive_cmds.pose.position.x;
cmd_vel.linear.y = drive_cmds.pose.position.y;
- cmd_vel.angular.z = tf::getYaw(drive_cmds.pose.orientation);
+ if(this->invert_steer_angle_cmd)
+ cmd_vel.angular.z = -tf::getYaw(drive_cmds.pose.orientation);
+ else
+ cmd_vel.angular.z = tf::getYaw(drive_cmds.pose.orientation);
//if we cannot move... tell someone
std::vector<geometry_msgs::PoseStamped> local_plan;
@@ -442,7 +447,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
ackermann_msgs::AckermannDrive ackermann;
odom_helper_.get_ackermann_state(ackermann);
// steer the car without moving if the steering angle is big
- if(this->use_steer_angle_cmd)
+ if(this->use_steer_angle_cmd && this->stopped_steering)
{
if(fabs(ackermann.steering_angle-cmd_vel.angular.z)>0.05)
{
--
GitLab