search

robotology / gz-sim-yarp-plugins

YARP plugins for Modern Gazebo (gz-sim).
BSD 3-Clause "New" or "Revised" License
8 stars 0 forks source link

Add gz-sim-yarp-basestate-system plugin to publish BaseState information #65

Closed xela-95 closed 5 months ago

xela-95 commented 6 months ago

This PR adds the gz-sim-yarp-basestate plugin that, as the correspondent plugin for gazebo classic, serves the purpose of publishing state information (base pose, velocity and acceleration) of the model base link.

Closes #59 Closes #66

codecov[bot] commented 6 months ago

Codecov Report

All modified and coverable lines are covered by tests :white_check_mark:

Comparison is base (25216f1) 83.19% compared to head (d971789) 83.19%.

Additional details and impacted files ```diff @@ Coverage Diff @@ ## main #65 +/- ## ======================================= Coverage 83.19% 83.19% ======================================= Files 14 14 Lines 964 964 ======================================= Hits 802 802 Misses 162 162 ```

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

xela-95 commented 6 months ago

@traversaro for this kind of system plugin, what kind of interfaces do you think I need to implement among:

The sensor plugins already ported implement ISystemPreUpdate and ISystemPostUpdate.

While the ISystemPostUpdate interface is surely needed to get data after physics simulation and update the singleton data strucure, I'm not sure about the ISystemPreUpdate. The documentation says it's useful to modify the state before physics runs (e.g. set control signals and perform network synchronization). In the other plugins is used as in

https://github.com/robotology/gz-sim-yarp-plugins/blob/25216f1c90ed524a30b664e7cc327b368eb2d649/plugins/forcetorque/ForceTorque.cc#L139-L148

but I'm not sure what this code does in detail.

Also ISystemReset is not implemented in the plugins but I think it should in order to manage simulation resets.

xela-95 commented 6 months ago

I've also noted that to read the base state the yarp port exporting the data is /basestate while for sensor plugins the port is, for example /forcetorque/measuments. The /measurements part is automatically added by some yarp interface from which it inherits? Since basestate is a "system" plugin and not a sensor do you think it is correct to have that port name exposing data?

xela-95 commented 6 months ago

While the ISystemPostUpdate interface is surely needed to get data after physics simulation and update the singleton data strucure, I'm not sure about the ISystemPreUpdate. The documentation says it's useful to modify the state before physics runs (e.g. set control signals and perform network synchronization). In the other plugins is used as in

https://github.com/robotology/gz-sim-yarp-plugins/blob/25216f1c90ed524a30b664e7cc327b368eb2d649/plugins/forcetorque/ForceTorque.cc#L139-L148

but I'm not sure what this code does in detail.

Also ISystemReset is not implemented in the plugins but I think it should in order to manage simulation resets.

@traversaro related to this I think I'm missing something on things works on Gazebo side (and I haven't found useful documentation): while in gazebo classic all the update of the data coming from gazebo was made on the callback OnUpdate, with the new Gazebo what I understand is that in the PreUpdate method we subscribe to some topic on which the Gazebo sensor is publishing data, passing a callback which has to goal of updating with the latest data a class member holding the message received, ftMsg in the ft plugin.

Then, in the PostUpdate method we retrieve the data stored in this data member and store it on the singleton data structure such that the Driver class can access it and send on the yarp port.

I'm thinking on how to transpose this for Model plugins as the basestate one. In gazebo-yarp-plugins the OnUpdate is the following: https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/basestate/src/BaseStateDriver.cpp#L80-L118

xela-95 commented 6 months ago

I think I've implemented the main mechanism to retrieve and store data, but I have to debug an error occurring while testing the plugin:

