| 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 false3. if best speed not found go back 4. change to ACTION_KEEP_SAME_SPEED |