1 | // 如果优化结果违反约束(由于是软约束，所以是可能的)， Saturate 速度 |
saturateVelocity
若规划出来的速度超过限制速度，则令其等于最大限制速度1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23// Limit translational velocity for forward driving
if (vx > max_vel_x)
vx = max_vel_x;
// limit strafing velocity
if (vy > max_vel_y)
vy = max_vel_y;
else if (vy < -max_vel_y)
vy = -max_vel_y;
if(vx>= -0.12 && vx< 0)
{
vx = -0.12;
}
else if(vx< -0.12)
{
planner_->clearPlanner();
}
// Limit angular velocity
if (omega > max_vel_theta)
omega = max_vel_theta;
else if (omega < -max_vel_theta)
omega = -max_vel_theta;
剩余部分也很简单1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17// 对汽车机器人，convert rot-vel to steering angle if desired
// The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
// and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself.
if (cfg_.robot.cmd_angle_instead_rotvel)
{ // 目前是false，省略
}
// a feasible solution should be found, reset counter
no_infeasible_plans_ = 0;
// 保存上一个速度命令 (for recovery analysis etc.)
last_cmd_ = cmd_vel;
// Now visualize everything
planner_->visualize();
visualization_->publishObstacles(obstacles_);
visualization_->publishViaPoints(via_points_);
visualization_->publishGlobalPlan(global_plan_);
return true; // 函数 computeVelocityCommands 结束
可视化部分，见下一篇