[INFO] gz-sim-yarp-basestate-system: configuration of device  basestate_plugin_device  loaded from yarpConfigurationString :  (yarpDeviceName basestate_plugin_device) (baseLink link_1) 
===================>> m_modelScopedName: model/basestate_model/link/link_1
[DEBUG] |yarp.dev.PolyDriver|gazebo_basestate| Parameters are (baseLink link_1) (device gazebo_basestate) (modelScopedName "model/basestate_model/link/link_1") (robot "model/basestate_model/link/link_1") (yarpDeviceName basestate_plugin_device)
[INFO] |yarp.dev.PolyDriver|gazebo_basestate| Created device <gazebo_basestate>. See C++ class BaseStateDriver for documentation.
[INFO] Registered YARP device with instance name: model/basestate_model/link/link_1/basestate_plugin_device
[DEBUG] Reading file basestate_nws.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file basestate_nws.xml
[INFO] Yarprobotinterface was started using the following enable_tags: 
[INFO] Yarprobotinterface was started using the following disable_tags: 
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags: 
[DEBUG] Preprocessor complete in:  9.53674e-07 s
[DEBUG] ===========> deviceDatabaseKey:  model/basestate_model/link/link_1/basestate_plugin_device
[DEBUG] ==========> yarpDeviceName:  basestate_plugin_device
===================> Key of device 0 is basestate_plugin_device
[INFO] startup phase starting...
[INFO] Opening device basestate_nws_yarp with parameters [("robotName" = "basestate"), ("name" = "/basestate"), ("period" = "50")]
[DEBUG] |yarp.dev.PolyDriver|basestate_nws_yarp| Parameters are (device analogServer) (id basestate_nws_yarp) (name "/basestate") (period 50) (robotName basestate)
[WARNING] |yarp.devices.AnalogWrapper| The 'AnalogWrapper' device is deprecated.
[WARNING] |yarp.devices.AnalogWrapper| Possible alternatives, depending on the specific type sensor data, are:
[WARNING] |yarp.devices.AnalogWrapper| 'MultipleAnalogSensorsRemapper`+`MultipleAnalogSensorsServer`, `PoseStampedRosPublisher`, `WrenchStampedRosPublisher`,`IMURosPublisher`,etc.
[WARNING] |yarp.devices.AnalogWrapper| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.devices.AnalogWrapper| Please update your scripts.
[INFO] |yarp.os.Port|/basestate/rpc:i| Port /basestate/rpc:i active at tcp://10.240.79.41:10002/
[INFO] |yarp.dev.PolyDriver|basestate_nws_yarp| Created wrapper <analogServer>. See C++ class AnalogWrapper for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device basestate_nws_yarp with parameters [("device" = "basestate_plugin_device")]
[INFO] basestate_nws_yarp is not an IWrapper. Trying IMultipleWrapper
[INFO] |yarp.os.Port|/basestate| Port /basestate active at tcp://10.240.79.41:10003/
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[INFO] startup phase finished.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.

I think this could be due to the way in which I'm retrieving Link data:

Link baseLink = Link(m_baseLinkEntity);

...
 // Get the pose of the origin of the link frame in the world reference frame
if (dataAvailable && baseLink.WorldPose(_ecm).has_value())
    math::Pose3d worldBasePose = baseLink.WorldPose(_ecm).value();
else
    dataAvailable = false;
traversaro commented 6 months ago

I think I've implemented the main mechanism to retrieve and store data, but I have to debug an error occurring while testing the plugin:

