Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Bugfix amrfm 2574 teb speed limits #33

Merged
merged 2 commits into from
Jul 11, 2022
Merged

Bugfix amrfm 2574 teb speed limits #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c810c90
Fix speed limits
Jul 11, 2022
8a8a011
fix parmas
Jul 11, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions teb_local_planner/include/teb_local_planner/teb_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -275,9 +275,9 @@ class TebConfig
robot.max_vel_y = 0.0;
robot.max_vel_theta = 0.3;
robot.base_max_vel_x = robot.max_vel_x;
robot.base_max_vel_x_backwards = robot.base_max_vel_x_backwards;
robot.base_max_vel_y = robot.base_max_vel_y;
robot.base_max_vel_theta = robot.base_max_vel_theta;
robot.base_max_vel_x_backwards = robot.max_vel_x_backwards;
robot.base_max_vel_y = robot.max_vel_y;
robot.base_max_vel_theta = robot.max_vel_theta;
robot.acc_lim_x = 0.5;
robot.acc_lim_y = 0.5;
robot.acc_lim_theta = 0.5;
Expand Down
4 changes: 4 additions & 0 deletions teb_local_planner/src/teb_config.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -201,9 +201,13 @@ void TebConfig::loadRosParamFromNodeHandle(const nav2_util::LifecycleNode::Share

// Robot
nh->get_parameter_or(name + "." + "max_vel_x", robot.max_vel_x, robot.max_vel_x);
nh->get_parameter_or(name + "." + "max_vel_x", robot.base_max_vel_x, robot.base_max_vel_x);
nh->get_parameter_or(name + "." + "max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
nh->get_parameter_or(name + "." + "max_vel_x_backwards", robot.base_max_vel_x_backwards, robot.base_max_vel_x_backwards);
nh->get_parameter_or(name + "." + "max_vel_y", robot.max_vel_y, robot.max_vel_y);
nh->get_parameter_or(name + "." + "max_vel_y", robot.base_max_vel_y, robot.base_max_vel_y);
nh->get_parameter_or(name + "." + "max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
nh->get_parameter_or(name + "." + "max_vel_theta", robot.base_max_vel_theta, robot.base_max_vel_theta);
nh->get_parameter_or(name + "." + "acc_lim_x", robot.acc_lim_x, robot.acc_lim_x);
nh->get_parameter_or(name + "." + "acc_lim_y", robot.acc_lim_y, robot.acc_lim_y);
nh->get_parameter_or(name + "." + "acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta);
Expand Down
18 changes: 9 additions & 9 deletions teb_local_planner/src/teb_local_planner_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1061,16 +1061,16 @@ void TebLocalPlannerROS::setSpeedLimit(
if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
// Restore default value
cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x;
cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards;
cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y;
cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta;
cfg_->robot.max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards;
cfg_->robot.max_vel_y = cfg_->robot.base_max_vel_y;
cfg_->robot.max_vel_theta = cfg_->robot.base_max_vel_theta;
} else {
if (percentage) {
// Speed limit is expressed in % from maximum speed of robot
cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * speed_limit / 100.0;
cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * speed_limit / 100.0;
cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * speed_limit / 100.0;
cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * speed_limit / 100.0;
cfg_->robot.max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * speed_limit / 100.0;
cfg_->robot.max_vel_y = cfg_->robot.base_max_vel_y * speed_limit / 100.0;
cfg_->robot.max_vel_theta = cfg_->robot.base_max_vel_theta * speed_limit / 100.0;
} else {
// Speed limit is expressed in absolute value
double max_speed_xy = std::max(
Expand All @@ -1083,9 +1083,9 @@ void TebLocalPlannerROS::setSpeedLimit(
// G. Doisy: not sure if that's applicable to base_max_vel_x_backwards.
const double ratio = speed_limit / max_speed_xy;
cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * ratio;
cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * ratio;
cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * ratio;
cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * ratio;
cfg_->robot.max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * ratio;
cfg_->robot.max_vel_y = cfg_->robot.base_max_vel_y * ratio;
cfg_->robot.max_vel_theta = cfg_->robot.base_max_vel_theta * ratio;
}
}
}
Expand Down