diff --git a/cfg/ModelCarControl.cfg b/cfg/ModelCarControl.cfg
index 834c1905aac93fb59d43cd2eb28ed579d1ec971c..fe5dbb95fed0c35baf34a6f8c736d27c5bfce5dd 100755
--- a/cfg/ModelCarControl.cfg
+++ b/cfg/ModelCarControl.cfg
@@ -38,19 +38,20 @@ from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Reconf.level Description Default Min Max
-gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
-gen.add("speed_Kp", double_t, 0, "Proportional constant of the speed PID", 1.0, 0.0, 1000.0)
-gen.add("speed_Ki", double_t, 0, "Integral constant of the speed PID", 0.0, 0.0, 1000.0)
-gen.add("speed_Kd", double_t, 0, "Differential constant of the speed PID", 0.0, 0.0, 1000.0)
-gen.add("speed_i_max", double_t, 0, "Maximum integral value pf the speed PID", 10.0, 0.0, 1000.0)
-gen.add("speed_deadband", double_t, 0, "Minimum valid speed command", 0.1, 0.1, 1.0)
-gen.add("watchdog_time", double_t, 0, "Maximum time between two control commands", 0.5, 0.0, 1000.0)
-gen.add("axel_distance", double_t, 0, "Distance between the two axels in meters", 0.36, 0.0, 10.0)
-gen.add("max_steer_angle", double_t, 0, "Maximum steering angle", 0.4, -1.0,1.0)
-gen.add("min_steer_angle", double_t, 0, "Minimum steering angle", -0.4, -1.0,1.0)
-gen.add("max_speed_control",double_t, 0, "Maximum output speed command value", 10.0, -100.0, 100.0)
-gen.add("min_speed_control",double_t, 0, "Minimum output speed command value", -10.0, -100.0, 100.0)
-gen.add("max_steer_control",double_t, 0, "Maximum output steer command value", 10.0, -100.0, 100.0)
-gen.add("min_steer_control",double_t, 0, "Minimum output steer command value", -10.0, -100.0, 100.0)
+gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
+gen.add("speed_Kp", double_t, 0, "Proportional constant of the speed PID", 1.0, 0.0, 1000.0)
+gen.add("speed_Ki", double_t, 0, "Integral constant of the speed PID", 0.0, 0.0, 1000.0)
+gen.add("speed_Kd", double_t, 0, "Differential constant of the speed PID", 0.0, 0.0, 1000.0)
+gen.add("speed_i_max", double_t, 0, "Maximum integral value pf the speed PID", 10.0, 0.0, 1000.0)
+gen.add("speed_deadband", double_t, 0, "Minimum valid speed command", 0.1, 0.1, 1.0)
+gen.add("watchdog_time", double_t, 0, "Maximum time between two control commands", 0.5, 0.0, 1000.0)
+gen.add("axel_distance", double_t, 0, "Distance between the two axels in meters", 0.36, 0.0, 10.0)
+gen.add("max_steer_angle", double_t, 0, "Maximum steering angle", 0.4, -1.0,1.0)
+gen.add("min_steer_angle", double_t, 0, "Minimum steering angle", -0.4, -1.0,1.0)
+gen.add("max_speed_control", double_t, 0, "Maximum output speed command value", 10.0, -100.0, 100.0)
+gen.add("min_speed_control", double_t, 0, "Minimum output speed command value", -10.0, -100.0, 100.0)
+gen.add("max_steer_control", double_t, 0, "Maximum output steer command value", 10.0, -100.0, 100.0)
+gen.add("min_steer_control", double_t, 0, "Minimum output steer command value", -10.0, -100.0, 100.0)
+gen.add("use_steer_angle_cmd",bool_t, 0, "Whether to use steer angle or angular speed as input",False)
exit(gen.generate(PACKAGE, "ModelCarControlAlgorithm", "ModelCarControl"))
diff --git a/config/car1_params.yaml b/config/car1_params.yaml
index dfab18d5dd2b4275d13323590b803e2b50ae4ca6..f2ce0cfd3183dd3eb152673ab189ba1a84c5e2b8 100644
--- a/config/car1_params.yaml
+++ b/config/car1_params.yaml
@@ -12,4 +12,4 @@ max_speed_control: 20.0
min_speed_control: -20.0
max_steer_control: 100.0
min_steer_control: -100.0
-
+use_steer_angle_cmd: False
diff --git a/config/car2_params.yaml b/config/car2_params.yaml
index dfab18d5dd2b4275d13323590b803e2b50ae4ca6..f2ce0cfd3183dd3eb152673ab189ba1a84c5e2b8 100644
--- a/config/car2_params.yaml
+++ b/config/car2_params.yaml
@@ -12,4 +12,4 @@ max_speed_control: 20.0
min_speed_control: -20.0
max_steer_control: 100.0
min_steer_control: -100.0
-
+use_steer_angle_cmd: False
diff --git a/config/params.yaml b/config/params.yaml
index cb2a7bdb92e204b66febbb3319f51f7a16ebb6ae..3c4cf20c2464831c50308b2d54af9de4cadc775e 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -12,4 +12,5 @@ max_speed_control: 20.0
min_speed_control: -20.0
max_steer_control: 62.0
min_steer_control: -77.0
+use_steer_angle_cmd: True
diff --git a/src/model_car_control_alg_node.cpp b/src/model_car_control_alg_node.cpp
index a697e0aa939ba00b9c99a56ed19a541b130da1b9..b4187d384122c278c1248e01d9229ed9d972a8a2 100644
--- a/src/model_car_control_alg_node.cpp
+++ b/src/model_car_control_alg_node.cpp
@@ -92,6 +92,16 @@ void ModelCarControlAlgNode::mainNodeThread(void)
else if(this->control_AckermannDriveStamped_msg_.drive.speed<this->config_.min_speed_control)
this->control_AckermannDriveStamped_msg_.drive.speed=this->config_.min_speed_control;
//angular speed
+ if(!this->config_.use_steer_angle_cmd)
+ {
+ if(this->steer_angle_control>=0)
+ this->control_AckermannDriveStamped_msg_.drive.steering_angle=(this->steer_angle_control/this->config_.max_steer_angle)*this->config_.max_steer_control;
+ else
+ this->control_AckermannDriveStamped_msg_.drive.steering_angle=(this->steer_angle_control/this->config_.min_steer_angle)*this->config_.min_steer_control;
+ }
+ }
+ if(this->config_.use_steer_angle_cmd)
+ {
if(this->steer_angle_control>=0)
this->control_AckermannDriveStamped_msg_.drive.steering_angle=(this->steer_angle_control/this->config_.max_steer_angle)*this->config_.max_steer_control;
else
@@ -125,11 +135,16 @@ void ModelCarControlAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstP
//this->cmd_vel_mutex_enter();
this->linear_speed_control=msg->linear.x;
- if(msg->angular.z==0.0)
- radius=std::numeric_limits<double>::max();
+ if(this->config_.use_steer_angle_cmd)
+ this->steer_angle_control=msg->angular.z;
else
- radius=msg->linear.x/msg->angular.z;
- this->steer_angle_control=atan2(this->config_.axel_distance,radius);
+ {
+ 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);
+ }
if(this->steer_angle_control>1.5707)
this->steer_angle_control-=3.14159;
if(this->steer_angle_control>this->config_.max_steer_angle)