| layout | title | package | category | namespace | tags | |||
|---|---|---|---|---|---|---|---|---|
reference |
may_nav |
may_nav_ros |
node |
may_nav |
|
May_nav generates a global plan to a waypoint and outputs twists to get the robot to follow it. Recovery behaviors like clearing maps are triggered externally (in gizmo), through ROS services provided by may_nav. There are also services to get information on distances to obstacles, which are used by gizmo to do things like decide whether to move the base while dancing.
may_nav has a custom 2D costmap implementation. A local costmap is generated at each time step that combines obstacles from the depth sensor (2-D fake-laser, high/low obstacle points, and cliff readings) with obstacles from the bump sensors. The local costmap is stamped atop the global costmap (which holds both the static map obstacles as well as previously-stamped local costmap obstacles) based on the robot's current estimated position.
Path planning is split into two parts: global planning and local planning. The global "plan" (actually, full grid containing costs to the goal from every location) is generated every few seconds, using the latest merged costmap. Every time step, the global path is updated to contain the shortest path to the goal from the robot's current estimated position. That path also has straight-line shortcutting applied to the start of the path (to make it more understandable / smooth-looking), as far as possible without getting too close to obstacles.
Local planning (selecting the output twist for each time step, based on the global path and the current costmap) is done using a DWA (Dynamic Window Approach) local planner. The majority of the ROS params (listed below) that one may want to tweak have to do with the local planner, so here is a more detailed explanation of how the local planner works:
The DWA (Dynamic Window Approach) local planner generates a set of possible velocity commands (twists), scores each one as described below, then picks the command with the best score.
v_start: starting velocity of robot in base_link frame
v_target: target velocity of robot in base_link frame
a_target: acceleration to apply
d_target: deceleration to apply
delta_t: time delta in seconds for forward integration
t_lookahead: how many seconds to propagate the trajectory forward
trajectory: resulting trajectory (Output param)
A trajectory is generated by iteratively stepping forward the state (Pose p, Velocity v), where we are given the current velocity v_start. The acceleration applied to reach v_target is assumed to be
a_target and d_target for decelerations. The time step is delta_t and we step the robots pose forward for t_lookahead steps. The trajectory's score will be dermined by the score of the pose at the end of the trajectory. We apply this method to a variety of values for v_target and choose the best scoring one.
The trajectory generation looks approximately as follows:
float vx_min, vx_max, va_min, va_max // Set to bound the end velocity to v_target
Acceleration a; // Determined from accleration parameters
Pose p = map_pose
Velocity v = v_start
for (float t = 0.0; t <= t_lookahead; t += delta_t) {
// Propagate pose forward
p.x += delta_t * (cos(p.theta) * v.x - sin(p.theta) * v.y);
p.y += delta_t * (sin(p.theta) * v.x + cos(p.theta) * v.y);
p.theta += delta_t * v.theta;
// Apply acceleration to velocity
v.x += delta_t * a.x;
v.y += delta_t * a.y;
v.theta += delta_t * a.theta;
// Avoid exceeding the target_velocity
if(v.x < vx_min) {v.x = vx_min;}
if(v.x > vx_max) {v.x = vx_max;}
if(v.theta < va_min) {v.theta = va_min;}
if(v.theta > va_max) {v.theta = va_max;}
}
trajectory.push_back(t, p, v, a);
The highest-scoring trajectory is the one selected by the local planner.
The method for computing a trajectory's score is as follows:
Note: Target pose is the pose along the global plan that the local planner is attempting to drive towards. The map pose is the pose of the robot in the map frame. This pose is selected by looking ahead along the global plan by
plan_lookahead_distance
score = -(k_target_distance * distance_to_target_pose +
k_plan_angle_difference * plan_angle_distance +
k_target_angle_difference * target_angle_difference +
k_obstacle_score * obstacle_cost +
k_heading_angle_difference * heading_angle_difference)
where the k_ coefficients are ROS params, defined below, and the terms of the scoring function are defined as follows (these are variables, not params):
Distance to the target_pose from the map pose
The absolute angle difference between the robot's pose in the map frame and the target pose
The absolute angle difference between the robot's pose in the map frame and the ray from the robot's position to the position of the target pose
The cost function applied to the distance of the end of the trajectory to the nearest obstacle
cost = 1.0 + cost_func_dist_scaling / d;
cost_func_dist_scalingis a global planner parameter
Difference of the robot's pose in the map frame to the heading of the global plan at our current position. Increasing this score will cause the robot to follow the global plan more closely.
geometry_msgsnav_msgssensor_msgsstd_srvsmobile_basedepthscanmay_nav_msgsmay_nav_coremay_nav_rosdynamic_reconfigure
may_nav_msgs/NavigateAction (Navigate.action)
Used to send navigation pose goals of different types (drive to waypoint vs drive to point in image).
The action feedback is used to coordinate with the navigation recovery behavior state machine in gizmo.
sensor_msgs/LaserScan
The laser scan from Kuri's depth sensor
Type: tf/tfMessage
Tree of transforms
nav_msgs/Odometry
Odometry data from mobile_base
depth_scan_msgs/CliffArray (CliffArray.msg)
Cliff sensor information from Kuri's forward depth sensor
nav_msgs/OccupancyGrid
Map provided by SLAM for navigation
geometry_msgs/Pose
Tracked person's pose for deprecated person following behavior
mobile_base_driver/Sensors
Used to get bump sensor information
sensor_msgs/PointCloud2
Used to incorporate low and high obstacles into the dynamic cost maps
tf2_msgs/TFMessage
Tree of static transforms
geometry_msgs/Twist
Commanded velocity
nav_msgs/OccupancyGrid
The static global map (the same as the one published by map_nav)
nav_msgs/OccupancyGrid
Local costmap containing bump obstacle information
nav_msgs/OccupancyGrid
Local cost map containing all obstacle information
nav_msgs/OccupancyGrid
Local cost map containing all laser information
visualization_msgs/MarkerArray
Used to visualize local planner trajectories in rviz
visualization_msgs/MarkerArray
Used to visualize local planner trajectories in rviz
nav_msgs/OccupancyGrid
Local cost map containing high and low obstacles
nav_msgs/OccupancyGrid
Global cost map containg the history of obstacles accumulated by stamping the local cost maps on to a blank global map
nav_msgs/Path
The path specified by the global planner
nav_msgs/OccupancyGrid
A grid containg a ring of obstacles to bound the global planner. Used in the drive to point in image nav mode
nav_msgs/OccupancyGrid
The global obstacle cost map with values normalized from [0, 100]. This is used to visualize the obstacle cost map in RViz
diagnostic_msgs/DiagnosticArray
rosgraph_msgs/Log
may_nav_msgs/ScoredTrajectories
Contains trajectory scoring data for the rotating_to_global_plan state
may_nav_msgs/ScoredTrajectories
Contains trajectory scoring data for the following_global_plan state
std_srvs/Empty
Clears all local maps. This should be done to clear out dynamic obstacles that have been retained in the maps
std_srvs/Empty
Clears the dynamic global map if dynamic obstacles are preventing the global planner from completing successfully
std_srvs/Empty
Uses an alternate set of local planner scoring params that emphasize following the global plan more closely over smoothness
may_nav_msgs/GetObstacleDist
Get the distance of the robot to the nearest obstacle
may_nav_msgs/GetCostMap
Get a cost map where the cost is the distance to the nearest obstacle
Configuration: $(find may_nav_ros)/config/local_planner_params_$(arg robot_name).yaml
Note: $(arg robot_name) refers to robot version, not hostname (for the final Kuri, this should be "p3-ds")
Length of each time step
Radius of the robot to determine collisions
Linear acceleration limit. Computation for the max possible velocity for the next time step is as follows:
float cmd_vx_max = fmin(
v_current.x + params.vx_cmd_max_difference,
v_cmd_previous.x + params.loop_period * params.ax);
Linear deceleration limit. Computation for the min possible velocity for the next time step
float cmd_vx_min = fmax(
v_current.x - params.vx_cmd_max_difference,
v_cmd_previous.x + params.loop_period * params.dx);
Max linear velocity
Min linear velocity
Angular accleration limit. Computation for the max possible angular acceleration at a particular time step is as follows:
float cmd_va_max = fmin(
v_current.theta + params.va_cmd_max_difference,
v_cmd_previous.theta + params.loop_period * params.aa);
Angular deceleration limit. Computation for the min possible angular acceleration at a particular time step is as follows:
float cmd_va_min = fmax(
v_current.theta - params.va_cmd_max_difference,
v_cmd_previous.theta - params.loop_period * params.aa);
Max angular velocity
Min angular velocity
Angular velocity increment between generated trajectories
Linear velocity increment between generated trajectories
Maximum linear velocity difference from the last command
Maximum angular velocity difference from the last command
How far ahead to lookahead when generating a trajectory
Step size when propagating a trajectory
How far ahead to look along the global plan to compute a target pose in meters
How many points along the global plan to look ahead to determine the heading of the global plan.
Scaling coefficient for obstacle costs
Determines whether trajectory obstacle costs are determined from the end of a trajectory or from the most costly point along the trajectory. This should just be set to true.
Max distance from goal to determine success
Max angle from goal to determine success
Maximum angular offset from the global plan before we decide to rotate in place to realign with the global plan.
Maximum linear velocity at which we need a ramp down period when approaching a goal
Maximum angular velocity at which we need a ramp down period when approaching a goal
Coefficient for the distance to the target pose
Coefficient for the distance from the global plan
Coefficient for the angle difference from the target_pose
Coefficient for the atan2(dy, dx) where dx, dy are the offset from the target pose
Coefficient applied to the distance from obstacles
Coefficient applied to the difference between the robot heading and the plan heading
Time before we need to clear the local map if we can't command a non-zero velocity
$(find may_nav_ros)/config/global_planner_params_$(arg robot_name).yaml
The global planner can modify the goal if planning fails. This sets the size of the search region for this alternate goal
Replan after this amount of time has expired since the last replan. Replan happens right away if a bump/cliff/clothesline obstacle is detected.
Minimum amount of time since the last replan. Prevents you from spamming the global planner if someone presses the bump sensor repeatedly
The cost map is scaled when planning. The function used is
1.0 + cost_func_dist_scaling/d where d is the distance to the nearest obstacle
A buffer region to keep discretization error from causing the plan to clip an obstacle
The global planner will shortcut pieces of the global plan to make them straight. This is to make plans more visually comprehensible to users. We do not want to do this on paths that run too close to obstacles. This smooth_buffer_size is the minimum obstacle-free buffer distance required around the robot for a straight, shortcutting path to be used.
$(find may_nav_ros)/config/local_map_params_$(arg robot_name).yaml
Width of local map
Height of local map
Resolution of local map. Should match resolution of the global map
Radius to be cleared based on the robot's footprint
Size of the obstacle blob placed in the local bumper map, when one of Kuri's three bump sensors is triggered
Distance to place bump obstacle blobs in the local map, from the center of the robot
Whether or not infinite range values should clear pixels. Recommend false because not all objects will be detected.
may_nav.launch