From 15afd12d1bf3badd3cd9f11568ce602719155efc Mon Sep 17 00:00:00 2001
From: Sergi Hernandez <shernand@iri.upc.edu>
Date: Thu, 25 Feb 2021 17:41:44 +0100
Subject: [PATCH] Solved a bug: the steering angle changed with the sign
tranlational velocity. The radius is now computed without sign.
---
src/model_car_control_alg_node.cpp | 7 +++++--
1 file changed, 5 insertions(+), 2 deletions(-)
diff --git a/src/model_car_control_alg_node.cpp b/src/model_car_control_alg_node.cpp
index 28c23e3..8fd656b 100644
--- a/src/model_car_control_alg_node.cpp
+++ b/src/model_car_control_alg_node.cpp
@@ -125,8 +125,11 @@ void ModelCarControlAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstP
if(msg->angular.z==0.0)
radius=std::numeric_limits<double>::max();
else
- radius=msg->linear.x/msg->angular.z;
- this->steer_angle_control=atan2(this->config_.axel_distance,radius);
+ radius=fabs(msg->linear.x/msg->angular.z);
+ if(msg->angular.z>0.0)
+ this->steer_angle_control=atan2(this->config_.axel_distance,radius);
+ else
+ this->steer_angle_control=atan2(this->config_.axel_distance,-radius);
if(this->steer_angle_control>1.5707)
this->steer_angle_control-=3.14159;
if(this->steer_angle_control>this->config_.max_steer_angle)
--
GitLab