Skip to content
Snippets Groups Projects
Commit 1954cfa8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'master' into 'development' 

Added a parameter to use steer angle instead of the angular speed for the control.

See merge request !3
parents 3cd066a4 e101a8e9
No related branches found
No related tags found
2 merge requests!4Development,!3Added a parameter to use steer angle instead of the angular speed for the control.
Hide whitespace changes
Inline Side-by-side
cfg/ModelCarControl.cfg
+ 15
− 14
View file @ 1954cfa8
......@@ -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"))
config/car1_params.yaml
+ 1
− 1
View file @ 1954cfa8
......@@ -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
config/car2_params.yaml
+ 1
− 1
View file @ 1954cfa8
......@@ -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
config/params.yaml
+ 1
− 0
View file @ 1954cfa8
......@@ -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
src/model_car_control_alg_node.cpp
+ 19
− 4
View file @ 1954cfa8
......@@ -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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment