ROS 2 API for controlling the Origin One
The velocity of the Origin One can be controlled via various different inputs, so that you as a user may switch on the fly from some from a computed velocity to a velocity set by the Origin's remote controller or some tele-operated input. The overall idea of this functionality is that multiple velocity inputs are supported and that, via a service call, one of these velocities is forwarded to the robot as the actual reference velocity. This functionality is implemented in the cmd_vel_controller.
The control mode is internal state of the cmd_vel_controller determines which input velocity, i.e., /robot/cmd_vel_XXX, is used to drive the Origin One. The control mode works like a state machine, where the service call /set_control_mode triggers the state machine to switch between different modes of operation. However, only certain transitions are allowed per mode. By default the robot starts in the NONE mode of operation, from which every transition is allowed. There are 2 control modes in which the robot is controlled by an operator or user (a.k.a. manually). These modes are called MANUAL (switching to /cmd_vel_joy) and TELEOPS (switching to /cmd_vel_teleops, this is currently not implemented). The other modes are used for driving the robot autonomously, which are AUTONOMOUS (switching to /cmd_vel_aut), and USER (switching to /cmd_vel_user). Therefore, if you want your application to be able to control the Origin One, you must set the control mode to the USER and publish to the /cmd_vel_user topic. When the robot is driving in one of its autonomous modes, transitions to the other autonomous mode and the manual modes are allowed. This allows human intervention when the robot is driving autonomously.
When the robot is driving in any of the manual modes, transitions to the autonomous modes are not allowed. This prevents the robot from taking over control while a user is driving the robot manually. Instead, any manual control mode should first be released. Using the service /reset_control_mode the control mode can be reset to the NONE mode of operation, from which any transition is allowed.
Alternatively, after taking over control from the robot, you may want to return the control back to the robot. You can do this by calling the service /set_previous_control_mode. This way, you can always return to the original control mode, without having to know in which mode the robot was actually operating before taking control. When calling this service call, you have to provide the mode you are trying to return from.
Velocity topics
The topics for interfacing with the cmd_vel_controller are summarized in the table below. A more detailed account per topic is presented later on this page.
| Topic | Format | Type | Function |
|---|---|---|---|
| /robot/cmd_vel_user | geometry_msgs/Twist | subscriber | Topic via which a user may command velocity. |
| /robot/cmd_vel_aut | geometry_msgs/Twist | subscriber | Topic that is used by Avular's autopilot for commanding velocity Do not publish to this topic manually. |
| /robot/cmd_vel_joy | geometry_msgs/Twist | subscriber | Topic that is used by the Origin's remote controller for commanding velocity Do not publish to this topic manually. |
| /robot/cmd_vel_teleop | geometry_msgs/Twist | subscriber | Topic that is used by Avular's (future) teleoperated application for commanding velocity over a cellular network Do not publish to this topic manually. |
| /robot/cmd_vel | geometry_msgs/Twist | publisher | Topic that is directly used by the robot itself for acquiring the robot's desired velocity Do not publish to this topic manually. |
| /robot/cmd_vel_controller/control_mode | origin_msgs/ControlMode | publisher | Topic that indicates the current velocity control mode |
Velocity
Each of the velocity topics above has the following characteristics:
Message type: geometry_msgs/Twist
Frame: odom & base_link
Frequency: depends on the publisher
Data: longitudinal velocity and yaw rate setpoint
Control mode
The topic /robot/cmd_vel_controller/control_mode has the following characteristics:
Message type: origin_msgs/ControlMode
Frame: n/a
Frequency: Latching and on change
Data: mode
Velocity services
In order to be able to switch the control mode there are a number of services available, which are listed in the table below. These services make use of a mode message definition, which is defined as follows:
| mode (X) | Meaning |
|---|---|
| 0 | NONE |
| 10 | MANUAL |
| 20 | AUTONOMOUS |
| 30 | USER |
| Service | Format | Request variable | Function |
|---|---|---|---|
| /robot/cmd_vel_controller/set_control_mode | origin_msgs/srv/SetControlMode | mode [mode] | Service to call for a switch to mode 'user', 'autonomous', 'manual' or 'teleops'. |
| /robot/cmd_vel_controller/reset_control_mode | origin_msgs/srv/ResetControlMode | n/a | Service to call for a switch to mode 'none'. |
| /robot/cmd_vel_controller/previous_control_mode | origin_msgs/srv/ReturnControlMode | mode_from [mode] | Service to call for a switch to the previous mode whilst the mode is currently at some mode_from. |
You can then use, for example ros2 service call /robot/cmd_vel_controller/set_control_mode origin_msgs/srv/SetControlMode "{ mode: { mode: X } }" to request a transition to the control mode X, which will be granted if the control mode has been released, i.e., when ros2 topic echo /origin/cmd_vel_controller/control_mode returns that the current mode is NONE. All service calls above return a success bool to indicate whether the requested transition has been completed successfully.
Example on setting a reference velocity
The following example is a copy of the steps you need to take from the moment you've set up the ssh-connection with the Origin until the mode is switched to MANUAL (10) successfully:
$ ros2 service call /robot/cmd_vel_controller/set_control_mode origin_msgs/srv/SetControlMode "{ mode: { mode: 10 } }"
requester: making request: origin_msgs.srv.SetControlMode_Request(mode=origin_msgs.msg.ControlMode(mode=10))
response:
origin_msgs.srv.SetControlMode_Response(success=True, message='Control mode set to manual.')
If you now want to reset the control mode to NONE (0), you can use the /reset_control_mode service as described in the following example (note that you'll have to give the mode it is coming from as input):
$ ros2 service call /robot/cmd_vel_controller/reset_control_mode origin_msgs/srv/ReturnControlMode "{ mode_from: { mode: 10 } }"
requester: making request: origin_msgs.srv.ReturnControlMode_Request(mode_from=origin_msgs.msg.ControlMode(mode=10))
response:
origin_msgs.srv.ReturnControlMode_Response(success=True, message='Control mode reset.')
In the next example, we want to let the Origin drive with a velocity of 0.5 [m/s] in forward direction and with a rotational velocity of 0.5 [rad/s]. Note that to do so, you will have to set the control mode to USER (30) and that you'll have to specify a rate/frequency at which the topic has to be published. The suggested frequency is 10Hz.
$ ros2 service call /robot/cmd_vel_controller/set_control_mode origin_msgs/srv/SetControlMode "{ mode: { mode: 30 } }"
requester: making request: origin_msgs.srv.SetControlMode_Request(mode=origin_msgs.msg.ControlMode(mode=30))
response:
origin_msgs.srv.SetControlMode_Response(success=True, message='Control mode set to user.')
$ ros2 topic pub /robot/cmd_vel_user geometry_msgs/msg/Twist "{linear: {x: 0.5}, angular: {z: 0.5}}" --rate 10
publisher: beginning loop
publishing #1: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.5, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.5))