move_base代码阅读

最新推荐文章于 2024-04-02 12:21:48 发布

Smile Hun

最新推荐文章于 2024-04-02 12:21:48 发布

阅读量255

点赞数

文章标签：机器人自动驾驶

本文链接：https://blog.csdn.net/SmileJayNew/article/details/133949692

版权

前置知识

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

costmap的分层机制

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

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

作用

完成MoveBase的构造

逻辑

参数列表

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) {

创建 move_base 的动作服务器 “move_base”
设置recovery_trigger为 PLANNING_R
参数配置
创建三个plan vector
1. planner_plan_
2. latest_plan_
3. controller_plan_
创建move_base_simple句柄, 用于发送导航目标点

注: move_base_simple/goal的类型为<geometry_msgs::PoseStamped>, 且为话题通信, 虽然确实调用的是move_base这个包, 但是无法收到move_base这个动作服务器返回的消息
将global_costmap传给planner_costmap_ros_ 创建全局路径规划的实例
将local_costmap传给controller_costmap_ros_ 创建局部路径规划的实例
开始接收costmap
创建make_plan以及clear_costmap的服务
如果costmap被关闭, 则停止move_base节点
加载恢复策略(recovery_behaviors)
设置state_ 为 PLANNING
设置recovery_index_= 0 根据官方描述, we’ll start executing recovery behaviors at the beginning of our list, 在规划开始时就会执行恢复策略
启动move_base 动作服务器
创建reconfig服务器

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

作用

动态配置参数

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

作用

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

逻辑

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

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

作用

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

逻辑

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

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

作用

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

逻辑

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

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

作用

重置global_costmap以及local_costmap层

逻辑

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

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

作用

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

逻辑

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

MoveBase::~MoveBase()

作用

析构, 没啥说的

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

作用

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

逻辑

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

void MoveBase::publishZeroVelocity()

作用

发布零速度

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

作用

检查四元数是否合法

逻辑

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

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()

作用

起规划线程

逻辑

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

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

作用

返回两个点之间的距离

逻辑

hypot()

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

作用

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

逻辑

获取当前位置 global_pose
创建用于发布到feedback的常量current_position = global_pose
发布feedback
判断是否在振荡(oscillation), 这里的oscillation_pose_没有初始化!!! 而且没有说如果在振荡会怎么处理
1. 将oscillation_pose_设置为当前位置
2. 如果当前恢复策略为 OSCILLATION_R则重置recovery_index为0
判断local_costmap的时间戳是否与当前时间接近
1. 如果不是, 则发布0速度
2. ROS_WARN 传感器数据过时
根据new_global_plan_ (在planThread中更新)判断是否收到新的目标点

在这里有一个线程指针的交换? 不太懂, 再学学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_);

调用全局路径规划器的setPlan
1. 如果没有调用成功, 则setAborted
重置recovery_index_为0
判断当前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的回调函数, 负责执行路径规划

逻辑

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