Robotics and Autonomous Systems Group, Research Engineering Facility, Research Infrastructure Queensland University of Technology
This repository comprises a preliminary set of packages based on which the robotic arm manipulation system of CGRAS 2025 may be developed. Its initial use-case is to offer a simulated environment for researching optimal measurements of the image capture arena, the key design element for CGRAS 2025.
The major components in the packages include:
- The robot arm manipulation for image acquisition system, including the core system, a server ROS interface for the core system, and a GUI demo ROS client for interactively sending tasks to the core system.
- The definition of the fixed robot and the arena (i.e. the world).
- A dockerfile for running the system in a container.
The robotic arm manipulation for image acquisition system has been re-developed from scratch, exploiting the benefits of the following REF-RASG in-house developed and third-party packages:
- Moveit Motion Planning Framework - Version 1
- The Arm Commander - Release 0.2.0: REF-RASG developed package for enhancing the Python Moveit API with support for asynchronous IO, motion specification, frame-referenced motions, constraint specification and others.
- The Task Trees Framework - Release 0.2.0: REF-RASG developed package for accelerating building of behaviour trees for robot arm manipulation.
cgras_robot # the root of this repository is also the root of the packages
├── cgras_moveit_config # configuration files generated by the moveit setup assistant
├── cgras_scene # definitions of the robot (the xacro and the meshes files) and the scene (the world) of operation
├── cgras_robot_manipulator # the software including the CGRAS task trees manager, the ROS server, a demo GUI ROS client, and application configuration
├──├── action # files containing definition of actions
├──├── launch # ROS launch files (not used)
├──├── nodes # ROS node files
├──├── srv # files containing definition of services
├──├── src # the root of the Python source files
├──├──├── cgras # the cgras module shared between ROS packages
├──├──├──├── robot # the cgras module shared between ROS packages
├──├──├──├── tools # the cgras module shared between ROS packages
├──├── CMakeLists.txt # cmake for the cgras_robot_manipulator package
├──├── package.xml # package definition for the cgras_robot_manipulator package
├──├── setup.py # the pythons setup for the cgras_robot_manipulator package
├── README.md # the README file for this repository
├── docs # assets for README.md and other documentation files
├── Dockerfile # the Dockerfile for building an image for running simulations based on the software
├── docker-compose.yaml # config file for docker compose service definitionsThe three ROS packages listed above are to be placed inside a catkin workspace.
root
├── build # target folder for workspace build
├── devel # target folder for workspace devel
├── src # root of the packages in this workspace
├──├── arm_commander
├──├── rviz_marker_tools
├──├── task_trees
├──├── cgras_robot
├──├──├── cgras_moveit_config # configuration files generated by the moveit setup assistant
├──├──├── cgras_scene # definitions of the robot and the scene (the world) of operation
├──├──├── cgras_robot_manipulator
├──├──├── README.md
├──├──├── docs
├──├──├── Dockerfile
├──├──├── docker-compose.yaml
├──├── requirements.txtFollow the instruction below for a relatively straightforward procedure that involves cloning this repository locally in a catkin workspace, and setup of a adequate execution environment in the form of a docker container.
- Moveit 1
- ROS 1 Noetic
- Python 3.8 or above
- The Arm Commander (REF-RASG package)
- The Task Trees (REF-RASG package)
- The RViz Marker Tools (optional) (REF-RASG package)
Create a catkin workspace.
mkdir -p ~/cgras2025_ws/src
cd ~/cgras2025_ws/srcClone this repository to the src directory.
git clone git@github.com:REF-RAS/cgras2025_arena.gitThe docker-compose.yaml file and the Dockerfile for building the environment is in the root directory of this repository. It has been tested with Docker 25.0.3 and Ubuntu 20.04. Download the file to your local disk and change directory to where the file is located.
docker compose build cgras2025If docker compose is not install, try the following.
docker build --tag cgras2025 - < DockerfileThe image building may take some time. When the build is completed, execute the following to check if the image arm_commander is there.
docker image lsExecute below to allow RViz in the container to display a GUI on the host.
xhost +Start a container based on the image. If docker compose is installed, execute the following in the same folder as the file docker-compose.yaml.
docker compose up cgras2025 On another command window, execute the following in the same folder as the file docker-compose.yaml.
docker compose exec cgras2025 bashIf docker compose is not installed, execute the following. Replace XXX with the path to the catkin workspace on your host computer.
docker run -it --net=host \
--env="DISPLAY" \
--env="QT_X11_NO_MITSHM=1" \
--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
--volume="/home/XXX/cgras2025_ws/src/cgras2025_arena:/home/qcr/cgras2025_ws/src/cgras_robot:rw" cgras2025On the prompt resulting from the previous command, attempt to launch the ur20 moveit simulated scene that has been pre-built in the repository.
cd ~/cgras2025_ws
source devel/setup.bash
roslaunch cgras_moveit_config demo.launch
The following will be displayed in the launch rviz application.
An adequate understanding of the Task Trees and the Arm Commander packages is required as the system is structured according to the Task Trees Framework.
The program files are found under cgras_robot_manipulator/src/cgras/robot.
The Core System
The first program file task_trees_manager_cgras.py contains the setup procedure for the core system.
| Files | Remarks | Basis |
|---|---|---|
| task_trees_manager_cgras.py | Specialization of the task trees manager for CGRAS 2025 | task_trees.task_trees_manager.TaskTreesManager |
| behaviours_cgras.py | Definitions of new behaviours specific to CGRAS 2025 | The base classes of task_trees.behaviours_base and task_trees.behaviours_move |
| task_scene_cgras.py | Specialization of the task_scene.py for CGRAS 2025 | task_trees.task_scene.Scene |
| tasks_cgras.py | Definitions of the tasks (classes) for the task trees manager | task_trees.task_trees_manager.BasicTask |
| task_scene.yaml | Configurations of the CGRAS 2025 scene |
The Test Program
The program file run_test.py contains scripts for testing the CGRAS 2025 task trees manager. It submits a test sequence of tasks to the core system.
The ROS Server
The program file ros_server.py is the executable program for starting the ROS server interface for the core system.
The ROS Client
The program file ros_client.py is the executable program for starting a GUI ROS client program for testing the ROS server interactively.
The branches are defined in the program file task_trees_manager_cgras.py. The branches in the initialize and the priority subtrees are incomplete and to be customized.
| Task Trees Branch Type | Related Tasks | Builder Function | Remarks | | ---------- | ------- | ----- | | Initialize | N/A | create_initialize_branch | executed when the task trees is first started up | | Priority | N/A | create_timeout_branch | executed when a timeout happens while waiting for new tasks | | Task | CalibrateTask | create_calibrate_task_branch | | | Task | MoveNamedJointPoseTask | create_move_named_joint_pose_task_branch | | | Task | N/A | create_tank_not_calibrated_trap_branch | triggerd to prevent executing the next two task branches if the tank_id not calibrated | | Task | MoveCapturePoseTask | create_move_capture_pose_task_branch | | | Task | MoveReadRFIDTagTask | create_move_read_rfid_task_branch | |
Several implementated tasks have been implemented in this demo system for illustration the use of the Task Trees Framework.
This task involves a sequence of behaviours that progressively move the tool to the prescribed pose for robot-to-tank-tag calibration. The prescribed pose for each tank is obtaine from the configuration file task_scene.yaml.
The tank_id specifies the tank to be involved in the calibration. The following illustrates calibration of another tank.
This task involves a sequence of behaviours that progressively move the tool to a named joint pose as specified in the configuration file.
This task involves a sequence of behaviours that progressively move the tool to a image capture pose, and in transit, if needed, moving to poses to enable reading of the RFID tag at the target placement grid cell.
The behaviours include those enable the tool to move between two tanks, as illustrated in the following demo.
This task involves a sequence of behaviours that progressively move the tool to poses enabling the RFID reader to read the tag at the target placement grid cell.
It is expected that the core system is to be enhanced by defining new tasks. The following rudimentarily lists the steps required.
- Specify the name and the input parameters of the task.
- Add a new task class in
tasks_cgras.pybased on the name and the input parameters. The input parameters should be specified through the constructor and getter functions may be defined for facilitating their access. - Create one or more new behaviour classes in
behaviours_cgras.pyif the pre-defined classes of the Task Trees Framework do not satisfy the requirement. The behaviour classes can access the current task and other model variables of the task tree managers through thepytreeblackboard. - Create a new builder function in
task_trees_manager_cgras.py. Within the function body, define apytreebranch that contains the required subtree of behaviours for the task. - Define supporting functions in
task_trees_manager_cgras.pyfor the behaviour subtree of the new task. The type of supporting functions include accessing the task parameter, testing conditions and computing poses, etc. These supporting functions are used as parameters (i.e. function references) in the behaviour subtree definition. - Add the task subtree definition to the task trees manager. In the constructor of
CGRASTaskTreesManagerintask_trees_manager_cgras.py, add the following line before the_install_bt_and_spincall.
self._add_task_branch(self.create_new_task_branch(), NewTask)
...
self._install_bt_and_spin(self.bt, spin_period_ms)The file task_scene.yaml specifies the scene parameters associated with the tasks.
The file task_scene.yaml is for application level configuration. Note that the package cgras_scene specifies the parameters that are essential for the Moveit Motion Planner Framework to function. These two play important and different roles in the core system.
The file defines two task scenes, namely, root and tank_operation. The root scene parameters are divided into the following branches.
The Root Scene
| Branches | Remarks |
|---|---|
| named_poses | The named joint poses |
| regions | 2D or 3D regions of task significance |
| positions | point, line, or plane of task significance |
| num_tanks | number of tank in the arena |
| robot_to_calibrate_tag_tf | information for calibration of each tank |
| measures_of_tanks | measurements of each tank |
| measures_of_tile | measurement of the tiles |
The Root Scene: robot_to_calibrate_tag_tf
Each tank has its own sub-branch.
| Keys | Remarks |
|---|---|
| joints | The joint pose hinting the initial calibration pose |
| xyzrpy | The pose in xyzrpy hinting the initial calibration pose (ignored if joints is defined) |
| frame | The frame from which xyzrpy is defined (ignore if joints is defined) |
| size | The size of the apriltag |
| simulated_xyzrpy | The result of simulated calibration |
The Root Scene: measures_of_tanks
Each tank has its own sub-branch. All poses are to be interpreted in the frame of the tank.
| Keys | Remarks |
|---|---|
| size | The size of the tank |
| placement_grid_dim | The number of placement cells in both axes |
| placement_grid_size | The size of each placement cell |
| tag_to_tank_position | The pose tf from the tag to the tank centre |
| tank_to_tile00_position | The pose tf from the centre of placement grid index (0, 0) to the tank centre |
| workspace_3d | the workspace defined for the tank |
The Root Scene: measures_of_tile
All tiles are assumed to have the same measurements. All poses are to be interpreted in the frame of the tile.
| Keys | Remarks |
|---|---|
| size | The size of a tile |
| capture_grid_dim | The number of image capture positions in both axes |
| rfid_positions | A list of one or more position where RFID tag is to be read |
| capture_positions | The definitions of all the capture positions |
The Tank Operation Scene
The second scene is the tank_operation. This scene is applicable to all the tanks in the arena. All poses are to be interpreted in the frame of the tank.
| Branches | Remarks |
|---|---|
| positions | point, line, or plane of task significance |
| rotations | orientations of task significance |
The ROS server interface offers an action server for executing move tasks and a service server for querying system parameters. It is implemented in the file ros_server.py and the main class is CGRASRobotAgent.
Make sure that the cgras moveit simulator has already been started.
roslaunch cgras_moveit_config demo.launchThen start the ROS server for the robot arm manipulator for image acquisition system.
rosrun cgras_robot_manipulator ros_server.pyThe topic of the action server that receives various goals for the core system is customizable through ROS params and constructor parameters of the class CGRASRobotAgent. Refer to the following table for the names of the parameters. Note that the names of the ROS parameter and the corresponding constructor keyword parameters are the same.
| ROS Param Name | Default | Remarks |
|---|---|---|
| move_action_server_topic | /cgras/robot/move | Executing tasks in the core system |
The goal definition of the action server of /cgras/robot/move is listed below.
### GOAL DEFINITION
int8 type
int32 tank_id
int32 placement_x
int32 placement_y
int32 capture_x
int32 capture_y
The type specifies the task to be executed as the action goal and some or all of the remaining parameters may be needed for each type.
| Type | Corresponding tasks | Parameters required | Return values |
|---|---|---|---|
| MOVE_TO_CAPTURE = 1 | MoveCapturePoseTask |
tank_id, placement_x, placement_y, capture_x, capture_y | Nil |
| MOVE_TO_STOW = 2 | MoveNamedJointPoseTask |
Nil | Nil |
| MOVE_TO_CALIBRATE_TAG = 3 | CalibrateTask |
tank_id | The tile_id of the target placement index |
| MOVE_TO_READ_RFID_TAG = 4 | MoveReadRFIDTagTask |
tank_id, placement_x, placement_y | The tile_id of the target placement index |
The topic of the service server is customizable through ROS params and constructor parameters of the class CGRASRobotAgent. Refer to the following table for the names of the parameters. Note that the names of the ROS parameter and the corresponding constructor keyword parameters are the same.
| ROS Param Name | Default | Remarks |
|---|---|---|
| arena_params_srv_topic | /cgras/robot/arena_params | Querying parameters of the system |
The request definition of the service server of /cgras/robot/arena_params is listed below.
int8 the_query
int8 tank_id
int8 placement_x
int8 placement_y
int8 capture_x
int8 capture_y
The the_query specifies the key for the query. Some or all of the remaining parameters may be needed for each query type.
| Query Type | Remarks | Parameters required | Return values |
|---|---|---|---|
| NUM_TANKS=0 | The number of tanks | Nil | The data index 0 of the results array |
| PLACEMENT_GRID_DIM = 1 | The dimension of the placement grid for a tank | tank_id | The data index 0 and 1 of the results array contains the x and y of the dimension respectively |
| CAPTURE_GRID_DIM = 2 | The dimension of the image capture grid for a tank | Nil | The data index 0 and 1 of the results array contains the x and y of the dimension respectively |
| IS_CALIBRATED = 3 | Whether a tank has been calibrated | tank_id | The data index 0 of the results array contains the result as an integer |
| TILE_ID = 4 | The tile_id of a placement grid cell | tank_id, placement_x, placement_y | The tile_id_array and the orientation_array array contains the tile_id and the orientation of one or more placement grid cell |
| CAPTURE_POS = 5 | The capture position of an image capture position | capture_x, capture_y | The fresults contains the capture position offset |
The last query type TILE_ID supports wildcard specification for tank_id, placement_x, placement_y. The wildcard value is numeric -1, which implies all. For example, setting tank_id to -1 implies querying the tile_id for all tanks. The number of tile_id and orientation returned will be the same as the resulting placement cells included by the wildcard specification. The results returned will be ordered first by the tank_id, placement_y and finally placement_x.
For example, querying TILE_ID with parmaters tank_id=-1, placement_y=1, placement_x=-1 will return the results for all the placement grid locations in all tanks, the row index 1 and all columns in the tanks. If there are 2 tanks and the placement grid dimension is 9 x 3 and it is same for all tanks, then the length of the returning results is 2 x 9 = 18.
A ROS client ros_client.py is provided as a reference implementation for illustrating the use of the ROS server.
Start the client with the following script.
rosrun cgras_robot_manipulator ros_client.pyThe initial screen offers buttons for calibrating the robot-to-tank-pose-tf for a tank, and one for cancelling the current goal.
The client obtains the number of tanks through the service server and provides calibrate buttons accordingly.
Pressing one of the calibrate buttons will initiate a calibration task. At completion, the client will provide an interactive map showing the placement grid and the image capture grid. The dimensions are again obtained through the service server.
The interactive map shows the placement grid cells, each of which has a yellow strip on top indicating a read of the RFID tag and the small cells indicating image capture positions. Clicking on one of the strips or the cells will initiate a MOVE_TO_READ_RFID_TAG or MOVE_TO_CAPTURE action goal.
The core system uses the rviz_tool to publish markers of significance to the planning of behaviours. The following shows some examples. Logical sets of markers may be set visible or hidden on Rviz during exploration.
The following displays the regions and positions under the root scene. For example, the large purple box represents the regions.workspace 3D region. The yellow plane represents positions.safe_zplane.
The class task_trees.scene_to_rviz.SceneToRviz (a subclass of task_trees.tools.rviz_tools.RVizVisualizer) offers useful functions and services for publishing visualization markers. Refer to the class task_trees_manager_cgras.py for the code that create the markers. The lines are excerpted below.
self.scene_to_rviz:SceneToRViz = SceneToRViz(self.the_scene, arm_commander.get_world_reference_frame(), False)
self.scene_to_rviz.clear_all_markers_in_rviz(arm_commander.get_world_reference_frame())
self.scene_to_rviz.display_bbox_regions('root.regions', rgba=[1.0, 0.0, 1.0, 0.2])
self.scene_to_rviz.display_positions('root.positions', rgba=[1.0, 1.0, 0.0, 0.8]) The following shows the markers associated with one of the tanks. The two light yellow planes on top of the tank represent tank_operation.hover and tank_operation.submerged.
Refer to the end of the update function in the class behaviours_cgras.SimCalibrate for the code that generates the markers, which are excerpted as follows.
rviz.display_rotations('tank_operation.rotations', arrow_length=0.5, rgba=[0.8, 0.0, 0.5, 0.8], reference_frame=target_pose.tank_frame)
rviz.display_positions('tank_operation.positions', rgba=[1.0, 1.0, 0.0, 0.2], reference_frame=target_pose.tank_frame) The example below shows one of the arrows indicating the keys under tank_operation.rotations. The arrow is found next to the frame tank_1.
Dr Andrew Lui, Senior Research Engineer
Robotics and Autonomous Systems, Research Engineering Facility
Research Infrastructure
Queensland University of Technology
First version: 15 Jul 2024 Latest update: Jul 2024