关于ROS Navigation小车失控的问题
2022-12-22 # 实用教程 #Autonomous Driving #C/C++ #Motion Planning #ROS

问题描述

当你在rivz看到小车规划出来的全局路径是没问题的，但小车却会跑歪，那就说明小车失控了。

这里失控的原因大概率是控制命令下发不及时，但控制频率丢失大多会跟local_costmap的更新频率丢失一起出现，在小车程序运行的终端会打印类似如下的警告：

[Warn]： Control loop missed its desired rate of xxxHz… the loop actually took xx seconds

[Warn]： Map update loop missed its desired rate of xxxHz… the loop actually took xx seconds

原因追溯

该现象主要是小车周边代价地图更新太慢导致的，而地图更新慢，跟生成代价地图的参数以及小车自身的算力有关。> 直接看解决措施 <
根据终端报的警告，我们可以把问题追溯到move_base.cpp文件下的executeCb()函数和costmap_2d_ros.cpp文件下的mapUpdateLoop()函数。

1.costmap

可以先看下costmap报警告的代码，跟我们遇到的问题关系不大的代码，我都用…把他们省略了，这样方便我们认识代码和排查原因。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void Costmap2DROS::mapUpdateLoop(double frequency)
{
  ...
  while (nh.ok() && !map_update_thread_shutdown_)
  {
    ...
    
    updateMap();
    ...
    r.sleep();
    // make sure to sleep for the remainder of our cycle time
    if (r.cycleTime() > ros::Duration(1 / frequency))
      ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
               r.cycleTime().toSec());
  }
}

很明显mapUpdateLoop()这个函数一直在调用updateMap()函数来更新局部地图，然后检测更新超时就报警。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
void Costmap2DROS::updateMap()
{
  if (!stop_updates_)
  {
    // get global pose
    geometry_msgs::PoseStamped pose;
    if (getRobotPose (pose))
    {
      double x = pose.pose.position.x,
             y = pose.pose.position.y,
             yaw = tf2::getYaw(pose.pose.orientation);
      layered_costmap_->updateMap(x, y, yaw);
      geometry_msgs::PolygonStamped footprint;
      footprint.header.frame_id = global_frame_;
      footprint.header.stamp = ros::Time::now();
      transformFootprint(x, y, yaw, padded_footprint_, footprint);
      footprint_pub_.publish(footprint);
      initialized_ = true;
    }
  }
}

然后updateMap()函数主要获取小车位姿信息，以这个位姿为中心更新地图并添加小车的footprint，最后发布出去。
其实分析到这，已经能预料到这是局部地图参数欠优化的问题了。

2.move_base

不过我们还是再来看下move_base吧，首先是executeCb()函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal){
    ...
    while(n.ok())
    {
      ...
      if(as_->isPreemptRequested()){...
      ...
      //the real work on pursuing a goal is done here
      bool done = executeCycle(goal, global_plan);
      //if we're done, then we'll return from execute
      if(done)
        return;
      ...
      r.sleep();
      //make sure to sleep for the remainder of our cycle time
      if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
        ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
    }
    ...
  }

可以很方便地看出executeCb()函数一直在检测是否有新的抢占试请求（即新的导航点Nav Goal）,有的话他会调用plan线程为新目标规划全局路径，其次他会一直执行executeCycle()函数直到done为真，即到达目标点才会退出，没到终点的话借助sleep()方法来满足我们设定的控制频率，最后是我们报警告的地方，当以上代码的执行时间大于设定的控制时间时就会警告。
那是哪段代码执行太久了呢？新目标在小车运行中我们是没有添加的，而且即使添加了目标，也只是调用一个plan线程在外面并行地规划全局路径，这里的代码会继续执行下去，那就到executeCycle()这个函数了，代码如下，同样，无关的代码被我省略了。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
    ...
    //we need to be able to publish velocity commands
    geometry_msgs::Twist cmd_vel;
    ...
    //the move_base state machine, handles the control logic for navigation
    switch(state_){
      //if we are in a planning state, then we'll attempt to make a plan
      case PLANNING:
        ...
      //if we're controlling, we'll attempt to find valid velocity commands
      case CONTROLLING:
        ...
        if(tc_->computeVelocityCommands(cmd_vel)){...
      //we'll try to clear out space with any user-provided recovery behaviors
      case CLEARING:
        ...
    ...
  }

这段程序多数时间运行在switch代码块里，它主要帮我们计算并发布小车的控制命令。不出意外的话，小车一般运行在CONTROLLING状态，所以问题主要出在CONTROLLING case下的这句代码。

tc_->computeVelocityCommands(cmd_vel)

有关tc_这个指针的定义写在move_base的include目录下的move_base.h文件里

boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;

创建tc_指针实例的代码如下：

tc_ = blp_loader_.createInstance(local_planner);

显然它是用来存放LocalPlanner的入口地址，来调用LocalPlanner方法的。LocalPlanner有多种选择，但逻辑应该都差不多，以我们用的teb_local_planner为例来看下问题。

1
2
3
4
5
6
bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
  ...
  uint32_t outcome = computeVelocityCommands(dummy_pose, dummy_velocity, cmd_vel_stamped, dummy_message);
  ...
}

进到computeVelocityCommands()函数里可以发现，他继续调用了传入四个参数的computeVelocityCommands()函数来完成我们需要的控制指令cmd_vel的计算。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose,const geometry_msgs::TwistStamped& velocity,geometry_msgs::TwistStamped &cmd_vel,std::string &message)
{
  ...
  // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin
  if (costmap_converter_)
    updateObstacleContainerWithCostmapConverter();
  else
    updateObstacleContainerWithCostmap();
  
  // also consider custom obstacles (must be called after other updates, since the container is not cleared)
  updateObstacleContainerWithCustomObstacles();
  
  // Now perform the actual planning
  bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
  ...
  // Get the velocity command for this sampling interval
  if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.trajectory.control_look_ahead_poses)){...
  ...
}

这个函数做的工作挺多，但许多工作计算量其实并不大，如检查插件初始化、是否到终点、获取机器人位姿、坐标转换等等，这些都不是引发问题的关键，所以我就把他们都省略了。
现在可以比较清晰地了解这个函数的作用，先更新局部代价地图上的障碍物信息，在此基础上规划出一条局部路径，然后提取速度指令。
所以很明显问题出在局部代价地图的更新上。一方面遍历障碍物对它们每个做处理，本身工作量就不少；另一方面他还涉及costmap的获取，地图给到不及时他就没法往下规划了。

解决措施

最经济的方式，在/Path/To/Your/config目录下，将local_costmap_params.yaml这个文件里的width和height这两个参数调小，resolution这个参数调大即可。

上一页
2022-12-22 13:55:43 # 实用教程 #Autonomous Driving #C/C++ #Motion Planning #ROS
下一页