Simulation Interfaces to Development

[michalpelka]

What does this PR do?

This PR is implementation of two RFCs:
o3de/sig-simulation#95
ros-infrastructure/rep#410

Those are ROS 2 interface, implemented using ROS 2 services and ROS 2 actions.

This PR mainly :

• Adds new gems : SimulationInterfaces and Simulation InterfacesROS2.
  The SimulationInterfaces is responsible for implementing features from #410 against O3DE's API (mainly AzPhysics). InterfacesROS2 is enabler to ROS 2 interfaces.
• Some changes to ROS 2 API (they need to be tested by reviewer against existing projects):
  • We extended ITimeSource interface to allow setting time. It was implemented for SimulationTimeSource
  • We fixed the bug The ROS2CameraSensorComponent was reflected twice #851 in this PR's branch
  • We added RPISystem from GetRequiredServices to GetDependentServices enabling running ROS 2 gem in test environment.

How was this PR tested?

• We achieve satisfactory coverage with Unit / Integration tests using GTest and GMock.
• We also wrote a GUI tooling that emulates SimulationInterface client - https://github.com/michalpelka/q_simulation_interfaces

[adamdbrw]

Great work on this PR everyone!
Part of review, to be continued..

[norbertprokopiuk]

I went through all files and I marked thing that I missed during previous reviews

[norbertprokopiuk]

There should be link to the documentation when it is ready

[norbertprokopiuk]

Most of comments were resolved in the PR #889. I would kindly invite reviewers to check out those changes

[jhanca-robotecai]

Code review

Mainly nits, that can be ignored.

Tests

One test is failing on my machine:

cwd = /home/jhanca/devroot/projects/TestProject/build/linux/External/SimulationInterfaces-b8b19630/Code
LIB: /home/jhanca/devroot/projects/TestProject/build/linux/bin/profile/libSimulationInterfaces.ROS2Tests.so
Loading: /home/jhanca/devroot/projects/TestProject/build/linux/bin/profile/libSimulationInterfaces.ROS2Tests.so
OKAY Library loaded: /home/jhanca/devroot/projects/TestProject/build/linux/bin/profile/libSimulationInterfaces.ROS2Tests.so
OKAY Symbol found: AzRunUnitTests
Note: Google Test filter = -*SUITE_smoke*:*SUITE_periodic*:*SUITE_benchmark*:*SUITE_sandbox*:*SUITE_awsi*
[==========] Running 17 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 17 tests from SimulationInterfaceROS2TestFixture
[ RUN      ] SimulationInterfaceROS2TestFixture.TestIfROS2NodeIsAvailable
[       OK ] SimulationInterfaceROS2TestFixture.TestIfROS2NodeIsAvailable (0 ms)
[ RUN      ] SimulationInterfaceROS2TestFixture.SmokeTestROS2Domain
/home/jhanca/devroot/devo3de/o3de-extras/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp:130: Failure
Expected equality of these values:
  receivedMsgs
    Which is: 9
  10
Did not receive all messages.

Functional tests

I tried to test the implementation from the perspective of a person who does not know the details of the standard, does not know the details of O3DE and who was too lazy to study the details. Instead, I started playing with all available interfaces and being surprised with the effect (a typical user).

Spawning an entity:

• call get_spawnables to get the available names
• get product_asset:///rosbot_velodyne.spawnable uri example
• spawn product_asset:///rosbot_velodyne.spawnable -> OK; anything else not OK

question: Should we require product_asset:///? What's wrong with rosbot_velodyne.spawnable or rosbot_velodyne? Can we have anything else than product_asset:/// and anything else than .spawnable?

Getting entities:

• create a level with rosbot.spawnable
• spawn product_asset:///rosbot.spawnable as asdf
• call get_entities

issue: the root entity body_link in original robot is named asdf in the spawned robot instead of asdf_body_link (which would follow other names, e.g. asdf_imu_link vs imu_link)

