搜狗站长平台验证网站,wordpress wp_update_post,外贸平台网站,企业网站源码php前置知识:
costmap_2d::Costmap2DROS costmap;
costmap_2d::Costmap2DROS 是一个ROS包中提供的用于处理2D成本地图的类。它是一个高级的接口#xff0c;通常用于与ROS导航栈中的导航规划器和本地路径跟踪器等模块进行集成。
costmap 是一个指向 Costmap2DROS 对象的指针。通…前置知识:
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::cectorgeometry_msgs::Point oriented_footprint)
构建机器人在当前姿态下的足迹结果将存储在参数 oriented_footprint 中, 这个函数可以返回机器人当前位置的足迹, 获取的是一个机器人在世界坐标系下的坐标, 通常用于碰撞检测或生成局部代价地图
costmap-getRobotFootprint()
获取机器人的轮廓, 表现为一系列std::vectorgeometry_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: 发布当前控制点, 即ftc代码判断的机器人的位置, 由 current_index控制
obstacle_marker_pub: 发布障碍物的信息, 当靠近障碍物时, rviz中的障碍物会由绿色变为黄色, 当障碍物变为红色时, 机器人停止移动
setPlan(const std::vectorgeometry_msgs::PoseStamped plan)
参数:
plan 包含位姿信息的向量, 从起始点指向结束点
逻辑:
定义当前状态 PRE_ROTATE复制plan到global_plan如果规划中大于两个点(路径规划存在), 则将倒数第二个点的朝向改为倒数第三个点的朝向 global_plan[global_plan.size() - 2].pose.orientation global_plan[global_plan.size() - 3].pose.orientation;创建导航路径 nav_msgs::Path path;将收到的路径plan原封不动放到path.poses中发布path到”/ftc_local_planner/global_plan”
distanceLookahead() #未知
double FTCPlannerROS::distanceLookahead(){if (global_plan.size() 2){return 0;}Eigen::Quaterniondouble 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::Quaterniondouble 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坐标系下的点 判断当前执行状态 PRE_ROTATE 将global_plan[0].pose存入current_control_point FOLLOWING 计算前视距离 straight_distdistanceLookahead()如果前视距离大于满速行进的阈值, 则全速前进若目标速度大于当前速度, 则根据配置的加速度对当前速度进行提升如果: 需要移动的距离大于零, 同时需要转动的角度大于零且当前点不为全局路径规划的倒数第二个点, 进入循环 从全局规划中获取下两个点, currentPose以及nextPose计算两个点的距离 pose_distance如果两个点之间的距离小于等于0, 则警告: 跳过重复点(Skipping duplicate point in global plan)通过current_progress(0-1之间的数), 计算剩余距离与角度如果剩余的距离和角度可以在当前时间步内到达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**如果无法在当前时间步内到达下一个位置则更新**current_progress**以确保机器人在下一个时间步内能够到达下一个位置 最后通过插值计算机器人在当前时间步的控制点位置和方向然后将其存储在**current_control_point**中以供后续控制使用 POST_ROTATE 将global_plan中的最后一个点保存到current_control_point中 WAITTING_FOR_GOAL_APPROACH break FINISHED break 创建目标点, viz用于发布current_control_point的坐标 通过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);计算误差 lat_error(横向误差), lon_error(纵向误差), angle_error(角度误差)
update_planner_state()
逻辑:
根据当前状态更新下一个状态 PRE_ROTATE 判断setPlan的时间是否超时, 是, ERROR: Timeout in PRE_ROTATE phase; is_crashedtrue; return FINISHED;判断angle_error是否小于max_goal_angle_error, 是, INFO: FTCLocalPlannerROS: PRE_ROTATE finished return FOLLOWINGbreak FOLLOWING 计算distance local_control_point.translation().norm()判断distance是否大于最大追踪距离max_follow_distance 是, ERROR: FTCLocalPlannerROS: Robot is far away from global plan. distance is_crashedtrue return FINISHED判断current_index是否为倒数第二个点 是, INFO: FTCLocalPlannerROS: switching planner to position mode return WAITING_FOR_GOAL_APPROACHbreak WAITING_FOR_GOAL_APPEOACH 计算distance local_control_point.translation().norm()判断是否超时 是, WARN: FTCLocalPlannerROS: Could not reach goal position return POST_ROTATE判断距离是否小于max_goal_distance_error 是, INFO: FTCLocalPlannerROS: Reached goal position. return POST_ROTATEbreak POST_ROTATE 判断是否超时 是, WARN: FTCLocalPlannerROS: Could not reach goal rotation return FINISHED判断角度误差是否小于max_goal_angle_error 是, INFO: FTCLocalPlannerROS: POST_ROTATE finished. return FINISHEDbreak FINISHED break 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
逻辑:
判断是否开启了障碍检测如果global_plan.size() max_points 则将最大值设为路径中点的总数判断是否开启了obstacle_footprint 使用实际足迹检测碰撞 costmap-getOrientedFootprint(footprint);获取当前机器人足迹, 保存到footprint遍历footprint, 将每一个点从world转换到map获取每一个点的代价判断当前代价是否大于既定的致命值costs costmap_2d::LETHAL_OBSTACLE是, WARN: FTCLocalPlannerROS: Possible collision of footprint at actual pose. Stop local planner. return true for (int i 0; i max_points; i) 判断前视距离是否有障碍物 获取current_index i 点的坐标将该坐标转换到map坐标系获取该坐标的代价值判断 costs 127 costs previous_cost 是, WARN(FTCLocalPlannerROS: Possible collision. Stop local planner.) return true更新previous_cost为costs开始下个循环 return false
void calculate_velocity_commands(double dt, geometry_msgs::Twist cmd_vel)
参数:
dt 现在时间与上次执行时间之差
cmd_vel 速度置零
逻辑:
判断current_state FINISHED || is_crashed 速度置零, 返回计算PID参数判断current_stateFOLLOWING 通过PID计算线速度如果线速度 0 且 forward_only 为true 速度归零限制线速度在 -max_cmd_vel_speed 到 max_cmd_vel_speed之间cmd_vel.linear.x lin_speed 角速度的运算分两部分, current_state FOLLOWING以及current_state PRE_ROTATE 判断current_stateFOLLOWING 通过PID计算角速度限制角速度cmd_vel.angular.z ang_speed 否则(也就是机器人处于PRE_ROTATE阶段) 通过PID计算角速度限制角速度cmd_vel.angular.z ang_speed震荡检测, 如果震荡, 则角速度拉满
bool computeVelocityCommands(geometry_msgs::Twist cmd_vel)
参数:
cmd_vel 速度
逻辑:
计算dt 执行该代码时的时间 - 上次执行改代码的时间判断是否碰撞(is_crashed) 是, 速度置零, return false判断是否完成(current_stateFINISHED), 是, 速度置零, return true根据dt, 更新控制点 update_control_point(dt)更新当前状态 auto new_planner_state update_planner_state()判断: 新状态与当前状态是否一致 是, INFO: FTCLocalPlannerROS: Switching to state 更新state_entered_time (用于判断一个状态是否会超时) 更新当前状态为新状态判断是否存在碰撞 checkCollision(obstacle_lookahead) 是, 速度置零 is_crashed true return false计算速度 calculate_velocity_commands(dt, cmd_vel)
