The system begins at the bottom of the pictured cyberphysical architecture with the sensing stack. All of the sensor data is used to build a map of the environment and localize the robot within that map. The logical flow then moves to the ArUco tag detection block in the center. If an ArUco marker is not detected, the system enters the goal generation stack. The system detects the center lane line, and uses this line to impose artificial obstacles on the map on the lane edges. This ensures that all of the plans generated by the navigation subsystem will only be drawn within the lane.
Given this map with artificial boundaries, the goal generation stack generates a waypoint that lies ahead of the robot within the lane. This waypoint is then fed into the timed-elastic-band (TEB) local planner in the navigation stack, along with the map output from the SLAM stack. The TEB planner outputs a trajectory that gets fed into the ROS movebase package and gets converted into commands to the Husky’s velocity controller. The velocity controller runs a PID loop to ensure the robot properly tracks the path. This loop repeats and the robot continues traversing the garage, until an ArUco marker is detected, signifying that we have found a free parking spot.
We then move to the parking stack, which generates a desired parking pose based on the detected position of the ArUco marker. The parking system also generates two intermediate waypoints between the current robot pose and the final parking pose, ensuring a smooth turn into the spot. These three waypoints get fed into the planner, and the system generates and executes a path through the two initial waypoints to the final parking pose. When the robot is sufficiently close to the goal, the system will successfully terminate.