Description

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:

Trajectory Generation

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);

Trajectory Scoring

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_target_pose

Distance to the target_pose from the map pose

plan_angle_distance

The absolute angle difference between the robot’s pose in the map frame and the target pose

target_angle_difference

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

obstacle_cost

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_scaling is a global planner parameter

heading_angle_difference

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.

Dependencies

• geometry_msgs
• nav_msgs
• sensor_msgs
• std_srvs
• mobile_base
• depthscan
• may_nav_msgs
• may_nav_core
• may_nav_ros
• dynamic_reconfigure

Action API

/navigate

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.

Subscribed Topics

/scan_reduced_nav

sensor_msgs/LaserScan
The laser scan from Kuri’s depth sensor

/tf

Type: tf/tfMessage
Tree of transforms

/odom

nav_msgs/Odometry
Odometry data from mobile_base

/depthscan/front_cliff

depth_scan_msgs/CliffArray (CliffArray.msg)
Cliff sensor information from Kuri’s forward depth sensor

/map_nav

nav_msgs/OccupancyGrid
Map provided by SLAM for navigation

/tracked_person

geometry_msgs/Pose
Tracked person’s pose for deprecated person following behavior

/mobile_base/sensors

mobile_base_driver/Sensors
Used to get bump sensor information

/obstacle_cloud

sensor_msgs/PointCloud2
Used to incorporate low and high obstacles into the dynamic cost maps

/tf_static

tf2_msgs/TFMessage
Tree of static transforms

Published Topics

/cmd_vel_mux/inputs/navi

geometry_msgs/Twist
Commanded velocity

/static_global_map

nav_msgs/OccupancyGrid
The static global map (the same as the one published by map_nav)

/dynamic_local_bumper_map

nav_msgs/OccupancyGrid
Local costmap containing bump obstacle information

/dynamic_local_merged_map

nav_msgs/OccupancyGrid
Local cost map containing all obstacle information

/dynamic_local_laser_map

nav_msgs/OccupancyGrid
Local cost map containing all laser information

/rotating_to_global_plan/trajectories/markers

visualization_msgs/MarkerArray
Used to visualize local planner trajectories in rviz

/following_global_plan/trajectories/markers

visualization_msgs/MarkerArray
Used to visualize local planner trajectories in rviz

/dynamic_local_obstacle_cloud_map

nav_msgs/OccupancyGrid
Local cost map containing high and low obstacles

/dynamic_global_map

nav_msgs/OccupancyGrid
Global cost map containg the history of obstacles accumulated by stamping the local cost maps on to a blank global map

/global_plan

nav_msgs/Path
The path specified by the global planner

/bounding_grid

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

/obstacle_cost_map

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

/diagnostics

diagnostic_msgs/DiagnosticArray

/rosout

rosgraph_msgs/Log

/rotating_to_global_plan/trajectories/scored_trajectories

may_nav_msgs/ScoredTrajectories
Contains trajectory scoring data for the rotating_to_global_plan state

/following_global_plan/trajectories/scored_trajectories

may_nav_msgs/ScoredTrajectories
Contains trajectory scoring data for the following_global_plan state

Services

/clear_local_map

std_srvs/Empty

Clears all local maps. This should be done to clear out dynamic obstacles that have been retained in the maps

/clear_global_map

std_srvs/Empty

Clears the dynamic global map if dynamic obstacles are preventing the global planner from completing successfully

/run_safe_mode

std_srvs/Empty

Uses an alternate set of local planner scoring params that emphasize following the global plan more closely over smoothness

/get_obstacle_dist

may_nav_msgs/GetObstacleDist

Get the distance of the robot to the nearest obstacle

/get_obstacle_cost_map

may_nav_msgs/GetCostMap

Get a cost map where the cost is the distance to the nearest obstacle

Parameters

Local Planner

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”)

local_planner/loop_period

Length of each time step

local_planner/robot_radius

Radius of the robot to determine collisions

local_planner/ax

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);

local_planner/dx

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);

local_planner/vx_max

Max linear velocity

local_planner/vx_min

Min linear velocity

local_planner/aa

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);

local_planner/da

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);

local_planner/va_max

Max angular velocity

local_planner/va_min

Min angular velocity

local_planner/va_increment

Angular velocity increment between generated trajectories

local_planner/vx_increment

Linear velocity increment between generated trajectories

local_planner/vx_cmd_max_difference

Maximum linear velocity difference from the last command

local_planner/va_cmd_max_difference

Maximum angular velocity difference from the last command

local_planner/t_lookahead

How far ahead to lookahead when generating a trajectory

local_planner/delta_t

Step size when propagating a trajectory

local_planner/plan_lookahead_distance

How far ahead to look along the global plan to compute a target pose in meters

local_planner/heading_lookahead_steps

How many points along the global plan to look ahead to determine the heading of the global plan.

local_planner/cost_func_dist_scaling

Scaling coefficient for obstacle costs

local_planner/score_from_end_point

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.

local_planner/goal_reached_distance

Max distance from goal to determine success

local_planner/goal_reached_angle_difference

Max angle from goal to determine success

local_planner/plan_alignment_threshold

Maximum angular offset from the global plan before we decide to rotate in place to realign with the global plan.

local_planner/vx_ramp_down_threshold

Maximum linear velocity at which we need a ramp down period when approaching a goal

local_planner/va_ramp_down_threshold

Maximum angular velocity at which we need a ramp down period when approaching a goal

local_planner/k_target_distance

Coefficient for the distance to the target pose

local_planner/k_plan_distance

Coefficient for the distance from the global plan

local_planner/k_plan_angle_difference

Coefficient for the angle difference from the target_pose

local_planner/k_target_angle_difference

Coefficient for the atan2(dy, dx) where dx, dy are the offset from the target pose

local_planner/k_obstacle

Coefficient applied to the distance from obstacles

local_planner/k_heading_angle_difference

Coefficient applied to the difference between the robot heading and the plan heading

local_planner/local_map_clearing_timeout

Time before we need to clear the local map if we can’t command a non-zero velocity

Global Planner

$(find may_nav_ros)/config/global_planner_params_$(arg robot_name).yaml

global_planner/goal_search_radius

The global planner can modify the goal if planning fails. This sets the size of the search region for this alternate goal

global_planner/max_replan_period

Replan after this amount of time has expired since the last replan. Replan happens right away if a bump/cliff/clothesline obstacle is detected.

global_planner/min_replan_period

Minimum amount of time since the last replan. Prevents you from spamming the global planner if someone presses the bump sensor repeatedly

global_planner/cost_func_dist_scaling

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

global_planner/robot_radius_buffer

A buffer region to keep discretization error from causing the plan to clip an obstacle

global_planner/smooth_buffer_size

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.

Local Map

$(find may_nav_ros)/config/local_map_params_$(arg robot_name).yaml

Fields

local_map/size_x

Width of local map

local_map/size_y

Height of local map

local_map/resolution

Resolution of local map. Should match resolution of the global map

local_map/self_filter_radius

Radius to be cleared based on the robot’s footprint

local_map/bumper_contact radius

Size of the obstacle blob placed in the local bumper map, when one of Kuri’s three bump sensors is triggered

local_map/bumper_radius

Distance to place bump obstacle blobs in the local map, from the center of the robot

local_map/clear_laser_infs

Whether or not infinite range values should clear pixels. Recommend false because not all objects will be detected.

Launch File

may_nav.launch