followme_laser包解读

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