Planning

The follower motion planning consists of two parts: waypoints generation and follower target position generation. The inputs for the desired waypoints generation of the follower are the relative pose estimated from perception subsystem and the follower pose estimated by the localization subsystem. Then the follower target pose is selected to have a longitudinal offset from leader’s position. A flowchart of this subsystem is shown in Figure 1.

Figure 1. Subsystem Architecture

Waypoint Generation

The first step in follower waypoint generation is to obtain the leader’s waypoints. The output of the perception subsystem is the tf transform between the camera frame and the leader base frame. Based on tf transforms, we compute the leader’s base pose in the follower odometry frame and the leader’s path is recovered. By adding a lateral offset on the estimated leader’s waypoints, we generate the follower’s waypoints, which form the follower path. There is an initial gap between the leader and the follower. We interpolate n waypoints from the initial position of the follower to the first waypoint among the generated waypoints. This is illustrated in Figure 2.

Figure 2. Lateral offset Illustration

We compute a KDT tree at every time step to find the waypoint closest to the current position in the follower desired path. Then, we take some waypoints from the closest one to form a local desired path which will be published to the lateral controller. This is illustrated in Figure 3.

Figure 3. Local Path Illustration

Target Position Generation

The three steps to generate the target position of the follower:

  1. Calculate the longitudinal offset based on the arc length of the leader’s path.
  2. Get virtual follower by taking lateral and longitudinal offset of the estimated leader.
  3. Set the virtual follower as the targeting pose
Figure 6. Target Position of the Follower (Virtual Follower)

At each time step, the follower target position is published through ROS topic to the longitudinal controller, so that the longitudinal controller can output the velocity commands for the follower to maintain a constant longotudinal distance from the leader.