search

arnaud-ramey / followme_laser

followme_laser is used to follow a moving goal.
5 stars 3 forks source link

What does this code do, and how well did it work? #1

Open candersen10 opened 6 years ago

candersen10 commented 6 years ago

Is this used to allow a robot to follow a person with a 2D lidar? If so, how well did it work?

arnaud-ramey commented 6 years ago

Yes, it is made of two components, one for localising a moving cluster, the other to control the motion of the robot towards this cluster. Sorry for the poor documentation, however you can find details in the following published article "Fast 3D cluster tracking for a mobile robot using 2D techniques on depth images" by A Ramey. Hope it helps :)

HaoQChen commented 5 years ago

So I just need to publish a topic with the type geometry_msgs::PoseStamped and goal_cb function in ros_goal_dynamic_window_tracker.h will accept it then follow the goal? And goal's coordinate is relate to base_link ? I use monocular and 2D laser to get target people's pose and want to use your goal_dynamic_window_tracker to follow the target.

HaoQChen commented 5 years ago

Thanks for your code, and I find an bug in vision_utils/timer.h

const static Time NOTIME = -1; should be constexpr static Time NOTIME = -1;

According to Why can't I have a non-integral static const member in a class?, this will cause an error in a higher g++ version.

HaoQChen commented 5 years ago

Hope this will help

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

image

image

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
3. change to ACTION_KEEP_SAME_SPEED