The plan
The Components
The components themselves have been designed using a server-client architecture, either in the form of actions or services. The servers have been implemented as stand-alone packages, while the clients have been aggregated together under a single package.
The packages and their functions are as follows:
Perception: This package consists of a service that takes in a reference frame and returns a list of coordinates for each block color in that reference frame.
Pose Trajectory Controller: This package implements an action server that accepts a goal pose and moves the robot to that pose. The goal pose is in the form of position and desired yaw. The action service also accepts two Boolean values allowing the service to ignore the orientation of the goal pose (only navigate to the position) or ignore the position (in-place rotation).
Move Group Interface: This package implements an action server to use the Moveit C++ API. The server accepts a position and a Boolean. The Boolean represents whether the arm is performing a pick or place command and the position is the location of the object. The service uses predefined end-effector poses. If a pick is commanded, the arm first moves to a pre-grasp position, then to a grasp position, and finally the grippers are commanded to close. If a place is commanded, the arm moves to a place position and then opens the gripper.
Link Attacher: This package provides a service to attach a block to the end effector. This service creates a joint between the end-effector and a specified entity in the gazebo environment through a plugin. Under ideal conditions, with force control, this service would not be necessary. However, in this project, it was needed to simulate grasping of the block.
Apriltag Ros: This package provides methods to localize the locations of Apriltags in an environment. It also consists of a launch file to spawn the April tags in the gazebo environment.
Action Client: This package consists of the clients for all the services and the action servers. The clients are implemented in the form of class definitions that can be imported as dependencies into a task planning script or a state machine.
Final Project: This package aggregates launch files, assets, pattern specifications, and methods to achieve the collaborative task. The key script in this package is the state machine and task planning script that brings together dependencies and structures the logic to complete the task. The package also has helper functions to deal with frame transformations and pose estimation with respect to the April tags.
Task Planning
Localize the April tag locations in the world frame; these locations are assumed to be static.
Move towards the midpoint of the resource zone and construct the pattern.
Localize the block of choice.
Use the move group server to move to the grasp location near the block.
Use the link attached service to attach the block to the end effector.
Navigate to the drop-off point, place the block on the ground, and detach it from the end effector.
Move the base back to the resource zone.
Move to viewing position and observe the co-bot pattern.
Repeat (2) for the co-bot pattern. The resource collection point is modified and moved towards the new pattern creation station.
Move back to starting point.
Challenges
Perception
Block coordinates required in different frames
Ros-time and tf2 tree lag behind sim-time, which affects perception of the client
Arm Control and Planning
Absence of Python API for MoveIt2 move_group interface
Implemented a predefined grasp sequence using the C++ interface
Grasp planning
Grasp planning is not available for ROS2 → Predefined grasp sequence
Issue with Gazebo physics: block fails to adhere → Grasp plugin for attaching/detaching
Integration
Putting the components together
Non-trivial system design: careful combination of servers, clients, subs and pubs
Message crowding causes latency in simulation