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

Added check for obstacle pointer validity in StageInequalitySE2

master
Christoph Rösmann 2020-05-27 10:24:34 +02:00
parent 81f60e5439
commit 4133cbbc26
1 changed files with 41 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -35,12 +35,14 @@ int StageInequalitySE2::getNonIntegralControlDeviationTermDimension(int k) const
int StageInequalitySE2::getNonIntegralStateTermDimension(int k) const
{
if (!_obstacles) return 0;
assert(k < _relevant_obstacles.size());
return (int)_relevant_obstacles[k].size();
}
int StageInequalitySE2::getNonIntegralStateDtTermDimension(int k) const
{
if (!_obstacles) return 0;
assert(k < _relevant_dyn_obstacles.size());
return (int)_relevant_dyn_obstacles[k].size();
}
@ -50,8 +52,8 @@ bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryI
corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector<double>& /*dts*/,
const corbo::DiscretizationGridInterface* grid)
{
assert(_obstacles);
assert(_robot_model);
PRINT_WARNING_COND_ONCE(!_obstacles, "StageInequalitySE2 requires a valid obstacle ptr which is not null (ignoring obstacle avoidance).");
PRINT_WARNING_COND_ONCE(!_robot_model, "StageInequalitySE2 requires a valid robot model ptr which is not null (ignoring obstacle avoidance).");
// Setup obstacle avoidance
@ -89,6 +91,8 @@ bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryI
_relevant_dyn_obstacles[k].clear();
// iterate obstacles
if (_obstacles && _robot_model)
{
for (const ObstaclePtr& obst : *_obstacles)
{
// check for dynamic obstacle
@ -131,6 +135,7 @@ bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryI
}
}
}
}
// add left and right obstacle
if (left_obstacle) _relevant_obstacles[k].push_back(left_obstacle);
@ -158,7 +163,6 @@ bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryI
void StageInequalitySE2::computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(_obstacles);
assert(k < _relevant_obstacles.size());
assert(cost.size() == _relevant_obstacles[k].size());
@ -173,7 +177,6 @@ void StageInequalitySE2::computeNonIntegralStateTerm(int k, const Eigen::Ref<con
void StageInequalitySE2::computeNonIntegralStateDtTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, double dt_k,
Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(_obstacles);
assert(k < _relevant_dyn_obstacles.size());
assert(cost.size() == _relevant_dyn_obstacles[k].size());