Define a utility class to communicate with Gazebo in a test

[S-Dafarra]

Description to be updated.

Copying relevant information gathered during the t2t meeting with @dic-iit/telexistence

This issue has some information
https://github.com/dic-iit/element_floating-base-estimation/issues/23
Espcially this comment
https://github.com/dic-iit/element_floating-base-estimation/issues/23#issuecomment-461381713
Guys another interesting pointer from Silvio,
https://github.com/dic-iit/element_floating-base-estimation/issues/85#issuecomment-531687562
https://github.com/kuka-isir/rtt_gazebo_embedded

[traversaro]

If you intend to communicate with the Gazebo simulation just via YARP ports, then you have two possible options: either you launch gzserver as an external process and you wait it to be up (option A), or you launch it calling the appropriate functions to launch it as part of the process of the test (option B).

For option A, I think it is may worth to check libraries that simplify the spawn of new process, @diegoferigo in the past used tiny-process-library in gym-ignition (see https://github.com/robotology/gym-ignition/blob/c43085ff17282f1c4b004fd1fcdd24d3ee5d93c3/cpp/scenario/gazebo/src/GazeboSimulator.cpp#L287) and I think was quite satisfied, an alternative instead could be reproc. Note that you could also spawn "fixture" process outside of C++ using for example CMake (https://crascit.com/2016/10/18/test-fixtures-with-cmake-ctest/) or robot-testing-framework (https://github.com/robotology/robot-testing-framework), but in my limited experience keeping all the logic of the test in language of the component that you test simplifies maintenance.

For option B, beside the pointers on how to build a custom main for Gazebo that I provided in the other comments (i.e. https://github.com/osrf/gazebo/blob/6fd426b3949c4ca73fa126cde68f5cc4a59522eb/examples/stand_alone/custom_main/custom_main.cc/custom_main.cc?at=default, gazebosim/gazebo-classic#1145 and https://github.com/kuka-isir/rtt_gazebo_embedded/blob/master/src/rtt_gazebo_embedded.cc) another interesting piece of software is the one provided in the Gazebo's header gazebo/test/ServerFixture.hh (example in https://github.com/osrf/gazebo/tree/6fd426b3949c4ca73fa126cde68f5cc4a59522eb/examples/stand_alone/test_fixture), that basically exposes to external users the testing framework used internally in Gazebo. The only downside is that is quite tightly coupled with the gtest version used to compile Gazebo, but other then that it may be interesting, see https://github.com/robotology/gazebo-fmi/blob/fee735e5b17a06f478b05d1a1b4a34c30f66d06e/plugins/actuator/test/FMIActuatorPluginPositionRegulationTest.cc .

[diegoferigo]

I want to drop some pointers here of what I discussed yesterday f2f with @S-Dafarra, not sure if this is the best place. The exploration of the RL team with Ignition Gazebo are being quite successful and we managed to 1) simplify the installation of our sw stack and 2) streamline its packaging and distribution.

The C++ library that powers all the machinery is called ScenarI/O, and its Gazebo backend provides something very similar to what you're accomplishing with IRobotControl and ISensorBridge (correct me if I'm wrong). We use extensively its version exposed to Python to quickly write tests.

Under the assumption that BLF has 1) python bindings #53 and 2) has no middleware involved, it would be very easy to write unit testing from Python. You can have a look at test_custom_controllers.py, where the only difference is that the controller runs as a gazebo plugin. In your case it should be slightly to modified to i) instantiate a BLF object, ii) extract data from the simulator, iii) set references using BLF Python APIs, iv) step gazebo, v) assert correctness.

Note that if you prefer avoiding Python, everything could be done also from C++. However, being primarily a C++ developer, I much appreciate the fast development cycle that Python offers and the flexibility of the pytest suite.

Note also that this approach would ensure reproducibility, and even better, offers a single-process approach that makes CI integration much easier.

[S-Dafarra]

Regarding option A, since we will focus mainly on Ubuntu for the time being, we were also considering some simple bash scripts to launch Gazebo. @GiulioRomualdi did a similar thing to launch bash scripts as a tests in CMake for the iFeel GUI.

For option B instead, will we be able to load the YARP plugins then?

[traversaro]

For option B instead, will we be able to load the YARP plugins then?

As long as the directory containing the YARP plugins are correctly passed to Gazebo (for example using the GAZEBO_PLUGIN_PATH env variable as we usually do, or using the gazebo::common::SystemPaths::AddPluginPaths APIs if you want to do that directly from the code) the YARP plugin should load normally. The example in https://github.com/robotology/gazebo-fmi/blob/fee735e5b17a06f478b05d1a1b4a34c30f66d06e/plugins/actuator/test/FMIActuatorPluginPositionRegulationTest.cc indeed tests a Gazebo plugin, so the plugin under test is loaded by Gazebo as any other plugin.

[S-Dafarra]

Indeed, the ServerFixture stuff seems pretty interesting.

