The behavior planner was constructed such that the robot can perform the action from start to finish i.e. traverse the setup, stop at the bounding box, perform the necessary actions, and continue. This was done via server-client services.

As shown in Fig 1, a menu was created such that each subsystem could be controlled independently for testing as well.

The entire pipeline is shown in Fig 2.

The customized tool subsystem was integrated with the manipulation subsystem. The behavior planner was integrated with the manipulation subsystem. The behavior planner was also integrated with the explore mode of the navigation subsystem. The subsystems were modified by adding the server-client functions and operations. The dummy packages were then substituted with the real packages. A single launch file launched all the nodes, including the Stretch robot, camera, LiDAR, and aruco packages.

All the packages are launched – perception, navigation and manipulation. The perception package detects and localizes clusters, then publishes the cluster locations. All cluster locations are continuously being published. The navigation package subscribes to the cluster locations. If there are no clusters, the robot continues to traverse the environment. When a cluster location is received, the robot navigates to it. If there are multiple clusters, the closest cluster is chosen. The navigation package also publishes this chosen cluster. The manipulation package actuates the arm and lift to the location and performs the action. The behavior planner specifically controls and switches between the navigation and manipulation package depending on which actuation is needed to reach the cluster – the navigation package is first controlled, and once the base reaches the cluster, the manipulation package is controlled so that the arm and lift reach the cluster. This flow is summarized in Fig. 3.

The architecture and structural planning of the behavior planner proved to be interesting. A dummy navigation and manipulation package was created to test the performance was working before integration with the actual packages. The subsystems were modified by adding the server-client functions and operations. The dummy packages were then substituted with the real packages. Apart from constructing the entire pipeline, each subsystem had to be looked at individually to ensure clear definition and use of communication. Currently, these is sufficient communication between the behavior planner and subsystems. However, control is passed over to the subsystem until a particular task is finished. Ideally, control of the system should rest with the behavior planner entirely in case of need of emergency stops. This will have to be changed in the future. Apart from this, the behavior planner does not keep a check on any of the subsystems, so even if it had the ability to intervene, the planner would not know when to. A heartbeat or watchdog timer must be added in the future.

Upon integration, the robot started to hang quite often. Apart from this, upon continuous usage of the bot, the motors do not run at full torque. This is a big risk as the bot then stops operating/displays unpredictable behavior and has to be completely restarted once this happens. This has to be looked into and fixed on priority. Each subsystem and their integration with other subsystems will have to be inspected further to find the cause of latency.