Robot and sensor APIs - osrf/mbzirc GitHub Wiki

The MBZIRC simulator provides a set of ROS2 topics for controlling robots and reading data from the sensors.

Unmanned Aerial Vehicles (UAVs)

The quadrotor and hexrotor UAVs are velocity controlled using Twist messages. The velocities are expressed in the body frame of the UAVs.

ROS Topic Description Message type
/<ROBOT_NAME>/cmd_vel Target velocity (m/s) geometry_msgs/msg/Twist

The fixed wing UAV has a service call for take-off and uses attitude controller.

ROS Service / Topic Description Message type
/<ROBOT_NAME>/cmd/tol Take-off service. Only min_pitch and altitude fields are supported mavros_msgs/CommandTOL
/<ROBOT_NAME>/cmd/attitude Attitude control. Only orientation and thrust fields are supported mavros_msgs/AttitudeTarget

See the Fixed Wing UAV page for instructions on how to control a fixed wing UAV.

Unmanned Surface Vehicle (USV)

There are two thrusters on the USV, which are controlled individually. Each thruster takes a thrust force and a joint position (steering) command.

ROS Topic Description Message type
/<ROBOT_NAME>/left/thruster/cmd_thrust Target thrust (N) std_msgs/msg/Float64
/<ROBOT_NAME>/right/thruster/cmd_thrust Target thrust (N) std_msgs/msg/Float64
/<ROBOT_NAME>/left/thruster/joint/cmd_pos Target joint position (± 1.57079 rad) std_msgs/msg/Float64
/<ROBOT_NAME>/right/thruster/joint/cmd_pos Target joint position (± 1.57079 rad) std_msgs/msg/Float64

USV Hardware Interface

The simulator also offers 2 additional APIs that match the interfaces from the physical catamaran

ROS Topic Description Message type
/cat/cmd_thrusters Actuates the voltage which feeds the motor. The input values are in the range of [-1000, 1000]. This must contains only 2 entries: the first for the left thruster and the second for the right thruster. std_msgs/msg/Int16MultiArray
/cat/cmd_pod Controls the angular position of the whole pod. The input values represents the desired angular position of each pod, expressed in hundreds of degrees, in the range of [-9000, 9000]. This must contains only 2 entries: the first for the left thruster and the second for the right thruster. std_msgs/msg/Int16MultiArray

Manipulator

The robot manipulator is mounted on the USV and uses the USV robot namespace in its topic names. It offers a position control interface.

ROS Topic Description Message type
/<ROBOT_NAME>/arm/joint/azimuth/cmd_pos Target joint position (± 2.0944 rad) std_msgs/msg/Float64
/<ROBOT_NAME>/arm/joint/shoulder/cmd_pos Target joint position (± 1.5708 rad) std_msgs/msg/Float64
/<ROBOT_NAME>/arm/joint/elbow/cmd_pos Target joint position ([-1.5708, 1.4072] rad) std_msgs/msg/Float64
/<ROBOT_NAME>/arm/joint/pitch/cmd_pos Target joint position (± 2.3562 rad) std_msgs/msg/Float64
/<ROBOT_NAME>/arm/joint/roll/cmd_pos Target joint position (± 1.5708 rad) std_msgs/msg/Float64
/<ROBOT_NAME>/arm/joint/wrist/cmd_pos Target joint position (± 6.2832 rad) std_msgs/msg/Float64

Finger Gripper

The finger style gripper also provides a position control interface.

For USV arm gripper:

ROS Topic Description Message type
/<ROBOT_NAME>/arm/gripper/joint/finger_left/cmd_pos Target joint position (rad) std_msgs/msg/Float64
/<ROBOT_NAME>/arm/gripper/joint/finger_right/cmd_pos Target joint position (rad) std_msgs/msg/Float64

For UAV gripper:

ROS Topic Description Message type
/<ROBOT_NAME>/gripper/joint/finger_left/cmd_pos Target joint position (rad) std_msgs/msg/Float64
/<ROBOT_NAME>/gripper/joint/finger_right/cmd_pos Target joint position (rad) std_msgs/msg/Float64

Suction Gripper

For USV arm gripper:

ROS Topic Description Message type
/<ROBOT_NAME>/arm/gripper/suction_on True/False to enable/disable suction std_msgs/msg/Bool

For UAV gripper:

ROS Topic Description Message type
/<ROBOT_NAME>/gripper/suction_on True/False to enable/disable suction std_msgs/msg/Bool

UAV Sensors

Each UAV has a set of base sensors: IMU, Magnetometer and air pressure sensors.

Sensor type ROS topic Message type
IMU /<ROBOT_NAME>/imu/data sensor_msgs/msg/Imu
Magnetometer /<ROBOT_NAME>/magnetic_field sensor_msgs/msg/MagneticField
Pressure sensor /<ROBOT_NAME>/air_pressure sensor_msgs/msg/FluidPressure

USV Sensors

The base USV platform has an IMU sensor.

Sensor type ROS topic Message type
IMU /<ROBOT_NAME>/imu/data sensor_msgs/msg/Imu

UAV and USV Payload Sensors

Additional perception sensors can be mounted on a number of slots on the UAV and USV. The string in the topic names are from slot0 to slot<n>, where n is the max number of slots on a UAV / USV.

