FTC局部路径规划代码分析

2023-10-18 16:44
文章标签 分析 代码 规划 路径 局部 ftc

本文主要是介绍FTC局部路径规划代码分析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

前置知识:

costmap_2d::Costmap2DROS costmap;

costmap_2d::Costmap2DROS 是一个ROS包中提供的用于处理2D成本地图的类。它是一个高级的接口，通常用于与ROS导航栈中的导航规划器和本地路径跟踪器等模块进行集成。

costmap 是一个指向 Costmap2DROS 对象的指针。通常，通过这个指针，你可以访问与导航和路径跟踪相关的成本地图信息，以及执行一些与导航相关的操作，如查询障碍物信息、获取机器人的当前全局位置等。这个指针指向了整个ROS导航栈中的成本地图管理器。

costmap_2d::Costmap2D costmap_map_;

costmap_2d::Costmap2D 是一个用于表示二维成本地图的类，通常用于底层的地图管理和处理。

costmap_map_ 是一个指向 Costmap2D 对象的指针。这个指针通常用于直接访问成本地图的数据，如地图的大小、分辨率、栅格信息以及每个栅格的成本值等。这个指针通常用于执行与成本地图的低级操作，如路径规划和避障算法。

costmap->getOrientedFootprint(std::cector<geometry_msgs::Point>& oriented_footprint)

构建机器人在当前姿态下的足迹，结果将存储在参数 oriented_footprint 中, 这个函数可以返回机器人当前位置的足迹, 获取的是一个机器人在世界坐标系下的坐标, 通常用于碰撞检测或生成局部代价地图

costmap->getRobotFootprint()

获取机器人的轮廓, 表现为一系列std::vector<geometry_msgs::Point>的形式

costmap->getRobotFootprintPolygon()

获取机器人的轮廓, 表现为一个凸多边形, geometry_msgs::Polygon 类型

initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)

参数:

name

tf

costmap_ros 一个ROS的上层接口

发布者:

global_plan_pub : 将global_planner中发过来的路径不经过任何处理发布到”/ftc_local_planner/global_plan”

global_point_pub:

obstacle_marker_pub

setPlan(const std::vector<geometry_msgs::PoseStamped> &plan)

参数:

plan 包含位姿信息的向量, 从起始点指向结束点

逻辑:

  1. 定义当前状态 PRE_ROTATE
  2. 复制plan到global_plan
  3. 如果规划中大于两个点(路径规划存在), 则将倒数第二个点的朝向改为倒数第三个点的朝向 global_plan[global_plan.size() - 2].pose.orientation = global_plan[global_plan.size() - 3].pose.orientation;
  4. 创建导航路径 nav_msgs::Path path;
  5. 将收到的路径plan原封不动放到path.poses中
  6. 发布path到”/ftc_local_planner/global_plan”

distanceLookahead() #未知

