# ROS 2 Gem

The Gem and its build instructions can be found in [this repository](https://github.com/RobotecAI/o3de-ros2-gem). The
ROS 2 Gem helps to build robotic simulations with [ROS 2 / Robot Operating System](https://www.ros.org/). For an example
of use see [Warehouse Demo Project](https://github.com/RobotecAI/Ros2WarehouseDemo).

## Gem classes documentation

You can browse Doxygen-generated documentation on [Gem's GitHub page](https://robotecai.github.io/o3de-ros2-gem/).

# Components overview

- __Central Singleton__
  - ROS2SystemComponent
- __Core abstractions__
  - ROS2FrameComponent
  - ROS2SensorComponentBase
- __Sensors__
  - ROS2CameraSensorComponent
  - ROS2GNSSSensorComponent
  - ROS2IMUSensorComponent
  - ROS2LidarSensorComponent
  - ROS2Lidar2DSensorComponent
  - ROS2OdometrySensorComponent
- __Robot control__
  - ROS2RobotControlComponent
  - AckermannControlComponent
  - RigidBodyTwistControlComponent
- __Spawner__
  - ROS2SpawnerComponent
  - ROS2SpawnPointComponent
- __Vehicle dynamics__
  - VehicleModelComponent
  - WheelControllerComponent
- __Robot Import (URDF) system component__
  - ROS2RobotImporterSystemComponent
- __Clock__
  - ROS2Clock
  - RealTimeSource
  - ROS2TimeSource
  - RealTimeSource
  
# The Gem and ROS 2 ecosystem

## Supported Platforms and versions

The Gem is currently Linux-only and is being tested with ROS 2 Humble on Ubuntu 22.04 as well as ROS 2 Galactic with
Ubuntu 20.04.

It is intended to support any modern ROS 2 version, following these priorities:

- The most recent LTS version (e.g. in June 2022, [ROS 2 Humble](https://docs.ros.org/en/humble/Installation.html)).
- The most recent non-LTS version ([ROS 2 Galactic](https://docs.ros.org/en/galactic/Installation.html)).
- The always-fresh [ROS 2 Rolling](https://docs.ros.org/en/rolling/Installation.html).
- Older versions.

Currently tested and validated versions / platforms will be detailed in
the [project repository](https://github.com/RobotecAI/o3de-ros2-gem).

If you have multiple versions installed, make sure
you [source](https://docs.ros.org/en/galactic/Tutorials/Workspace/Creating-A-Workspace.html#source-the-overlay) the one
you want to use. You can check which version is sourced in your console by checking the value of `ROS_DISTRO`
environment variable (`echo $ROS_DISTRO`).

## ROS 2 Concepts

Please refer to [ROS 2 Concepts documentation](https://docs.ros.org/en/humble/Concepts.html) if you are not familiar
with how ROS 2 works.

## Structure and Communication

The Gem creates a [ROS 2 node](https://docs.ros.org/en/galactic/Tutorials/Understanding-ROS2-Nodes.html) which is
directly a part of ROS 2 ecosystem. As such, your simulation will not use any bridges to communicate and is subject to
configuration through settings such as Environment Variables. It is truly a part of the ecosystem.

Note that the simulation node is handled through `ROS2SystemComponent` - a singleton. However, you are free to create
and use your own nodes if you need more than one.

Typically, you will be creating publishers and subscriptions. This is done
through [rclcpp API](https://docs.ros2.org/galactic/api/rclcpp/classrclcpp_1_1Node.html). Example:

```
auto ros2Node = ROS2Interface::Get()->GetNode();
AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), m_MyTopic);
m_myPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(fullTopic.data(), QoS());
```

Note that QoS class is a simple wrapper
to [rclcpp::QoS](https://docs.ros2.org/galactic/api/rclcpp/classrclcpp_1_1QoS.html).

### Frames

`ROS2FrameComponent` is a representation of an interesting physical part of the robot. It handles spatio-temporal
relationship between this part and other frames of reference. It also encapsulates namespaces, which help to distinguish
between different robots and different parts of the robot, such as in case of multiple identical sensors on one robot.

All Sensors and the Robot Control Component require `ROS2FrameComponent`.

### Sensors

Sensors are Components deriving from `ROS2SensorComponentBase` specialization. They acquire data from the simulated 
environment and publish it to ROS 2 domain.

- Each sensor has a configuration, including one or more Publishers.
- Sensors publish at a given rate (frequency).
- Some sensors can be visualised.

Sensors processing and publishing depends on their base class - `ROS2SensorComponentBase` that is specialized using the 
type of event source. At this moment, there are two event sources available: 
- TickBasedSource - that signals events according to draw calls,
- PhysicsBasedSource - that signals events based on physics.

Above sources events are further handled by an adapter that is internal to `ROS2SensorComponentBase`. This way an 
additional event (adapted event) is provided to synchronize source event signals to specific frequency. If you intend to 
add your own sensor, it might be useful to look at how sensors already provided within the Gem are implemented.

Sensors can be fall into one of two categories:
- sensors which replicate real devices to some degree of realism.
- ground truth "sensors", which can be useful for development and machine learning.

### Robot Control

The Gem comes with `ROS2RobotControlComponent`, which you can use to move your robot through:

- [Twist](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/Twist.msg) messages.
- [AckermannDrive](https://index.ros.org/p/ackermann_msgs/#rolling)
  The component subscribes to these command messages on a configured topic. The topic is "cmd_vel" by default, in a
  namespace as dictated by ROS2Frame.

To make use of received command messages, use either `AckermannControlComponent` or `RigidBodyTwistControlComponent`,
depending on steering type. You can also implement your own control component or use LUA scripting to handle these
commands.

Unless scripting is used, control components should translate ROS 2 commands to events on `VehicleInputControlBus`.
These events will be handled by a [`VehicleModelComponent`](#vehicle-model) if it is present.

You can use tools such as [rqt_robot_steering](https://index.ros.org/p/rqt_robot_steering/) to move your robot with
Twist messages.
`RobotControl` is suitable to use with [ROS 2 navigation stack](https://navigation.ros.org/).

It is possible to implement your own control mechanisms with this Component.

### Vehicle Model

`VehicleModelComponent` serves the purpose of converting inputs such as target velocity, steering or acceleration to
physical forces on parts of a vehicle (robot).
`VehicleModel` has a `VehicleConfiguration` which is used to define axles, parametrize and assign wheels.

The model requires a `WheelControllerComponent` present in each wheel entity. It also uses an implementation
of `DriveModel`, which converts vehicle inputs to forces acting on steering elements and wheels.

#### Simplified drive model

The only implementation of `DriveModel` available at this moment is the `SimplifiedDriveModel`. It
uses [PID controllers](https://en.wikipedia.org/wiki/PID_controller)
from [control_toolbox](https://github.com/ros-controls/control_toolbox) package. These controllers are likely not going
to work with default parameters. The user should tune PID parameters manually. They are exposed through
the `VehicleModel` component parameters.

#### Manual control

The `VehicleModel` will handle input events with names "steering" and "accelerate". This means you can add
an [InputComponent](https://www.o3de.org/docs/user-guide/components/reference/gameplay/input/) to the same entity and
define an input map for your input devices (such as keyboard or a game pad) to control the vehicle manually.

#### Design document

To understand how vehicle dynamics works, please refer to the
[design document](../design/vehicle_dynamics/design.md) and
[class diagram](../design/vehicle_dynamics/diagrams/ROSVehicleDynamics_planned.png)

You can use tools such as [rqt_robot_steering](https://index.ros.org/p/rqt_robot_steering/) to move your robot with
Twist messages.
`RobotControl` is suitable to use with [ROS 2 navigation stack](https://navigation.ros.org/).

It is possible to implement your own control mechanisms with this Component.

### Spawner

`ROS2SpawnerComponent` handles spawning entities during simulation.
Available spawnables have to be set up as the component's field before the simulation.
User is able to define named spawn points inside the Editor. This can be done by adding `ROS2SpawnPointComponent` to a child entity of an entity with `ROS2SpawnerComponent`.

During the simulation user can access names of available spawnables and request spawning using ros2 services. 
The names of services are `/get_available_spawnable_names` and `/spawn_entity` respectivly. 
GetWorldProperties.srv and SpawnEntity.srv types are used to handle these features.
In order to request defined spawn points names user can use `/get_spawn_points_names` service with GetWorldProperties.srv type.
Detailed information about specific spawn point (e.g. pose) can be accessed using `/get_spawn_point_info` service with GetModelState.srv type.
All used services types are defined in gazebo_msgs package.

- Spawning: spawnable name should be passed in request.name and the position of entity in request.initial_pose
  - example call: `ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'robot', initial_pose: {position:{ x: 4, y: 4, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}'`
- Spawning in defined spawn point: spawnable name should be passed in request.name and the name of the spawn point in request.xml
  - example call: `ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity '{name: 'robot', xml: 'spawn_spot'}'`
- Available spawnable names access: names of available spawnables are sent in response.model_names
  - example call: `ros2 service call /get_available_spawnable_names gazebo_msgs/srv/GetWorldProperties`
- Defined spawn points' names access: names of defined points are sent in response.model_names
  - example call: `ros2 service call /get_spawn_points_names gazebo_msgs/srv/GetWorldProperties`
- Detailed spawn point info access: spawn point name should be passed in request.model_name. Defined pose is sent in response.pose.
  - example call: `ros2 service call /get_spawn_point_info gazebo_msgs/srv/GetModelState '{model_name: 'spawn_spot'}'`

## Clock
You can control how simulation time is handled, which affects timestamps on all the data coming from the simulation. `ROS2Clock` class captures this behavior and publishes, or not, current time to `\clock` topic based on a configuration setting.
To modify the clock type and enable or disable time publishing, either change configuration parameters in the `clockconfiguration.setreg` file, which is located in the `Registry` directory, or pass a parameter in the console. There are three possible settings for the sources of time:
- realtime - runs with system time but starts at 0 when the simulation starts, effectively measuring the real time elapsed since the simulation start. If you are using the Editor, then this time source starts at 0 when the Editor starts and measures how long the Editor is open.
- ros2 - time taken from the central simulation ROS 2 node. By default, this is system time, but can be configured otherwise through node options.
- simulation - this is virtual time that follows a stable step and eliminates jitter from the data. It can be configured to be slower or faster than real time. This is a default clock.

You can change the clock type via the `.setreg` file by changing the value of the ClockType parameter to one of the above. You can also enable or disable time publishing by changing the value of the PublishClock parameter.
You can change the clock type via the console by adding the `--regset=` flag to the call to the simulation binary file, e.g:  
`./build/linux_working/bin/profile/Editor --regset="/O3DE/ROS2/ClockType=realtime"`  
You can enable or disable the time publishing in the same way, e.g:  
`./build/linux_working/bin/profile/Editor --regset="/O3DE/ROS2/PublishClock=false"`  
You can modify both parameters in one command, e.g:  
`./build/linux_working/bin/profile/Editor --regset="/O3DE/ROS2/ClockType=realtime" --regset="/O3DE/ROS2/PublishClock=false`

## Handling custom ROS 2 dependencies

The ROS 2 Gem will respect your choice of [__
sourced__](https://docs.ros.org/en/galactic/Tutorials/Workspace/Creating-A-Workspace.html#source-the-overlay) ROS 2
environment. The Gem comes with a number of ROS 2 packages already included and linked, but you might want to include
additional packages in your project. To do so, use the `target_depends_on_ros2` function:

```
target_depends_on_ros2_packages(<your_target> <ros_package1> <ros_package2>)
```

in your project's `Gem/CMakeLists.txt`.

#### Example

It could be the case that you need to create new type of sensor publishing a custom message.

Lets assume your project is called `MyProject`, the custom message package is called `my_sensor_msgs` and ROS 2
workspace
`my_ros2_ws`. Take following steps:

1. Build your ROS 2 message package in a workspace as you normally would (e.g. `~/projects/my_ros2_ws`)
2. Source the overlay: `source ~/projects/my_ros2_ws/install/setup.bash`.
3. Put `target_depends_on_ros2_packages(MyProject my_sensor_msgs)` in your `Gem/CMakeLists.txt` file.
4. You can now build `MyProject` and use the new messages.

Remember to __always have your ROS 2 overlay sourced__ when building and running the project as sourcing provides
visibility of ROS 2 package paths.

# Diagram of classes

Some classes with relationships and functions are presented on this diagram. Specific sensor classes (e.g. Lidar) are
not included. Some classes are presented in sub-diagrams:

- [Vehicle Dynamics](../design/vehicle_dynamics/diagrams/ROSVehicleDynamics_planned.png)

![classes diagram](static/diagrams/diagram_ros2_gem.png)