[INFO] gz-sim-yarp-basestate-system: configuration of device  basestate_plugin_device  loaded from yarpConfigurationString :  (yarpDeviceName basestate_plugin_device) (baseLink link_1) 
===================>> m_modelScopedName: model/basestate_model/link/link_1
[DEBUG] |yarp.dev.PolyDriver|gazebo_basestate| Parameters are (baseLink link_1) (device gazebo_basestate) (modelScopedName "model/basestate_model/link/link_1") (robot "model/basestate_model/link/link_1") (yarpDeviceName basestate_plugin_device)
[INFO] |yarp.dev.PolyDriver|gazebo_basestate| Created device <gazebo_basestate>. See C++ class BaseStateDriver for documentation.
[INFO] Registered YARP device with instance name: model/basestate_model/link/link_1/basestate_plugin_device
[DEBUG] Reading file basestate_nws.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file basestate_nws.xml
[INFO] Yarprobotinterface was started using the following enable_tags: 
[INFO] Yarprobotinterface was started using the following disable_tags: 
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags: 
[DEBUG] Preprocessor complete in:  9.53674e-07 s
[DEBUG] ===========> deviceDatabaseKey:  model/basestate_model/link/link_1/basestate_plugin_device
[DEBUG] ==========> yarpDeviceName:  basestate_plugin_device
===================> Key of device 0 is basestate_plugin_device
[INFO] startup phase starting...
[INFO] Opening device basestate_nws_yarp with parameters [("robotName" = "basestate"), ("name" = "/basestate"), ("period" = "50")]
[DEBUG] |yarp.dev.PolyDriver|basestate_nws_yarp| Parameters are (device analogServer) (id basestate_nws_yarp) (name "/basestate") (period 50) (robotName basestate)
[WARNING] |yarp.devices.AnalogWrapper| The 'AnalogWrapper' device is deprecated.
[WARNING] |yarp.devices.AnalogWrapper| Possible alternatives, depending on the specific type sensor data, are:
[WARNING] |yarp.devices.AnalogWrapper| 'MultipleAnalogSensorsRemapper`+`MultipleAnalogSensorsServer`, `PoseStampedRosPublisher`, `WrenchStampedRosPublisher`,`IMURosPublisher`,etc.
[WARNING] |yarp.devices.AnalogWrapper| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4.
[WARNING] |yarp.devices.AnalogWrapper| Please update your scripts.
[INFO] |yarp.os.Port|/basestate/rpc:i| Port /basestate/rpc:i active at tcp://10.240.79.41:10002/
[INFO] |yarp.dev.PolyDriver|basestate_nws_yarp| Created wrapper <analogServer>. See C++ class AnalogWrapper for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device basestate_nws_yarp with parameters [("device" = "basestate_plugin_device")]
[INFO] basestate_nws_yarp is not an IWrapper. Trying IMultipleWrapper
[INFO] |yarp.os.Port|/basestate| Port /basestate active at tcp://10.240.79.41:10003/
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[INFO] startup phase finished.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.
[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.

I think this could be due to the way in which I'm retrieving Link data:

Link baseLink = Link(m_baseLinkEntity);

...
 // Get the pose of the origin of the link frame in the world reference frame
if (dataAvailable && baseLink.WorldPose(_ecm).has_value())
    math::Pose3d worldBasePose = baseLink.WorldPose(_ecm).value();
else
    dataAvailable = false;

I think one of my comments is related to that.

xela-95 commented 5 months ago

@traversaro I updated the code addressing your comments and trying to refactor the code to be more explainable and avoiding useless variables. I tested the code using the tutorial and it worked. However there is still the log of the analog sensor when gazebo is launched but the simulation is not yet started (since the base state data is not yet available):

[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.

How do you suggest to address it?

traversaro commented 5 months ago

@traversaro I updated the code addressing your comments and trying to refactor the code to be more explainable and avoiding useless variables. I tested the code using the tutorial and it worked. However there is still the log of the analog sensor when gazebo is launched but the simulation is not yet started (since the base state data is not yet available):

[WARNING] BaseState data not available
[ERROR] |yarp.devices.AnalogWrapper| AnalogWrapper: : Sensor returned error with code 1.

How do you suggest to address it?

This is actually an error in AnalogWrapper, that however is a deprecated piece of software that it does not make sense to fix. We can just ignore, alternativly we could also block (with a timeout) on the first read method, to not return until a first good measure is available.

Something like adding a m_firstDataAvailable boolean attribute to yarp::dev::gzyarp::BaseStateDriver, that at the beginning is false, and modify read as:

    int read(yarp::sig::Vector& out)
    {
        if (out.size() != YarpBaseStateChannelsNumber)
        {
            out.resize(YarpBaseStateChannelsNumber);
        }

        bool dataAvailable = false;
        {
            std::lock_guard<std::mutex> lock(m_baseLinkData->mutex);
            dataAvailable = m_baseLinkData->dataAvailable;
        }

        // This code just get executed in the first read
        if (!m_firstDataAvailable)
        {
             if (dataAvailable)
             {
                 m_firstDataAvailable = true;
             } else {
                 double waitingTimeInS = 0.0;
                 double timeoutInFirstWaitInS = 1.0;
                 double checkPeriodInS = 0.01;

                 while (waitingTimeInS <= timeoutInFirstWaitInS && !dataAvailable )
                 {
                     std::this_thread::sleep_for(std::chrono::milliseconds(checkPeriodInS*1000));
                     {
                          std::lock_guard<std::mutex> lock(m_baseLinkData->mutex);
                          dataAvailable = m_baseLinkData->dataAvailable;
                     }
                 }

                 if (!dataAvailable)
                 {
                     // priint warning and return error value
                 }
             }
        }

        {
            std::lock_guard<std::mutex> lock(m_baseLinkData->mutex);

            for (size_t i = 0; i < YarpBaseStateChannelsNumber; i++)
            {
               out[i] = m_baseLinkData->data[i];
            }
        }

        return AS_OK;
    }

(I also removed an unnecessary buffer).

traversaro commented 5 months ago

@xela-95 can you provide a meaningful title for the PR? Something like Add gz-sim-yarp-basestate-system plugin to publish BaseState information is totally ok, just avoid the automatically generated title that does not mean anything. This is important both for people reviewing the PR, and also as the changelog for a given release is automatically generated by the title of the merged PR.

Also a short description (just one or two sentences) in the PR is useful, for people in the future that find the PR.

xela-95 commented 5 months ago

@traversaro all comments related to this plugin have been addressed. Merging.