Setting the entity state:

• spawn product_asset:///prefabs/warehouse_storage/storage_rack3.spawnableas asdf at (10, 0, 2)
• reset level
• spawn product_asset:///prefabs/warehouse_storage/storage_rack3.spawnableas fdsa at (12, 0, 0)
• set_state fdsa position (10, 0, 2)

question: why do we move only the root entity, rather than the whole entity? I understand we want to be able to move parts of the robot (sensors?) around, but this makes the tool useless in other cases

• spawn product_asset:///rosbot_velodyne.spawnable as asdf
• set the twist->linear->x = 0.1 to asdf

question: should this be somewhat equivalent to calling cmd_vel x = 0.1? if so, it is not, as the robot moves to (0, 0, 0) and it does not move anymore (even though get_entity_state shows twist->linear->x = 0.1

Getting the entity state:

• spawn product_asset:///rosbot_velodyne.spawnable as asdf
• set the twist->linear x = 0.1 to asdf
• get_entity_state of asdf

question: what is the expected value? the robot moved (see above), but the position did not change

Despawning entities

• spawn product_asset:///prefabs/warehouse_storage/storage_rack3.spawnableas fdsa at (12, 0, 0)
• get entities to see what is in there
• try to remove Rack entity (part of the level) -> FAIL with error 4 Request could not be completed successfully even though feature is supported. Is it supported?
• try to remove fdsa-> OK

question: how do we know which entities can be removed and which can't?

• spawn product_asset:///rosbot_velodyne.spawnable as asdf
• despawn asdf_velodyne_puck_link

question: what is the expected outcome? I would expect the lidar entity to disappear, but the whole robot does.

btw. why this is not called despawn_entity, but delete_entity?

Getting simulation features

question: shouldn't we return the list in the increasing order rather than unordered? It would be easier to read.

Simulation state

comment: default state == 0 (stopped) is quite counter-intuitive to what is typically seen; starting the simulation and seeing that nothing happens (publishing messages to topics such as /cmd_vel have no result) can surprise even experienced users

No comments to the remaining interfaces.

[jhanca-robotecai]

Why not DespawnEntity()?

[michalpelka]

question: Should we require product_asset:///? What's wrong with rosbot_velodyne.spawnable or rosbot_velodyne? Can we have anything else than product_asset:/// and anything else than .spawnable?

Yes we need product_asset:/// since ros-simulation/simulation_interfaces requires URI.
We proposed URI that defines available spawnables in cache directory for GameLauncher.
I would love to see other sources in the future.
Note that other products has multiple sources for things to put in simulation.
Gazebo can use package:///robot_package/description/robot.urdf.xacro, or repositories like fuel.
So I am not a fan dropping from URI ".spawnable" or product assets, it is not future-proof.

question: why do we move only the root entity, rather than the whole entity? I understand we want to be able to move parts of the robot (sensors?) around, but this makes the tool useless in other cases

It allows to move objects that are imported by SDF / URDF importer.
If the prefab designer, does not use URDF / SDF workflow, he should mimic the output.
I've described it in https://github.com/RobotecAI/o3de.org/blob/pantosz/simulation_interfaces/content/docs/user-guide/interactivity/robotics/simulation-interfaces.md#motivation.
What I can see in the example you posted, is someone who shot himself in the foot - the root entity has no Physics components, so simulation interfaces could not get handle to that object.
We elected to use this approach since well-designed physical prefabs (a.k.a. robots) will behave correctly when moving it around by applying state to their root.
Note, we cannot apply state to non-physical entities, as you suggest in your question, since, it will cause secondary issues:

• no speed support,
• it won't be effective for children's physical entities (especially simulated rigid bodies).

question: should this be somewhat equivalent to calling cmd_vel x = 0.1? if so, it is not, as the robot moves to (0, 0, 0) and it does not move anymore (even though get_entity_state shows twist->linear->x = 0.1

Standard ros-simulation/simulation_interfaces defines a state as a set of values:

• transform (unit length quaternion + vector of size 3 of real numbers)
• angular velocity ( vector of size 3 of real numbers )
• linear velocity ( vector of size 3 of real numbers )
• angular acceleration ( vector of size 3 of real numbers )
• linear acceleration ( vector of size 3 of real numbers )

We silently ignore acceleration (we should warn about that), but we cannot choose what part of state to apply or not.
So answering you question it is not equivalent to setting cmd_vel x = 0.1.
In cmd_vel we set desired velocity to simulated robot controller, but interface you tested is more-or-less Hand of God.

question: what is the expected value? the robot moved (see above), but the position did not change

I can see two issues in your test. You probably expect robot to move in 'STATE_STOPPED'.
It should be placed on the ground in the first few frames of the simulation due to penetrating contact.
Secondly, you ask for the robot to move with an angular rate 0.1 rad/s.
It is slow, this state will be set, but will be converged to stationary due to simulated friction and robot controllers thaw will counter-act the movement.
I will not expect the movement of the robot both in 'STATE_STOPPED' and 'STATE_PLAYING'

question: how do we know which entities can be removed and which can't?

It is simple - what to spawned, you can remove.

question: what is the expected outcome? I would expect the lidar entity to disappear, but the whole robot does.

It is expected behavior that prevents leaks of spawn tickets.
We will try to make it more flexible in future, but I elected conservative, simple approach for this release.

btw. why this is not called despawn_entity, but delete_entity?

Please file your complain here https://github.com/ros-simulation/simulation_interfaces/issues.

question: shouldn't we return the list in the increasing order rather than unordered? It would be easier to read.

Good idea, you are more than welcome to provide a PR.

comment: default state == 0 (stopped) is quite counter-intuitive to what is typically seen; starting the simulation and seeing that nothing happens (publishing messages to topics such as /cmd_vel have no result) can surprise even experienced users

I follow standard during implementation that clearly states:

uint8 STATE_STOPPED      = 0           # Simulation is stopped, which is equivalent to pausing and resetting with ALL.
                                       # This is typically the default state when simulator is launched.
                                       # Stopped simulation can be played. It can also be paused, which means
                                       # starting simulation in a paused state immediately,

Please file your complain here https://github.com/ros-simulation/simulation_interfaces/issues.

[jhanca-robotecai]

Thank you for the detailed reply.

[byrcolin]

Everything I see that has to do with platform seems good to me.

[jhanca-robotecai]

I pushed some small fixes to this feature branch via PR #893 and #896.
There are still two things that I noticed when doing a review, but I did not include them in the changes, as each developer might have a different opinion:

• I would prefer to keep SystemComponent in the names of the classes that are SystemComponents
• I believe ReloadLevel name in the API might be confusing; it is used as a ResetSimulation in our case

Other than that, I have some problems with the tests:

[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
/home/jhanca/devroot/devo3de/o3de-extras/Gems/SimulationInterfaces/Code/Tests/Tools/SimulationIterfaceAppTest.cpp:49: Failure
Failed to get asset id for sampleasset/testsimulationentity.spawnable

 [ RUN      ] SimulationInterfaceROS2TestFixture.SmokeTestROS2Domain
/home/jhanca/devroot/devo3de/o3de-extras/Gems/SimulationInterfaces/Code/Tests/Tools/InterfacesTest.cpp:147: Failure
Expected equality of these values:
  receivedMsgs
    Which is: 9
  10

Edit: the first problem is gone after I removed my Cache and rebuilt all assets.

[jhanca-robotecai]

The last PR fixes the test mentioned above; however I discovered one more problem. There is inconsistency between launching a game launcher (simulation is stopped) and the game mode from the editor (simulation is playing). I am perfectly fine with the simulation playing, but the service to get the simulation state should return the valid one.

[jhanca-robotecai]

All my comments were addressed.