double FTCPlannerROS::distanceLookahead(){if (global_plan.size() < 2){return 0;}Eigen::Quaternion<double> current_rot(current_control_point.linear());Eigen::Affine3d last_straight_point = current_control_point;for (uint32_t i = current_index + 1; i < global_plan.size(); i++){tf2::fromMsg(global_plan[i].pose, last_straight_point);// check, if direction is the same. if so, we add the distanceEigen::Quaternion<double> rot2(last_straight_point.linear());if (abs(rot2.angularDistance(current_rot)) > config.speed_fast_threshold_angle * (M_PI / 180.0)){break;}}return (last_straight_point.translation() - current_control_point.translation()).norm();}

void FTCPlannerROS::update_control_point(double dt)

作用:

根据当前状态确定执行内容

状态: PRE_ROTATE, FOLLOWING, POST_ROTATE, WAITTING_FOR_GOAL_APPROACH, FINISHED

参数:

dt: 当前时间 - 上次执行的时间

逻辑: current_control_point: map坐标系下的点

local_control_point: base_link坐标系下的点

  1. 判断当前执行状态

    1. PRE_ROTATE
      1. 将global_plan[0].pose存入current_control_point
    2. FOLLOWING
      1. 计算前视距离 straight_dist=distanceLookahead()
      2. 如果前视距离大于满速行进的阈值, 则全速前进
      3. 若目标速度大于当前速度, 则根据配置的加速度对当前速度进行提升
      4. 如果: 需要移动的距离大于零, 同时需要转动的角度大于零且当前点不为全局路径规划的倒数第二个点, 进入循环
        1. 从全局规划中获取下两个点, currentPose以及nextPose
        2. 计算两个点的距离 pose_distance
        3. 如果两个点之间的距离小于等于0, 则警告: 跳过重复点(Skipping duplicate point in global plan)
        4. 通过current_progress(0-1之间的数), 计算剩余距离与角度
        5. 如果剩余的距离和角度可以在当前时间步内到达（remaining_distance_to_next_pose < distance_to_move*并且*remaining_angular_distance_to_next_pose < angle_to_move），则将机器人移动到下一个位置，更新**current_progress，减少distance_to_move和angle_to_move**
        6. 如果无法在当前时间步内到达下一个位置，则更新**current_progress**以确保机器人在下一个时间步内能够到达下一个位置
      5. 最后，通过插值计算机器人在当前时间步的控制点位置和方向，然后将其存储在**current_control_point**中，以供后续控制使用
    3. POST_ROTATE
      1. 将global_plan中的最后一个点保存到current_control_point中
    4. WAITTING_FOR_GOAL_APPROACH
      1. break
    5. FINISHED
      1. break

  2. 创建目标点, viz用于发布current_control_point的坐标

  3. 通过tf::doTransform(currnet_control_point, local_control_point, map_to_base)转化current_control_point的坐标到 local_control_point. 注: current_control_point为基于map的全局坐标, 而local_control_point通过下面 两行, 将map全局坐标系下的点, 转化为从base_link到map的局部变换的点

    auto map_to_base = tf_buffer->lookupTransform("base_link", "map", ros::Time(), ros::Duration(1.0));
tf2::doTransform(current_control_point, local_control_point, map_to_base);

  4. 计算误差 lat_error(横向误差), lon_error(纵向误差), angle_error(角度误差)

update_planner_state()

逻辑:

  1. 根据当前状态更新下一个状态
    1. PRE_ROTATE
      1. 判断setPlan的时间是否超时, 是, ERROR: Timeout in PRE_ROTATE phase; is_crashed=true; return FINISHED;
      2. 判断angle_error是否小于max_goal_angle_error, 是, INFO: FTCLocalPlannerROS: PRE_ROTATE finished return FOLLOWING
      3. break
    2. FOLLOWING
      1. 计算distance = local_control_point.translation().norm()
      2. 判断distance是否大于最大追踪距离max_follow_distance 是, ERROR: FTCLocalPlannerROS: Robot is far away from global plan. distance is_crashed=true return FINISHED
      3. 判断current_index是否为倒数第二个点 是, INFO: FTCLocalPlannerROS: switching planner to position mode return WAITING_FOR_GOAL_APPROACH
      4. break
    3. WAITING_FOR_GOAL_APPEOACH
      1. 计算distance = local_control_point.translation().norm()
      2. 判断是否超时 是, WARN: FTCLocalPlannerROS: Could not reach goal position return POST_ROTATE
      3. 判断距离是否小于max_goal_distance_error 是, INFO: FTCLocalPlannerROS: Reached goal position. return POST_ROTATE
      4. break
    4. POST_ROTATE
      1. 判断是否超时 是, WARN: FTCLocalPlannerROS: Could not reach goal rotation return FINISHED
      2. 判断角度误差是否小于max_goal_angle_error 是, INFO: FTCLocalPlannerROS: POST_ROTATE finished. return FINISHED
      3. break
    5. FINISHED
      1. break
  2. return current_state

bool checkCollision(int max_points)

参数:

max_points 向前预判的点的个数, 根据实际global_plan中每一个点之间的距离设置max_points

例如: 机器人当前点为 global_plan[current_index], 而max_points则负责检测从global_plan[current_index]到global_plan[current_index + max_points]这段路径中间是否有障碍物, 有则为true

逻辑:

  1. 判断是否开启了障碍检测
  2. 如果global_plan.size() < max_points 则将最大值设为路径中点的总数
  3. 判断是否开启了obstacle_footprint 使用实际足迹检测碰撞
    1. costmap->getOrientedFootprint(footprint);获取当前机器人足迹, 保存到footprint
    2. 遍历footprint, 将每一个点从world转换到map
    3. 获取每一个点的代价
    4. 判断当前代价是否大于既定的致命值costs >= costmap_2d::LETHAL_OBSTACLE
    5. 是, WARN: FTCLocalPlannerROS: Possible collision of footprint at actual pose. Stop local planner. return true
  4. for (int i = 0; i < max_points; i++) 判断前视距离是否有障碍物
    1. 获取current_index + i 点的坐标
    2. 将该坐标转换到map坐标系
    3. 获取该坐标的代价值
    4. 判断 costs >127 && costs > previous_cost 是, WARN(FTCLocalPlannerROS: Possible collision. Stop local planner.) return true
    5. 更新previous_cost为costs
    6. 开始下个循环
  5. return false

void calculate_velocity_commands(double dt, geometry_msgs::Twist &cmd_vel)

参数:

dt 现在时间与上次执行时间之差

cmd_vel 速度置零

逻辑:

  1. 判断current_state == FINISHED || is_crashed 速度置零, 返回
  2. 计算PID参数
  3. 判断current_state==FOLLOWING
    1. 通过PID计算线速度
    2. 如果线速度 < 0 且 forward_only 为true 速度归零
    3. 限制线速度在 -max_cmd_vel_speed 到 max_cmd_vel_speed之间
    4. cmd_vel.linear.x = lin_speed
  4. 角速度的运算分两部分, current_state == FOLLOWING以及current_state == PRE_ROTATE
    1. 判断current_state==FOLLOWING
      1. 通过PID计算角速度
      2. 限制角速度
      3. cmd_vel.angular.z = ang_speed
    2. 否则(也就是机器人处于PRE_ROTATE阶段)
      1. 通过PID计算角速度
      2. 限制角速度
      3. cmd_vel.angular.z = ang_speed
      4. 震荡检测, 如果震荡, 则角速度拉满

bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel)

参数:

cmd_vel 速度

逻辑:

  1. 计算dt 执行该代码时的时间 - 上次执行改代码的时间
  2. 判断是否碰撞(is_crashed) 是, 速度置零, return false
  3. 判断是否完成(current_state==FINISHED), 是, 速度置零, return true
  4. 根据dt, 更新控制点 update_control_point(dt)
  5. 更新当前状态 auto new_planner_state = update_planner_state()
  6. 判断: 新状态与当前状态是否一致 是, INFO: FTCLocalPlannerROS: Switching to state 更新state_entered_time (用于判断一个状态是否会超时) 更新当前状态为新状态
  7. 判断是否存在碰撞 checkCollision(obstacle_lookahead) 是, 速度置零 is_crashed = true return false
  8. 计算速度 calculate_velocity_commands(dt, cmd_vel)

这篇关于FTC局部路径规划代码分析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/233863。

相关文章

性能分析之MySQL索引实战案例

性能分析之MySQL索引实战案例

文章目录 一、前言二、准备三、MySQL索引优化四、MySQL 索引知识回顾五、总结 一、前言 在上一讲性能工具之 JProfiler 简单登录案例分析实战中已经发现SQL没有建立索引问题，本文将一起从代码层去分析为什么没有建立索引？ 开源ERP项目地址：https://gitee.com/jishenghua/JSH_ERP 二、准备 打开IDEA找到登录请求资源路径位置
阅读更多...
hdu2544（单源最短路径）

hdu2544（单源最短路径）

模板题： //题意：求1到n的最短路径，模板题#include<iostream>#include<algorithm>#include<cstring>#include<stack>#include<queue>#include<set>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#i
阅读更多...
动态规划---打家劫舍

动态规划---打家劫舍

题目： 你是一个专业的小偷，计划偷窃沿街的房屋。每间房内都藏有一定的现金，影响你偷窃的唯一制约因素就是相邻的房屋装有相互连通的防盗系统，如果两间相邻的房屋在同一晚上被小偷闯入，系统会自动报警。 给定一个代表每个房屋存放金额的非负整数数组，计算你 不触动警报装置的情况下 ，一夜之内能够偷窃到的最高金额。 思路： 动态规划五部曲： 1.确定dp数组及含义 dp数组是一维数组，dp[i]代表
阅读更多...
活用c4d官方开发文档查询代码

活用c4d官方开发文档查询代码

当你问AI助手比如豆包，如何用python禁止掉xpresso标签时候，它会提示到 这时候要用到两个东西。https://developers.maxon.net/论坛搜索和开发文档 比如这里我就在官方找到正确的id描述 然后我就把参数标签换过来
阅读更多...
poj 1258 Agri-Net（最小生成树模板代码）

poj 1258 Agri-Net（最小生成树模板代码）

感觉用这题来当模板更适合。 题意就是给你邻接矩阵求最小生成树啦。~ prim代码：效率很高。172k...0ms。 #include<stdio.h>#include<algorithm>using namespace std;const int MaxN = 101;const int INF = 0x3f3f3f3f;int g[MaxN][MaxN];int n
阅读更多...
poj 1734 （floyd求最小环并打印路径）

poj 1734 （floyd求最小环并打印路径）

题意： 求图中的一个最小环，并打印路径。 解析： ans 保存最小环长度。 一直wa，最后终于找到原因，inf开太大爆掉了。。。 虽然0x3f3f3f3f用memset好用，但是还是有局限性。 代码： #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#incl
阅读更多...
软考系统规划与管理师考试证书含金量高吗？

软考系统规划与管理师考试证书含金量高吗？

2024年软考系统规划与管理师考试报名时间节点： 报名时间：2024年上半年软考将于3月中旬陆续开始报名 考试时间：上半年5月25日到28日，下半年11月9日到12日 分数线：所有科目成绩均须达到45分以上（包括45分）方可通过考试 成绩查询：可在“中国计算机技术职业资格网”上查询软考成绩 出成绩时间：预计在11月左右 证书领取时间：一般在考试成绩公布后3~4个月，各地领取时间有所不同
阅读更多...
poj 2976 分数规划二分贪心（部分对总体的贡献度） poj 3111

poj 2976 分数规划二分贪心（部分对总体的贡献度） poj 3111

poj 2976： 题意： 在n场考试中，每场考试共有b题，答对的题目有a题。 允许去掉k场考试，求能达到的最高正确率是多少。 解析： 假设已知准确率为x，则每场考试对于准确率的贡献值为： a - b * x，将贡献值大的排序排在前面舍弃掉后k个。 然后二分x就行了。 代码： #include <iostream>#include <cstdio>#incl
阅读更多...
计算机毕业设计 大学志愿填报系统 Java+SpringBoot+Vue 前后端分离 文档报告 代码讲解 安装调试

计算机毕业设计 大学志愿填报系统 Java+SpringBoot+Vue 前后端分离 文档报告 代码讲解 安装调试

🍊作者：计算机编程-吉哥 🍊简介：专业从事JavaWeb程序开发，微信小程序开发，定制化项目、 源码、代码讲解、文档撰写、ppt制作。做自己喜欢的事，生活就是快乐的。 🍊心愿：点赞 👍 收藏 ⭐评论 📝 🍅 文末获取源码联系 👇🏻 精彩专栏推荐订阅 👇🏻 不然下次找不到哟~Java毕业设计项目~热门选题推荐《1000套》 目录 1.技术选型 2.开发工具 3.功能
阅读更多...
SWAP作物生长模型安装教程、数据制备、敏感性分析、气候变化影响、R模型敏感性分析与贝叶斯优化、Fortran源代码分析、气候数据降尺度与变化影响分析

SWAP作物生长模型安装教程、数据制备、敏感性分析、气候变化影响、R模型敏感性分析与贝叶斯优化、Fortran源代码分析、气候数据降尺度与变化影响分析

查看原文>>>全流程SWAP农业模型数据制备、敏感性分析及气候变化影响实践技术应用 SWAP模型是由荷兰瓦赫宁根大学开发的先进农作物模型，它综合考虑了土壤-水分-大气以及植被间的相互作用；是一种描述作物生长过程的一种机理性作物生长模型。它不但运用Richard方程，使其能够精确的模拟土壤中水分的运动，而且耦合了WOFOST作物模型使作物的生长描述更为科学。 本文让更多的科研人员和农业工作者
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2