码迷,mamicode.com
首页 > 其他好文 > 详细

followme_laser包解读

时间:2019-06-19 00:29:06      阅读:124      评论:0      收藏:0      [点我收藏+]

标签:collision   between   ota   tor   main   com   ack   sub   rop   

param name default meaning
_time_pred 5 the forecast time in seconds
_time_step 0.2 the time step simulation in seconds
_speed_recomputation_timeout 1 recompute speeds if the last speed are too old
_goal_timeout 1 recompute speeds if the last goal are too old
_min_goal_distance 0.6 min distance to goal when stop
_max_goal_angle 0.2 max angle to goal when stop
_laser_thickness
_min_v 0.1 fabs value at ROSGoalDynamicWindowTracker
_max_v 0.3 fabs value at ROSGoalDynamicWindowTracker
_max_w 0.5 fabs value at ROSGoalDynamicWindowTracker, min is 0.1 max
SPEED_STEPS 40+1 decide how much steps use to sim
_dv decided by SPEED_STEPS, will used to make trajectory from _min_v to _max_v and -_max_w to _max_w
_dw decided by SPEED_STEPS
_robot_radius 0.5 use to set_costmap and pass it to _laser_thickness if it is within a range
_costmap_cell_centers laser coordinate of xy in base_link calculated at scan_cb()
_trajs precompute_trajectories() cal from -_max_w:_dw:_max_w of _min_v:_dv:_max_v

RGDWT = ROSGoalDynamicWindowTracker GDWT = GoalDynamicWindowTracker

graph TD subgraph main GDWT["GDWT::GoalDynamicWindowTracker()"] ==> RGDWT["RGDWT::ROSGoalDynamicWindowTracker()"] RGDWT ==> GCB["RGDWT::void goal_cb(geometry_msgs::PoseStamped)"] GCB ==> SG["GDWT::set_goal and ACTION_RECOMPUTE_SPEED"] SG ==> SCB["RGDWT::scan_cb"] subgraph scan_cb CSDTXY["convert_sensor_data_to_xy"] --> CXYVF["convert_xy_vec_frame //to base_link"] CXYVF --> SCT["GDWT::set_costmap //just pass param"] SCT --> RSIN["GDWT::recompute_speeds_if_needed"] RSIN --> PUBLISH["_vel_pub.publish and _traj_pub.publish"] end end
st=>start: GDWT::recompute_speeds_if_needed part1
r1=>end: false

op_un=>operation: ACTION_UNDEFINED
op_ksp=>operation: ACTION_KEEP_SAME_SPEED
op_st=>operation: ACTION_STOP
op_rop=>operation: ACTION_ROTATE_ON_PLACE
op_rs=>operation: ACTION_RECOMPUTE_SPEED

c_goalset=>condition: _goal_set
c_gto=>condition: _goal_timeout
c_srt=>condition: _speed_recomputation_timeout
c_wingd=>condition: within min goal distance
c_winga=>condition: within max goal angle
c_rs=>condition: current is recompute
c_cs=>condition: collision soon

st->c_goalset

c_goalset(no, right)->op_st
c_goalset(yes)->c_gto

c_gto(yes, right)->op_st
c_gto(no)->c_srt

c_srt(yes, right)->op_rs
c_srt(no)->c_wingd

c_wingd(yes, right)->c_winga
c_winga(yes, right)->op_st
c_winga(no, bottom)->op_rop
c_wingd(no)->c_rs

c_rs(yes, right)->op_rs
c_rs(no)->c_cs

c_cs(yes, right)->op_rs
c_cs(no)->op_ksp

GDWT::recompute_speeds_if_needed part2:

state action
ACTION_KEEP_SAME_SPEED nothing to do
ACTION_STOP stop_robot();//to cal _best_traj_idx
ACTION_ROTATE_ON_PLACE best w is 0.5 * (fabs(curr_goal_angle) - _max_goal_angle)
ACTION_RECOMPUTE_SPEED 1. best_grade_in_range() which call trajectory_grade() to compare each _trajs with _costmap_cell_centers return 1 / dis of goal and _trajs.back()
2. and trajectory_grade() call vectors_dist_thres() to cal min dis between traj and _costmap_cell_centers and if min_dist < _laser_thickness return false
3. if best speed not found go back
4. change to ACTION_KEEP_SAME_SPEED

followme_laser包解读

标签:collision   between   ota   tor   main   com   ack   sub   rop   

原文地址:https://www.cnblogs.com/HaoQChen/p/11048642.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!