From 1bee86e41a7c24a9c1b7d9e8e958e24f886342fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20R=C3=B6smann?= Date: Thu, 28 May 2020 16:05:46 +0200 Subject: [PATCH] Changed obstacle parameters cutoff_factor and force_inclusion_factor to cutoff_dist and force_inclusion_dist --- mpc_local_planner/cfg/test_mpc_optim_node.yaml | 4 ++-- .../optimal_control/stage_inequality_se2.h | 14 +++++++------- mpc_local_planner/src/controller.cpp | 10 +++++----- .../src/optimal_control/stage_inequality_se2.cpp | 4 ++-- .../cfg/carlike/mpc_local_planner_params.yaml | 6 +++--- .../mpc_local_planner_params_minimum_time.yaml | 4 ++-- .../mpc_local_planner_params_quadratic_form.yaml | 4 ++-- 7 files changed, 23 insertions(+), 23 deletions(-) diff --git a/mpc_local_planner/cfg/test_mpc_optim_node.yaml b/mpc_local_planner/cfg/test_mpc_optim_node.yaml index eccb51e..d600e34 100644 --- a/mpc_local_planner/cfg/test_mpc_optim_node.yaml +++ b/mpc_local_planner/cfg/test_mpc_optim_node.yaml @@ -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: diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h index e37744e..7a6efae 100644 --- a/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h @@ -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; diff --git a/mpc_local_planner/src/controller.cpp b/mpc_local_planner/src/controller.cpp index ce489db..3deec63 100644 --- a/mpc_local_planner/src/controller.cpp +++ b/mpc_local_planner/src/controller.cpp @@ -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 diff --git a/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp b/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp index dd9fb11..a95adf3 100644 --- a/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp +++ b/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp @@ -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 diff --git a/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml b/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml index 656e4a1..a0469db 100644 --- a/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml +++ b/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml @@ -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 diff --git a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml index 0ff945e..099a758 100644 --- a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml +++ b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml @@ -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 diff --git a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml index 4b58ba5..9445f8a 100644 --- a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml +++ b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml @@ -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