Sensor type ROS topic Message type
Planar Lidar /<ROBOT_NAME>/<SLOT>/scan sensor_msgs/msg/LaserScan
3D Lidar /<ROBOT_NAME>/<SLOT>/points sensor_msgs/msg/PointCloud2
2D camera /<ROBOT_NAME>/<SLOT>/image_raw sensor_msgs/msg/Image
/<ROBOT_NAME>/<SLOT>/camera_info sensor_msgs/msg/CameraInfo
/<ROBOT_NAME>/<SLOT>/optical/image_raw sensor_msgs/msg/Image
/<ROBOT_NAME>/<SLOT>/optical/camera_info sensor_msgs/msg/CameraInfo
RGBD camera /<ROBOT_NAME>/<SLOT>/points sensor_msgs/msg/PointCloud2
/<ROBOT_NAME>/<SLOT>/camera_info sensor_msgs/msg/CameraInfo
/<ROBOT_NAME>/<SLOT>/image_raw sensor_msgs/msg/Image
/<ROBOT_NAME>/<SLOT>/optical/points sensor_msgs/msg/PointCloud2
/<ROBOT_NAME>/<SLOT>/optical/camera_info sensor_msgs/msg/CameraInfo
/<ROBOT_NAME>/<SLOT>/optical/image_raw sensor_msgs/msg/Image
RF Range /<ROBOT_NAME>/<SLOT>/rfsensor ros_ign_interfaces/msg/ParamVec

Note about the RF range sensor: There are 2 models available: mbzirc_rf_range and mbzirc_rf_long_range. However,all vehicles must use the same model. Only one RF range sensor is allowed on a vehicle. An example that shows how to parse a ParamVec message can be found here. The parameters coming from the simulator are stored in a nested map. The bridge from the simulator to ROS 2 flattens this map by appending a prefix to each group of parameters param_<NUM>. The code in the example un-flattens this into one entry per competitor platform. Since we are trying to update an entire model at once, we temporarily store the values in the "Entry" structure, to then be copied over once all parameters are parsed.

USV Payload Sensors

The simulator has a mbzirc_naive_3d_scanning_radar radar model available. You should only mount this sensor on the USV and not the UAVs.

Sensor type ROS topic Message type
Radar /<ROBOT_NAME>/<SLOT>/radar/scan radar_msgs/msg/RadarScan

Manipulator sensors

Only one sensor payload can be mounted on the wrist of the manipulator so the string in the topic names is slot0 (to be released).

The joint states of the arm are also available - note that only the position and velocity fields are populated.

Sensor type ROS topic Message type
Planar Lidar /<ROBOT_NAME>/arm/<SLOT>/scan sensor_msgs/msg/LaserScan
3D Lidar /<ROBOT_NAME>/arm/<SLOT>/points sensor_msgs/msg/PointCloud2
2D camera /<ROBOT_NAME>/arm/<SLOT>/image_raw sensor_msgs/msg/Image
/<ROBOT_NAME>/arm/<SLOT>/camera_info sensor_msgs/msg/CameraInfo
/<ROBOT_NAME>/arm/<SLOT>/optical/image_raw sensor_msgs/msg/Image
/<ROBOT_NAME>/arm/<SLOT>/optical/camera_info sensor_msgs/msg/CameraInfo
RGBD camera /<ROBOT_NAME>/arm/<SLOT>/points sensor_msgs/msg/PointCloud2
/<ROBOT_NAME>/arm/<SLOT>/camera_info sensor_msgs/msg/CameraInfo
/<ROBOT_NAME>/arm/<SLOT>/image_raw sensor_msgs/msg/Image
/<ROBOT_NAME>/arm/<SLOT>/optical/points sensor_msgs/msg/PointCloud2
/<ROBOT_NAME>/arm/<SLOT>/optical/camera_info sensor_msgs/msg/CameraInfo
/<ROBOT_NAME>/arm/<SLOT>/optical/image_raw sensor_msgs/msg/Image
Joint states /<ROBOT_NAME>/arm/joint_states sensor_msgs/msg/JointState
Force Torque /<ROBOT_NAME>/arm/wrist/wrench geometry_msgs/msg/Wrench

Finger Gripper sensors

The gripper has a force torque sensor on each of its fingers. The joint states are also available - note that only the position and velocity fields are populated. The topic name will include the arm string if the gripper is attached to the arm on the USV.

Sensor type ROS topic Message type
Force Torque /<ROBOT_NAME>/<arm>/gripper/joint/finger_left/wrench geometry_msgs/msg/Wrench
/<ROBOT_NAME>/<arm>/gripper/joint/finger_right/wrench geometry_msgs/msg/Wrench
Joint states /<ROBOT_NAME>/<arm>/gripper/joint_states sensor_msgs/msg/JointState

Suction Gripper sensors

The suction gripper provides feedback on contacts at different parts of the suction surface. True means contacts detected, false means no contacts. The topic name will include the arm string if the gripper is attached to the arm on the USV.

Sensor type ROS topic Message type
Contact /<ROBOT_NAME>/<arm>/gripper/contacts/bottom std_msgs/msg/Bool
/<ROBOT_NAME>/<arm>/gripper/contacts/center std_msgs/msg/Bool
/<ROBOT_NAME>/<arm>/gripper/contacts/left std_msgs/msg/Bool
/<ROBOT_NAME>/<arm>/gripper/contacts/right std_msgs/msg/Bool
/<ROBOT_NAME>/<arm>/gripper/contacts/top std_msgs/msg/Bool

Communication

Each robot has the ability to communicate with another robot or the base station.

ROS Topic Description Message type
/<ROBOT_NAME>/tx Send data ros_ign_interfaces/msg/Dataframe
/<ROBOT_NAME>/rx Receive data ros_ign_interfaces/msg/Dataframe

See the Communication page for instructions on how to send and receive messages between robots.

⚠️ **GitHub.com Fallback** ⚠️