move_base代码阅读

前置知识

tf2::Transform::getIdentity() 是ROS (Robot Operating System) 中的一个函数，通常用于机器人控制和机器人操作中的坐标变换操作。这个函数的作用是创建一个表示"单位"或"恒等"变换的 tf2::Transform 对象。

costmap的分层机制

1. Static Layer：静态地图层, 来自map.pgm
2. Obstacle Layer：障碍物层, 来自传感器的实时感应
3. Inflation Layer：膨胀层, 根据障碍物层的信息, 对接收到的障碍物进行膨胀
   1. 膨胀层只能影响costmap_ed::INSCRIBED_INFLATED_OBSTACLE
   2. costmap_2d::LETHAL_OBSTACLE是由机器人的实际尺寸决定的, 即costmap_common_params.yaml中填写的机器人footprint的大小
4. 还有其他开发的插件, 比如可以延时消失深度点云的spatio_temporal_voxel_layer

MoveBase::MoveBase(tf2_ros::Buffer& tf)

作用

完成MoveBase的构造

逻辑

1. 参数列表

   tf_(tf),
   as_(NULL),
   planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
   bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
   blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
   recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
   planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
   runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
   

2. 创建 move_base 的动作服务器 “move_base”

3. 设置recovery_trigger为 PLANNING_R

4. 参数配置

5. 创建三个plan vector

   1. planner_plan_
   2. latest_plan_
   3. controller_plan_

6. 创建move_base_simple句柄, 用于发送导航目标点

   注: move_base_simple/goal的类型为, 且为话题通信, 虽然确实调用的是move_base这个包, 但是无法收到move_base这个动作服务器返回的消息

7. 将global_costmap传给planner_costmap_ros_ 创建全局路径规划的实例

8. 将local_costmap传给controller_costmap_ros_ 创建局部路径规划的实例

9. 开始接收costmap

10. 创建make_plan以及clear_costmap的服务

11. 如果costmap被关闭, 则停止move_base节点

12. 加载恢复策略(recovery_behaviors)

13. 设置state_ 为 PLANNING

14. 设置recovery_index_= 0 根据官方描述, we’ll start executing recovery behaviors at the beginning of our list, 在规划开始时就会执行恢复策略

15. 启动move_base 动作服务器

16. 创建reconfig服务器

void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level)

作用

动态配置参数

void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal)

作用

调用move_base动作服务器, 移动机器人

逻辑

1. 接收通过 /move_base_simple/goal传过来的参数
2. 将其转换为MoveBaseActionGoal
3. 转发给move_base/goal

bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap)

作用

获取机器人在代价地图的坐标

逻辑

1. 创建空tf, global_pose以及robot_pose
2. 设置robot_pose的frame_id为robot_base_frame_
3. 将robot_pose转换到cost_map坐标系下
4. 判断两个坐标系的时间是否同步(在某个容忍范围内)

void MoveBase::clearCostmapWindows(double size_x, double size_y)

作用

将机器人一定范围内的costmap置零

逻辑

1. 获取机器人在global_costmap坐标系下的位置
2. 根据size_z和size_y配置清除窗口clear_poly
3. 将clear_poly中的代价值置为0, 也就是costmap_2d::FREE_SPACE
4. 同样步骤, 将local_costmap中的代价值置零

bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)

作用

重置global_costmap以及local_costmap层

逻辑

1. 与clearCostmapWindows不同
2. 该服务负责直接重置两个costmap

bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)

作用

规划一条全局路径, 但是机器人不移动

逻辑

1. 创建起始空坐标 start
2. 如果在req中没有包含起始坐标系名称(可以认为没有设置起始点), 则将start设置为机器人当前位置
3. 否则start为req中的start
4. 如果设置了make_plan_clear_costmap_这个属性(默认为true), 在开始规划前调用clearCostmapWindows, 清空2 * clearing_radius_(可在参数服务器中设置)的值
5. 尝试调用planner_(全局路径规划器)的makePlan方法, 尝试寻找一条路径, 如果有, 则存入global_plan
6. 不管有没有通过planner_找到可行路径, 都会执行下面的代码
7. 设定p为goal的复制
8. 获取global_costmap的分辨率, 并根据 3 * 分辨率(search_increment)对搜寻范围进行自增
9. 如果在req中设置了tolerance, 即允许离目标点的最远距离, 或者tolerance小于前面配置的自增量search_increment, 则将search_increment设置为tolerance
10. 第一个循环(for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment))：这是外层的循环，用于逐渐增加最大偏移(max_offset)。循环的条件包括**max_offset小于等于容差且found_legal**为false。这个循循环逐渐扩大搜索范围。
11. 第二个循环(for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment))：在外层循环中，这个嵌套循环用于逐渐增加y方向的偏移。
12. 第三个循环(for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment))：在外层循环中，这个嵌套循环用于逐渐增加x方向的偏移。
13. 第四个嵌套循环(for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0))：在y方向上搜索的嵌套循环，允许搜索目标点的上下两个方向。
14. 第五个嵌套循环(for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0))：在x方向上搜索的嵌套循环，允许搜索目标点的左右两个方向。
15. 以上几个循环, 我没细看, gpt说的, 大概没问题.

MoveBase::~MoveBase()

作用

析构, 没啥说的

bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector& plan)

作用

真正在导航中用到的路径规划

逻辑

1. 通过getRobotPose获取机器人当前位置
2. 调用全局路径规划器的makePlan

void MoveBase::publishZeroVelocity()

作用

发布零速度

bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q)

作用

检查四元数是否合法

逻辑

1. 判断q.x, q.y, q.z, q.w是否包含NaN或者无穷大
2. 判断四元数长度(x^2+y2+z^2+w2)是否近似0
3. 归一化四元数, 检查是否能正确转换垂直向量 通过(0,0,1)这个垂直向量通过四元数进行旋转, 检查旋转后的向量是否接近1
4. 以上条件全满足则判断该四元数合法

geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg)

作用

将收到的goal_pose_msg转换到global_costmap坐标系下

逻辑

没啥说的….

void MoveBase::wakePlanner(const ros::TimerEvent& event)

作用

唤醒规划器? 不太明白, C++太差了

逻辑(GPT说的,我看不懂)

planner_cond_.notify_one() 是一个C++中用于线程同步的函数调用。它用于通知一个等待中的线程，告诉它有相关的条件已经发生，以便该线程可以醒来并执行相应的操作。以下是一些解释：

• planner_cond_ 是一个条件变量（condition variable）对象，通常用于线程间的协调和同步。
• notify_one() 是条件变量对象的成员函数，用于通知等待在该条件变量上的一个线程。

当调用 planner_cond_.notify_one() 时，它将唤醒等待在 planner_cond_ 上的一个线程。这个线程将从等待状态转换为可运行状态，并开始执行它的任务。

通常，notify_one() 被用于一种生产者-消费者模式，其中生产者线程生成数据或事件，并通知一个或多个消费者线程来处理这些数据或事件。这有助于实现多线程协作，确保线程在合适的时机等待或唤醒。

void MoveBase::planThread()

作用

起规划线程

逻辑

1. 调用makePlan尝试规划全局路径
2. 成功, new_global_plan_置为true
3. 规划失败则进入恢复模式
4. 设置定时器, 定时调用wakePlanner用来唤醒move_base

double MoveBase::distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2)

作用

返回两个点之间的距离

逻辑

hypot()

bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal)

作用

实际执行路径规划与导航的函数

逻辑

1. 获取当前位置 global_pose

2. 创建用于发布到feedback的常量current_position = global_pose

3. 发布feedback

4. 判断是否在振荡(oscillation), 这里的oscillation_pose_没有初始化!!! 而且没有说如果在振荡会怎么处理

   1. 将oscillation_pose_设置为当前位置
   2. 如果当前恢复策略为 OSCILLATION_R则重置recovery_index为0

5. 判断local_costmap的时间戳是否与当前时间接近

   1. 如果不是, 则发布0速度
   2. ROS_WARN 传感器数据过时

6. 根据new_global_plan_ (在planThread中更新)判断是否收到新的目标点

7. 在这里有一个线程指针的交换? 不太懂, 再学学c++再回来看

   //do a pointer swap under mutex
   std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
   
   boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
   

8. 调用全局路径规划器的setPlan

   1. 如果没有调用成功, 则setAborted

9. 重置recovery_index_为0

10. 判断当前planner的状态

    1. PLANNING(在goal的回调函数

       executeCb

       中被设置)

       1. 锁定进程

          boost::recursive_mutex::scoped_lock lock(planner_mutex_);
                    runPlanner_ = true;
                    planner_cond_.notify_one();
          

       2. runPlanner_ 置为true

    2. CONTROLLING (在planThread中被设置)

       1. 调用局部路径规划器的

          isGoalReached
          

          判断是否到达目标点

          1. 到达后重置move_base状态
          2. 关闭planner线程
          3. 设置move_base的result为succeeded

       2. 判断是否在振荡, 如果是采取恢复策略(将planner的状态置为CLEARING)

       3. 调用局部路径规划器的computeVelocityCommands发布速度

       4. 如果没有计算出速度, 多次尝试

       5. 多次失败后将速度置零, 报警

    3. CLEARING

       1. 尝试多个恢复策略
       2. 无法恢复, ERROR

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)

作用

move_base/goal的回调函数, 负责执行路径规划

逻辑

1. 判断move_base_goal中的四元数是否合法, 不合法直接返回, 告警
2. 通过调用goalToGlobalFrame将move_base_goal转化到global_costmap坐标系下
3. 发布零速度
4. 将runPlanner_ 置为true
5. 发布当前收到的目标点
6. 判断move_base是否收到新的消息, 即是否被打断(Prrempt)
   1. 如果被打断, 则判断新目标点是否合法
   2. 合法则将之前的目标点goal的点位更新为new_goal的点位
   3. 转换到global_costmap下, 并发布新点的坐标
   4. 如果被打断, 且该点不合法
   5. 则重置move_base的状态, 并中断move_base
7. 判断目标点的frame_id是否是global_costmap的id
   1. 如果不是, 则将其转换到global_costmap坐标系下
   2. 重置move_base的状态标签(recovery_index_=0和state_ = PLANNING)
   3. 重置定时器
8. 调用executeCycle开始导航
9. 判断有没有成功, 成功直接return
10. 判断是否因其他因素影响, 但是成功到达了目的地(目测没什么用)