teb_local_planner_ros.cpp
2019-03-04 10:08:43 0 举报
rdgdfhdrhgd
作者其他创作
大纲/内容
获取机器人的足迹以及从机器人中心到其足迹顶点的最小和最大距离。
将全局规划变换成坐标,为机器人创建一个动态窗口
“obstacle”
bool TebLocalPlannerROS::computeVelocityCommands
global_frame_ = costmap_ros_->getGlobalFrameID()
custom_via_points_active_=false
estimateLocalGoalOrientation():估计目标方向
clearPlanner()
footprint_spec_ = costmap_ros_->getRobotFootprint();
OdometryHelperRos::getRobotVel
\"odom_topic\"
validateFootprints
robot_pose_
从里程计中获得速度
\"via_point\"
是
把接受到的全局路径保存起来供局部规划器使用
robot_point
TebLocalPlannerROS::initialize
存储最后一个速度指令(用于恢复分析等)
判断轨迹是否可行
更新via-points容器
传入
判断是否激活同伦类计划(并行规划),并初始化相应规划器的类
goal_reached_=true
global_plan_ = orig_global_plan
获取当前目标点,transformed plan中的最后一个点goal_point
否
强制重新初始化
能否将世界坐标系下的局部路径转化到局部坐标系
使用实际机器人位置覆盖/更新已转换计划的初始位置transformed_plan.begin()
机器人速度信息
获取主代价地图,世界坐标系、基座坐标系
从代价地图中的获得机器人全局坐标系下的位姿
teb_config::cmd_angle_instead_rotvel
return false
costmap_ = costmap_ros_->getCostmap
robot_vel_
robot_goal_
设置里程计topic
TebLocalPlannerROS::convertTransRotVelToSteeringAngle
TebLocalPlannerROS::getRobotFootprint()
updateObstacleContainerWithCostmap()
是否用相应的转向角代替指令速度信息中的旋转速度
从参数服务器中取出相应参数并初始化变量
PlannerInterface::saturateVelocity
costmap_2d::calculateMinAndMaxDistances
bool TebLocalPlannerROS::setPlan
RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh)
初始化机器人位姿,cmd_vel中所有速度为零
将方案可视化
检查速度指令有效性
设置足迹验证函数
return true
给定机器人的当前位置,方向和速度,计算要发送到基地的速度命令
updateObstacleContainerWithCostmapConverter()
之后锁定配置,开始优化
确认方案可行,重置计数器
将代价地图初始化为多边形转换器????看不懂
如果未收到有效viapoint
bool Costmap2DROS::getRobotPose
transformGlobalPlan
检查是否以到达目标点(三个条件)
是否有起始点:if (transformed_plan.size()==1)
TebLocalPlannerROS::customViaPointsCB
++no_infeasible_plans_
从参数服务器获取机器人模型,存在变量robot_model中
初始化故障检测器
cmd_vel.angular.z是否是有限数
回调函数
设置保留50个震荡
PlannerInterface::isTrajectoryFeasible
topic:“footprint”或动态配置
转换
检查是否应该进入任何备份模式并应用设置
cfg_.loadRosParamFromNodeHandle(nh)
TebLocalPlannerROS::customObstacleCB
robot_base_frame_ = costmap_ros_->getBaseFrameID()
是否重写由全局规划器提供的局部子目标方向
no_infeasible_plans_ = 0
检查plan设置是否成功
删除全局路径中通过的部分(在机器人之前的空间)
updateObstacleContainerWithCustomObstacles()
创建两个subscriber
更新机器人的足迹并计算从机器人中心到其足迹顶点的最小和最大距离。
不可行方案数+1
transformed_plan是否为空
rosparam \"footprint\"
PlannerInterface::getVelocityCommand
清空障碍容器,使用costmap_converter插件提供的costmap信息或多边形信息和自定义障碍物信息更新障碍物容器
0 条评论
下一页