{"id":375,"date":"2022-03-18T03:50:21","date_gmt":"2022-03-18T03:50:21","guid":{"rendered":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/?page_id=375"},"modified":"2022-12-13T04:21:26","modified_gmt":"2022-12-13T04:21:26","slug":"planning","status":"publish","type":"page","link":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/implementation\/planning\/","title":{"rendered":"Planning"},"content":{"rendered":"\n

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.<\/p>\n\n\n\n

\"\"
Figure 1. Subsystem Architecture<\/figcaption><\/figure><\/div>\n\n\n\n

Waypoint Generation<\/h3>\n\n\n\n

The first step in follower waypoint generation is to obtain the leader\u2019s 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\u2019s base pose in the follower odometry frame and the leader\u2019s path is recovered. By adding a lateral offset on the estimated leader\u2019s waypoints, we generate the follower\u2019s 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.<\/p>\n\n\n\n

\"\"
Figure 2. Lateral offset Illustration<\/figcaption><\/figure><\/div>\n\n\n\n

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.<\/p>\n\n\n\n

\"\"
Figure 3. Local Path Illustration<\/figcaption><\/figure><\/div>\n\n\n\n

Target Position Generation<\/h3>\n\n\n\n

The three steps to generate the target position of the follower:<\/p>\n\n\n\n

  1. Calculate the longitudinal offset based on the arc length of the leader\u2019s path. <\/li>
  2. Get virtual follower by taking lateral and longitudinal offset of the estimated leader. <\/li>
  3. Set the virtual follower as the targeting pose<\/li><\/ol>\n\n\n\n
    \"\"
    Figure 6. Target Position of the Follower (Virtual Follower)<\/figcaption><\/figure><\/div>\n\n\n\n

    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.<\/p>\n","protected":false},"excerpt":{"rendered":"

    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
    + Read More<\/a><\/p>\n","protected":false},"author":292,"featured_media":598,"parent":361,"menu_order":5,"comment_status":"closed","ping_status":"closed","template":"","meta":{"footnotes":""},"class_list":["post-375","page","type-page","status-publish","has-post-thumbnail","hentry"],"_links":{"self":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/pages\/375","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/pages"}],"about":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/types\/page"}],"author":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/users\/292"}],"replies":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/comments?post=375"}],"version-history":[{"count":9,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/pages\/375\/revisions"}],"predecessor-version":[{"id":825,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/pages\/375\/revisions\/825"}],"up":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/pages\/361"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/media\/598"}],"wp:attachment":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teami\/wp-json\/wp\/v2\/media?parent=375"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}