Commit 16fa933f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'development' into 'master'

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

See merge request !3
parents 8542a97a 9f5fb74a
......@@ -40,55 +40,61 @@ from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
gen.add("max_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 10, 0, 20)
gen.add("min_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0)
gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0)
gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 32.0, 0.0)
gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 24.0, 0.0)
gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0)
gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0)
gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0)
gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0)
gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325)
gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0)
gen.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0)
gen.add("trans_vel_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1)
gen.add("steer_angle_samples", int_t, 0, "The number of samples to use when exploring the y velocity space", 10, 1)
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)
gen.add("yaw_goal_tolerance", double_t, 0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1)
gen.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1)
gen.add("rot_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1)
gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False)
gen.add("max_trans_vel", double_t, 0, "maximum translational speed", 0.3, 0, 20)
gen.add("min_trans_vel", double_t, 0, "minimum translational speed", -0.3, -20, 0)
gen.add("max_trans_acc", double_t, 0, "maximum translational acceleration", 1.0, 0, 20)
gen.add("max_steer_angle", double_t, 0, "maximum steer angle", 0.45, 0, 0.54)
gen.add("min_steer_angle", double_t, 0, "minimum steer angle", -0.45, -0.54, 0)
gen.add("max_steer_vel", double_t, 0, "maximum steer speed", 1.0, 0, 2)
gen.add("min_steer_vel", double_t, 0, "minimum steer speed", -1.0, -2, 0)
gen.add("max_steer_acc", double_t, 0, "maximum steer acceleration", 0.36, 0, 5)
gen.add("axis_distance", double_t, 0, "distance between axes", 1.65, -2.0, 2)
gen.add("wheel_distance", double_t, 0, "distance between wheels", 1.2, 0, 2)
gen.add("wheel_radius", double_t, 0, "Wheel diameter", 0.436, 0, 2)
gen.add("split_ignore_length", double_t, 0, "Paths segments under this length will be ignored",1.0,0.0,2.0)
gen.add("cmd_vel_avg", int_t, 0, "Number of cmd_vel to average", 1, 1, 20)
gen.add("odom_avg", int_t, 0, "Number of odom to average", 1, 1, 20)
gen.add("planner_patience", int_t, 0, "number of impossible paths before replanning",2, 0, 10)
gen.add("hdiff_scale", double_t, 0, "The weight for the heading distance part of the cost function", 1.0 ,0, 500)
gen.add("heading_points", int_t, 0, "The number of points to check the heading", 8, 1, 64)
# Name Type Reconfiguration level Description Default Min Max
gen.add("max_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 10, 0, 20)
gen.add("min_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0)
gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0)
gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 32.0, 0.0)
gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 24.0, 0.0)
gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0)
gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0)
gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0)
gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0)
gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325)
gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0)
gen.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0)
gen.add("trans_vel_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1)
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("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)
gen.add("yaw_goal_tolerance", double_t, 0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1)
gen.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1)
gen.add("rot_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1)
gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False)
gen.add("max_trans_vel", double_t, 0, "maximum translational speed", 0.3, 0, 20)
gen.add("min_trans_vel", double_t, 0, "minimum translational speed", -0.3, -20, 0)
gen.add("max_trans_acc", double_t, 0, "maximum translational acceleration", 1.0, 0, 20)
gen.add("max_steer_angle", double_t, 0, "maximum steer angle", 0.45, 0, 0.54)
gen.add("min_steer_angle", double_t, 0, "minimum steer angle", -0.45, -0.54, 0)
gen.add("max_steer_vel", double_t, 0, "maximum steer speed", 1.0, 0, 2)
gen.add("min_steer_vel", double_t, 0, "minimum steer speed", -1.0, -2, 0)
gen.add("max_steer_acc", double_t, 0, "maximum steer acceleration", 0.36, 0, 5)
gen.add("max_angular_vel", double_t, 0, "maximum angular speed", 0.3, 0, 20)
gen.add("min_angular_vel", double_t, 0, "minimum angular speed", -0.3, -20, 0)
gen.add("axis_distance", double_t, 0, "distance between axes", 1.65, -2.0, 2)
gen.add("wheel_distance", double_t, 0, "distance between wheels", 1.2, 0, 2)
gen.add("wheel_radius", double_t, 0, "Wheel diameter", 0.436, 0, 2)
gen.add("use_trans_vel_deadzone", bool_t, 0, "Whether to use a deadzone in the translational velocity or not", False)
gen.add("trans_vel_deadzone", double_t, 0, "Translatinal velocity minimum value", 0.1, 0.01, 1.0)
gen.add("split_ignore_length", double_t, 0, "Paths segments under this length will be ignored", 1.0, 0.0, 2.0)
gen.add("cmd_vel_avg", int_t, 0, "Number of cmd_vel to average", 1, 1, 20)
gen.add("odom_avg", int_t, 0, "Number of odom to average", 1, 1, 20)
gen.add("planner_patience", int_t, 0, "number of impossible paths before replanning", 2, 0, 10)
gen.add("hdiff_scale", double_t, 0, "The weight for the heading distance part of the cost function", 1.0, 0, 500)
gen.add("heading_points", int_t, 0, "The number of points to check the heading", 8, 1, 64)
exit(gen.generate(PACKAGE, "AckermannLocalPlannerAlgorithm", "AckermannLocalPlanner"))
......@@ -51,11 +51,16 @@ class AckermannPlannerLimits
double max_steer_vel;
double min_steer_vel;
double max_steer_acc;
// angular velocity limits
double max_angular_vel;
double min_angular_vel;
// kinematic attributes
double axis_distance;
double wheel_distance;
double wheel_radius;
// general planner limits
bool use_trans_vel_deadzone;
double trans_vel_deadzone;
double split_ignore_length;
bool prune_plan;
double xy_goal_tolerance;
......@@ -77,11 +82,16 @@ class AckermannPlannerLimits
double max_steer_vel,
double min_steer_vel,
double max_steer_acc,
// angular velocity limits
double max_angular_vel,
double min_angular_vel,
// kinematic attributes
double axis_distance,
double wheel_distance,
double wheel_radius,
// general planner limits
bool use_trans_vel_deadzone,
double trans_vel_deadzone,
double split_ignore_length,
double xy_goal_tolerance,
double yaw_goal_tolerance,
......@@ -96,9 +106,13 @@ class AckermannPlannerLimits
max_steer_vel(max_steer_vel),
min_steer_vel(min_steer_vel),
max_steer_acc(max_steer_acc),
max_angular_vel(max_angular_vel),
min_angular_vel(min_angular_vel),
axis_distance(axis_distance),
wheel_distance(wheel_distance),
wheel_radius(wheel_radius),
use_trans_vel_deadzone(use_trans_vel_deadzone),
trans_vel_deadzone(trans_vel_deadzone),
split_ignore_length(split_ignore_length),
prune_plan(prune_plan),
xy_goal_tolerance(xy_goal_tolerance),
......
......@@ -147,5 +147,6 @@ class AckermannPlannerROS : public nav_core::BaseLocalPlanner
bool stucked;
bool first;
bool use_steer_angle_cmd;
};
#endif
......@@ -91,6 +91,7 @@ class AckermannTrajectoryGenerator: public base_local_planner::TrajectorySampleG
double min_sim_time,
double sim_granularity,
double angular_sim_granularity,
bool use_steer_angle_cmd,
double sim_period = 0.0);
/**
......@@ -126,6 +127,7 @@ class AckermannTrajectoryGenerator: public base_local_planner::TrajectorySampleG
double max_sim_time_,min_sim_time_,sim_time_;
double sim_granularity_, angular_sim_granularity_;
double sim_period_; // only for dwa
bool use_steer_angle_cmd_;
};
#endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */
base_local_planner: "AckermannPlannerROS"
AckermannPlannerROS:
max_sim_time: 15 #8.0
max_sim_time: 3.0
min_sim_time: 1.0
sim_granularity: 0.2
angular_sim_granularity: 0.02
path_distance_bias: 3.0
goal_distance_bias: 0.5
occdist_scale: 0.01
path_distance_bias: 50.0
goal_distance_bias: 10.0
occdist_scale: 0.05
# stop_time_buffer:
oscillation_reset_dist: 0.002
oscillation_reset_angle: 0.001
# forward_point_distance:
# scaling_speed:
# max_scaling_factor:
trans_vel_samples: 21
steer_angle_samples: 11
trans_vel_samples: 31
steer_angle_samples: 21
angular_vel_samples: 21
use_steer_angle_cmd: false
prune_plan: true
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.5
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.2
trans_stopped_vel: 0.1
rot_stopped_vel: 0.1
restore_defaults: false
max_trans_vel: 2.0 #1.0
min_trans_vel: 0.0 #-1.0
max_trans_acc: 0.3
max_steer_angle: 0.35
min_steer_angle: -0.35
max_steer_vel: 0.5
min_steer_vel: -0.5
max_steer_acc: 1.0
axis_distance: 4.3
wheel_distance: 4.5
wheel_radius: 0.65465
cmd_vel_avg: 10
max_trans_vel: 1.0
min_trans_vel: -1.0
max_trans_acc: 0.5
max_steer_angle: 0.5
min_steer_angle: -0.5
max_steer_vel: 1.0
min_steer_vel: -1.0
max_steer_acc: 5.0
max_angular_vel: 0.5
min_angular_vel: -0.5
axis_distance: 0.36
wheel_distance: 0.265
wheel_radius: 0.05
use_trans_vel_deadzone: true
trans_vel_deadzone: 0.1
split_ignore_length: 0.1
cmd_vel_avg: 1
odom_avg: 1
planner_patience: 5
hdiff_scale: 0.5
heading_points: 8
......@@ -57,6 +57,7 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
config.min_sim_time,
config.sim_granularity,
config.angular_sim_granularity,
config.use_steer_angle_cmd,
sim_period_);
double resolution = planner_util_->get_costmap()->getResolution();
......@@ -80,9 +81,12 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
// obstacle costs can vary due to scaling footprint feature
obstacle_costs_.setParams(config.max_trans_vel, config.max_scaling_factor, config.scaling_speed);
int trans_vel_samp, steer_angle_samp;
int trans_vel_samp, steer_angle_angular_vel_samp;
trans_vel_samp = config.trans_vel_samples;
steer_angle_samp = config.steer_angle_samples;
if(config.use_steer_angle_cmd)
steer_angle_angular_vel_samp = config.steer_angle_samples;
else
steer_angle_angular_vel_samp = config.angular_vel_samples;
if (trans_vel_samp <= 0)
{
......@@ -91,15 +95,18 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
config.trans_vel_samples = trans_vel_samp;
}
if (steer_angle_samp <= 0)
if (steer_angle_angular_vel_samp <= 0)
{
ROS_WARN("You've specified that you don't want any samples in the steering dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
steer_angle_samp = 1;
config.steer_angle_samples = steer_angle_samp;
ROS_WARN("You've specified that you don't want any samples in the steering/angular velocity dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
steer_angle_angular_vel_samp = 1;
if(config.use_steer_angle_cmd)
config.steer_angle_samples = steer_angle_angular_vel_samp;
else
config.angular_vel_samples = steer_angle_angular_vel_samp;
}
vsamples_[0] = trans_vel_samp;
vsamples_[1] = steer_angle_samp;
vsamples_[1] = steer_angle_angular_vel_samp;
}
AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planner_util) :
......
......@@ -82,11 +82,15 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke
limits.max_steer_vel = config.max_steer_vel;
limits.min_steer_vel = config.min_steer_vel;
limits.max_steer_acc = config.max_steer_acc;
limits.max_angular_vel = config.max_angular_vel;
limits.min_angular_vel = config.min_angular_vel;
// kinematic attributes
limits.axis_distance = config.axis_distance;
limits.wheel_distance = config.wheel_distance;
limits.wheel_radius = config.wheel_radius;
// general planner limits
limits.use_trans_vel_deadzone = config.use_trans_vel_deadzone;
limits.trans_vel_deadzone = config.trans_vel_deadzone;
limits.split_ignore_length = config.split_ignore_length;
limits.xy_goal_tolerance = config.xy_goal_tolerance;
limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
......@@ -95,6 +99,7 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke
limits.rot_stopped_vel = config.rot_stopped_vel;
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;
odom_helper_.set_average_samples(config.odom_avg);
patience_=config.planner_patience;
......@@ -437,34 +442,39 @@ 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(fabs(ackermann.steer_angle-cmd_vel.angular.z)>0.05)
if(this->use_steer_angle_cmd)
{
// IRI
ROS_WARN("Setting forward speed to 0 (%f,%f)",ackermann.steer_angle,cmd_vel.angular.z);
cmd_vel.linear.x=0.0;
// EURECAT
//ROS_WARN("Setting forward speed to 0.1 (%f,%f)",ackermann.steer_angle,cmd_vel.angular.z);
//cmd_vel.linear.x=0.1;
if(fabs(ackermann.steering_angle-cmd_vel.angular.z)>0.05)
{
ROS_WARN("Setting forward speed to 0 (%f,%f)",ackermann.steering_angle,cmd_vel.angular.z);
cmd_vel.linear.x=0.0;
}
else
{
if(this->first)
this->first=false;
if(new_segment)
new_segment=false;
}
}
else
{
*/
if(this->first) this->first=false;
if(new_segment) new_segment=false;
if(this->first)
this->first=false;
if(new_segment)
new_segment=false;
}
}
else if(fabs(cmd_vel.linear.x)<0.015)
else if(fabs(cmd_vel.linear.x)<limits.trans_vel_deadzone)
{
/*
// get the position of the goal
base_local_planner::getGoalPose(*tf_,
transformed_plan,
costmap_ros_->getGlobalFrameID(),
goal_pose);
// get the distance to the goal
double dist=base_local_planner::getGoalPositionDistance(current_pose_,goal_pose.pose.position.x,goal_pose.pose.position.x);
double dist=base_local_planner::getGoalPositionDistance(current_pose_,goal_pose.pose.position.x,goal_pose.pose.position.y);
if(dist>limits.xy_goal_tolerance && dist<2*limits.xy_goal_tolerance)
{
stucked_count++;
......@@ -490,6 +500,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
return false;
}
}
*/
}
else
{
......@@ -497,7 +508,16 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
replan_count=0;
}
if (isOk)
{
publish_global_plan(transformed_plan);
if(limits.use_trans_vel_deadzone && fabs(cmd_vel.linear.x)<limits.trans_vel_deadzone)
{
if(cmd_vel.linear.x>0.0)
cmd_vel.linear.x=limits.trans_vel_deadzone;
else
cmd_vel.linear.x=-limits.trans_vel_deadzone;
}
}
else
{
ROS_WARN_NAMED("ackermann_local_planner", "Ackermann planner failed to produce path.");
......
......@@ -248,6 +248,7 @@ void AckermannTrajectoryGenerator::set_parameters(
double min_sim_time,
double sim_granularity,
double angular_sim_granularity,
bool use_steer_angle_cmd,
double sim_period)
{
max_sim_time_ = max_sim_time;
......@@ -255,6 +256,7 @@ void AckermannTrajectoryGenerator::set_parameters(
sim_granularity_ = sim_granularity;
angular_sim_granularity_ = angular_sim_granularity;
sim_period_ = sim_period;
use_steer_angle_cmd_ = use_steer_angle_cmd;
}
/**
......@@ -607,13 +609,24 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
time+=dt;
} // end for simulation steps
if(fabs(sample_target_vel[1])>0.0)
if(this->use_steer_angle_cmd_)
{
double radius=limits_->axis_distance/tan(sample_target_vel[1]);
traj.thetav_ = sample_target_vel[0]/radius;
traj.thetav_ = sample_target_vel[1];
if(traj.thetav_>limits_->max_angular_vel)
traj.thetav_=limits_->max_angular_vel;
else if(traj.thetav_<limits_->min_angular_vel)
traj.thetav_=limits_->min_angular_vel;
}
else
traj.thetav_ = 0.0;
{
if(fabs(sample_target_vel[1])>0.0)
{
double radius=limits_->axis_distance/tan(sample_target_vel[1]);
traj.thetav_ = sample_target_vel[0]/radius;
}
else
traj.thetav_ = 0.0;
}
return num_steps > 0; // true if trajectory has at least one point
}
......
......@@ -85,6 +85,7 @@ double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj
diff=fabs(diff-3.14159);
heading_diff+=diff;
}
heading_diff/=this->num_points;
return heading_diff;
}
......
Markdown is supported
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