Central Dogma of ROS Navigation

Fundamentals of Local Planning
Fun

What is thebest commandvelocity?

What Command Velocities are Available?

+xmax_vel_x-max_vel_theta-θ+max_vel_theta+θA Brief History Interlude:A Brief History Interlude:

What Command Velocities are Available?
vel_x, vel_theta+xmax_vel_x

vx_samples = 5
vtheta_samples = 9-max_vel_theta-θ+max_vel_theta+θWhat is thebest commandvelocity?

What would thecommands do?

What would the commands do?
pose:(0.0, 0.42, 0.0) /mapvel:(0.0, 0.0, 0.0)
cmd_vel: (1.0, 0.0, -0.8)

Key Parameters:
sim_time (t=1.5)
discretization parameters

t=0.75(0.63, 0.31, -0.51)
t=0.0(0.0, 0.42, 0.0)

t=1.50(1.12, -0.09, -1.03)

What is thebest command
velocity?
How do we define the"best" command?

How do we define the "best" command?

GOALABCWALL● Moves towards goal
● Doesn't hit obstacles

A: 10.0
B: 40.0
C: -1.0

Score Rules:
Negative is Invalid
Lower is Better

How do we define the "best" command?
Critics produce scores
Final Score = Weighted Sum of Scores
For each critic:
if critic.raw_score < 0: return critic.raw_score
score += critic.scale * critic.raw_score

How do we define the "best" command?
Cost on the Costmap (Obstacles)
GOAL

WALL

A
B

C

How do we define the "best" command?
Cost on the Costmap (Obstacles)
GOAL

global path

WALL

A
B

C

Distance to Global Path (PathDist)How do we define the "best" command?
Cost on the Costmap (Obstacles)
GOAL

WALL

Distance to Global Path (PathDist)Distance to Goal (GoalDist)ABCHow do we define the "best" command?
Cost on the Costmap (Obstacles)
GOAL

A

Distance to Global Path (PathDist)

WALL

B

Distance to Goal (GoalDist)
Orientation To Goal (GoalAlign)

How do we define the "best" command?
Cost on the Costmap (Obstacles)
GOAL

global path

Distance to Global Path (PathDist)Distance to Goal (GoalDist)

WALL

Orientation To Goal (GoalAlign)A

B

Orientation To Path (PathAlign)What is thebest command
velocity?

ROS Navigation Today:
Theory:Practice:ROS Navigation Today

base_local_planner

dwa_local_planner

ROS Navigation Today

nav_core/base_local_planner.hvoid initialize(string name, TransformListener* tf,Costmap2DROS* costmap_ros);bool setPlan(vector<PoseStamped> plan);bool computeVelocityCommands(Twist& cmd_vel);bool isGoalReached();ROS Navigation Tomorrow

The structure of the classes in the codeshould matchthe conceptual pieces of the algorithm,
which should matchthe ROS interfaces.

ROS Navigation Tomorrow
Elizabeth
Ada Lu2017-7-25

github.com/locusrobotics/robot_navigation

ROS Navigation Tomorrow

void initialize(string name, TransformListener::Ptr tf,Costmap2DROS::Ptr costmap_ros);
void setPlan(Path2D path);
nav_2d_msgs
Twist2DStamped computeVelocityCommands(Pose2DStamped pose, Twist2D velocity)bool isGoalReached(Pose2DStamped pose,
More explicit
computeVelocityCommands
Twist2D velocity);
Error handling
via Exceptions

Parameterized
isGoalReachedROS Navigation Tomorrow

dwb_local_planner
Velocity Iterator &Trajectory Generator

Trajectory
Critic[ ]GoalChecker

dwb_local_planner
void startNewIteration(Twist2D current_velocity);bool hasMoreTwists();
Twist2D nextTwist();Trajectory2D generateTrajectory(Pose2D start_pose,Twist2D start_vel, Twist2D cmd_vel);dwb_local_planner
void onInit();bool prepare(Pose2D pose, Twist2D vel,Pose2D goal, Path2D global_plan);double scoreTrajectory(Trajectory2D traj);
double getScale();void debrief(Twist2D cmd_vel);dwb_local_planner

CostmapLayer
TrajectoryGenerator
TrajectoryCriticGoalChecker

dwb_local_planner[dwb_msgs/GenerateTrajectory.srv]:
geometry_msgs/Pose2D start_pose
nav_2d_msgs/Twist2D start_vel
nav_2d_msgs/Twist2D cmd_vel--dwb_msgs/Trajectory2D traj
nav_2d_msgs/Twist2D velocity
duration durationgeometry_msgs/Pose2D[] posesdwb_local_planner[dwb_msgs/LocalPlanEvaluation.msg]:
std_msgs/Header header
dwb_msgs/TrajectoryScore[] twists
dwb_msgs/Trajectory2D traj
dwb_msgs/CriticScore[] scores
string namefloat32 raw_score, scale
float32 total
uint16 best_index, worst_index

ROS Navigation Tomorrow●○nav_core2
nav_core
nav_core_adapter/local_planner_adapter●○
dwa_local_planner

DWB Example

DWB Example

github.com/locusrobotics/robot_navigation

davidvlu@gmail.com@probablydavid@LocusRobotics
LocusRobotics.com/careers