12345qiupeng
/
mpc_local_planner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Changed obstacle parameters cutoff_factor and force_inclusion_factor to cutoff_dist and force_inclusion_dist

master
Christoph Rösmann 2020-05-28 16:05:46 +02:00
parent 4133cbbc26
commit 1bee86e41a
7 changed files with 23 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
mpc_local_planner/cfg/test_mpc_optim_node.yaml
View File

@ -34,8 +34,8 @@ footprint_model:
collision_avoidance:
min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_factor: 1.5
cutoff_factor: 5
force_inclusion_dist: 0.5
cutoff_dist: 2.5
## Planning grid
grid:

14
mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h
View File

@ -91,10 +91,10 @@ class StageInequalitySE2 : public corbo::StageInequalityConstraint
//! Set minimum distance allowed
void setMinimumDistance(double min_dist) { _min_obstacle_dist = min_dist; }
//! Set parameters for prior filtering of obstacles
void setObstacleFilterParameters(double force_inclusion_factor, double cutoff_factor)
void setObstacleFilterParameters(double force_inclusion_dist, double cutoff_dist)
{
_obstacle_filter_force_inclusion_factor = force_inclusion_factor;
_obstacle_filter_cutoff_factor = cutoff_factor;
_obstacle_filter_force_inclusion_dist = force_inclusion_dist;
_obstacle_filter_cutoff_dist = cutoff_dist;
}
//! Set to true to enable dynamic obstacle (constant-velocity prediction)
void setEnableDynamicObstacles(bool enable_dyn_obst) { _enable_dynamic_obstacles = enable_dyn_obst; }
@ -115,10 +115,10 @@ class StageInequalitySE2 : public corbo::StageInequalityConstraint
int _num_du_lb_finite = 0;
int _num_du_ub_finite = 0;
double _min_obstacle_dist = 0.1;
double _obstacle_filter_force_inclusion_factor = 1.5;
double _obstacle_filter_cutoff_factor = 5;
bool _enable_dynamic_obstacles = false;
double _min_obstacle_dist = 0.1;
double _obstacle_filter_force_inclusion_dist = 1.5;
double _obstacle_filter_cutoff_dist = 5;
bool _enable_dynamic_obstacles = false;
Eigen::VectorXd _du_lb;
Eigen::VectorXd _du_ub;

10
mpc_local_planner/src/controller.cpp
View File

@ -722,11 +722,11 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::
nh.param("collision_avoidance/enable_dynamic_obstacles", enable_dynamic_obstacles, enable_dynamic_obstacles);
_inequality_constraint->setEnableDynamicObstacles(enable_dynamic_obstacles);
double force_inclusion_factor = 1.5;
nh.param("collision_avoidance/force_inclusion_factor", force_inclusion_factor, force_inclusion_factor);
double cutoff_factor = 5;
nh.param("collision_avoidance/cutoff_factor", cutoff_factor, cutoff_factor);
_inequality_constraint->setObstacleFilterParameters(force_inclusion_factor, cutoff_factor);
double force_inclusion_dist = 0.5;
nh.param("collision_avoidance/force_inclusion_dist", force_inclusion_dist, force_inclusion_dist);
double cutoff_dist = 2;
nh.param("collision_avoidance/cutoff_dist", cutoff_dist, cutoff_dist);
_inequality_constraint->setObstacleFilterParameters(force_inclusion_dist, cutoff_dist);
// configure control deviation bounds

4
mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp
View File

@ -109,13 +109,13 @@ bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryI
double dist = _robot_model->calculateDistance(pose, obst.get());
// force considering obstacle if really close to the current pose
if (dist < _min_obstacle_dist * _obstacle_filter_force_inclusion_factor)
if (dist < _obstacle_filter_force_inclusion_dist)
{
_relevant_obstacles[k].push_back(obst);
continue;
}
// cut-off distance
if (dist > _min_obstacle_dist * _obstacle_filter_cutoff_factor) continue;
if (dist > _obstacle_filter_cutoff_dist) continue;
// determine side (left or right) and assign obstacle if closer than the previous one
if (cross2d(pose_orient, obst->getCentroid()) > 0) // left

6
mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml
View File

@ -31,10 +31,10 @@ MpcLocalPlannerROS:
## Collision avoidance
collision_avoidance:
min_obstacle_dist: 0.25 # Note, this parameter must be chosen w.r.t. the footprint_model
min_obstacle_dist: 0.27 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_factor: 1.5
cutoff_factor: 5
force_inclusion_dist: 0.5
cutoff_dist: 2.5
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0

4
mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml
View File

@ -22,8 +22,8 @@ MpcLocalPlannerROS:
collision_avoidance:
min_obstacle_dist: 0.2 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_factor: 1.5
cutoff_factor: 5
force_inclusion_dist: 0.5
cutoff_dist: 2.5
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.5

4
mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml
View File

@ -22,8 +22,8 @@ MpcLocalPlannerROS:
collision_avoidance:
min_obstacle_dist: 0.2 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_factor: 1.5
cutoff_factor: 5
force_inclusion_dist: 0.5
cutoff_dist: 2.5
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.5