The only downside is that is quite tightly coupled with the gtest version used to compile Gazebo

Maybe it is possible to copy
https://github.com/osrf/gazebo/tree/6fd426b3949c4ca73fa126cde68f5cc4a59522eb/gazebo/test and simply convert the gtest related calls (ASSERT_NO_THROW, EXPECT_TRUE, ..) with the catch2 correspondents (CHECK_NOTHROW, REQUIRE,..) 🤔 Maybe starting only with the calls we need.

[traversaro]

The only downside is that is quite tightly coupled with the gtest version used to compile Gazebo

Maybe it is possible to copy
https://github.com/osrf/gazebo/tree/6fd426b3949c4ca73fa126cde68f5cc4a59522eb/gazebo/test and simply convert the gtest related calls (ASSERT_NO_THROW, EXPECT_TRUE, ..) with the catch2 correspondents (CHECK_NOTHROW, REQUIRE,..) 🤔 Maybe starting only with the calls we need.

Indeed, it may be possible. I don't really remember why I thought it was coupled with gtest so much.

[traversaro]

Indeed, it may be possible. I don't really remember why I thought it was coupled with gtest so much.

Probably it is because I was writing test to be parametric in the physic engine, and so I used https://github.com/osrf/gazebo/blob/6fd426b3949c4ca73fa126cde68f5cc4a59522eb/gazebo/test/helper_physics_generator.hh that indeed is coupled with gtest.

[prashanthr05]

Keeping in mind all the above discussion, we chose the following route as described,

• The Gazebo instance with a specific world and model will be launched using the bash script (all the relevant GazeboYarpPlugins loaded from Gazebo, YARP, and GazeboYarpPlugins installation and by setting relevant environment variables)
• The yarpserver instance will be launched through the same script
• the test instance using the Gazebo utility class will launch through the bash script
• The bash script will take care of proper tear down of all the running process

Instead, the Gazebo utility class might use the ServerFixture and replicate related calls to catch2 correspondences in order to do the following,

• checking if the Gazebo and yarpserver instances are running
• check if proper handshakes are made and the fixture has been set up with full model loaded
• Get the default Gazebo WorldPtr
• Access the model information through the WorldPtr to access the underlying sensors/physics data
• Run some assertion checks (for example, compare the IMU measurements read through YarpSensorBridge and from Gazebo data buffers are the same)

But here my question is that since the Gazebo instance would already be loaded by the bash script, we will not have control over such an instance and I don't understand how to establish a communication with the running Gazebo instance using the ServerFixture. Please see https://answers.gazebosim.org//question/8985/connect-to-a-running-gazebo-server-from-c/

Since with ServerFixture, we actually load the world here in the FMI example, which means we have complete control over the simulation.
Should we go about using Gazebo::Transport https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/group__gazebo__transport.html since we cannot be taking the Plugin approach as well?

[S-Dafarra]

But here my question is that since the Gazebo instance would already be loaded by the bash script, we will not have control over such an instance and I don't understand how to establish a communication with the running Gazebo instance using the ServerFixture. Please see https://answers.gazebosim.org//question/8985/connect-to-a-running-gazebo-server-from-c/

Since with ServerFixture, we actually load the world here in the FMI example, which means we have complete control over the simulation.
Should we go about using Gazebo::Transport https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/group__gazebo__transport.html since we cannot be taking the Plugin approach as well?

This was one of the open points. If Gazebo runs as a separate process, it is not possible to use the APIs. To be honest, I am not aware on how to access the Gazebo topics once this is running, nor which kind of information you can retrieve. In any case, probably we will need to access at least the base position. Then, we can test the interface when the robot is already in a known position. After we have the SensorBridge tested, we can use it directly in future tests to check if the robot reached a particular state.

[prashanthr05]

I can experiment with the Transport API, if we can agree it might be one possible right way to go.

However, in this case, if I am right, we would rely on a thread based process (where each subscriber might spawn a thread for a relevant callback function to update the buffers for the topic we subscribe to). This might be a source of non-determinism.

[prashanthr05]

Anyways, now after the discussion we had and reading through all the above pointers, I think I understand what needs to be done. I might have to do some active experimentation to understand the best way forward, indeed.

[traversaro]

Indeed, it may be possible. I don't really remember why I thought it was coupled with gtest so much.

Probably it is because I was writing test to be parametric in the physic engine, and so I used https://github.com/osrf/gazebo/blob/6fd426b3949c4ca73fa126cde68f5cc4a59522eb/gazebo/test/helper_physics_generator.hh that indeed is coupled with gtest.

No, actually now I remember: in https://github.com/osrf/gazebo/blob/6fd426b3949c4ca73fa126cde68f5cc4a59522eb/gazebo/test/ServerFixture.hh#L66 the gazebo::ServerFixture inherits from testing::Test , that is a gtest class. If we want to use it without gtest, then the best option is to use the code to use standalone Gazebo on its own.