Merge pull request #675 from xela-95/modernizebasestateplugin

Modernize gazebo_yarp_basestate plugins

CHANGELOG.md

@@ -7,6 +7,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ### Added
 - The `gazebo_yarp_controlboard` plugin gained support to load at runtime arbitrary couplings specified by a yarp device that exposes the [`yarp::dev::IJointCoupling`](https://github.com/robotology/yarp/blob/v3.9.0/src/libYARP_dev/src/yarp/dev/IJointCoupling.h#L16) interface. The name of the yarp device used to load the coupling is passed via the `device` parameter in the `COUPLING` group. For an example of PR that uses this new feature, check https://github.com/icub-tech-iit/ergocub-software/pull/178 .
+- The `gazebo_yarp_basestate` has been modified to be aligned w.r.t. of plugins in gazebo-yarp-plugins, supporting `disableImplicitNetworkWrapper` and `yarpDeviceName` parameters, and by supporting the use of `gazebo_yarp_robotinterface` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/675).
 
 ### Removed
 

plugins/basestate/include/gazebo/BaseState.hh

@@ -47,7 +47,9 @@ namespace gazebo
         yarp::dev::IMultipleWrapper* m_networkWrapper;  ///< Interface to attach device driver to network device
         std::string m_robot;                            ///< name of robot model
         yarp::os::Property m_config;                    ///< Property to read configuration
-
+
+        bool m_deviceRegistered{false};
+        std::string m_scopedDeviceName;
     };
 }
 

plugins/basestate/include/yarp/dev/BaseStateDriver.h

@@ -4,7 +4,6 @@
  * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT
  */
 
-
 #ifndef GAZEBOYARP_BASESTATEDRIVER_H
 #define GAZEBOYARP_BASESTATEDRIVER_H
 
@@ -42,98 +41,121 @@ namespace yarp
 
 /**
  * YARP device driver to give the gazebo state information of a desired base link of the robot
- * The state information include 
+ * The state information include
  * - absolute pose of the base link (x, y, z. roll, pitch, yaw)
  * - linear and angular velocity of the base link in the world frame
  * - linear and angular acceleration of the base link in the world frame
- * \warning Note that the position angles, angular velocity and angular accelerations 
- *          are expressed in radians-based units which is different from 
+ * \warning Note that the position angles, angular velocity and angular accelerations
+ *          are expressed in radians-based units which is different from
  *          the usual YARP convention of expressing angles in degrees
- * 
+ *
  * This device can be used by adding the following line in the SDF file of your robot,
  *  ```
  *      <plugin name="basestate" filename="libgazebo_yarp_basestate.so">
  *          <yarpConfigurationFile>model://path-to-the-configuration-file</yarpConfigurationFile>
  *      </plugin>
  *  ```
- * 
+ *
+ * For configuration, the device supports two style. The first one, that is activated if disableImplicitNetworkWrapper parameter
+ * is given, is the following:
+ *
+ * | Parameter name| Type |Units|Default Value|Required |                 Description                |              Notes             |
+ * |:-------------:|:----:|:---:|:-----------:|:-------:|:------------------------------------------:|:------------------------------:|
+ * | yarpDeviceName | string |  |             |   Yes   | name of the device used to identify the device instance in the gazebo_yarp_robotinterface plugin   |             |
+ * | baseLink          |string|  -  |    -        |   Yes   |  name of the floating base link   |                                      |
+ *
+ * An example configuration file might look like:
+ *
+ * ```
+ * disableImplicitNetworkWrapper
+ * yarpDeviceName basestate_device
+ * baseLink torso_link
+ * ```
+ *
+ * the same options can be passed via yarpConfigurationString tag to pass the configuration as a string, as in:
+ * ```
+ *      <plugin name="basestate" filename="libgazebo_yarp_basestate.so">
+ *          <yarpConfigurationString>(disableImplicitNetworkWrapper) (yarpDeviceName basestate_device) (baseLink torso_link)</yarpConfigurationFile>
+ *      </plugin>
+ * ```
+ *
+ *
+ * Alternatively, if disableImplicitNetworkWrapper parameter is not given, the following DEPRECATED style is used:
+ *
  * The configuration file must contain two groups [WRAPPER] and [DRIVER]
- * 
+ *
  * Parameters accepted in the config argument of the open method for [WRAPPER]:
- * 
+ *
  * | Parameter name| Type |Units|Default Value|Required |                 Description                |              Notes             |
  * |:-------------:|:----:|:---:|:-----------:|:-------:|:------------------------------------------:|:------------------------------:|
  * | name          |string|  -  |    -        |   Yes   | full name of the port opened by the device | MUST start with a '/' character|
- * | period        |double|  ms |    20       |   No    | refresh period of broadcast value in ms    |    optional, default 20ms      |   
- * | device        |string|  -  |    -        |   Yes   | name of the network wrapper to connect to  | MUST be set to analogServer    |    
+ * | period        |double|  ms |    20       |   No    | refresh period of broadcast value in ms    |    optional, default 20ms      |
+ * | device        |string|  -  |    -        |   Yes   | name of the network wrapper to connect to  | MUST be set to analogServer    |
  *
  * Parameters accepted in the config argument of the open method for [DRIVER]:
- * 
+ *
  * | Parameter name|      Type     |Units|Default Value|Required |                       Description                      |                        Notes                     |
  * |:-------------:|:-------------:|:---:|:-----------:|:-------:|:------------------------------------------------------:|:------------------------------------------------:|
  * | device        |    string     |  -  |    -        |   Yes   | name of the device driver to instantiate               | MUST be set to gazebo_basestate           |
  * | baseLink      |    string     |  -  |    -        |   Yes   | name of the floating base link                         |                            -                     |
  *
  * An example configuration file might look like,
- * 
+ *
  * ```
  * [WRAPPER]
  * name /icubSim/floating_base/state:o
  * period 16
  * device analogServer
- * 
+ *
  * [DRIVER]
  * device gazebo_basestate
  * baseLink torso_link
  * ```
+ *
+ * This style is going to be removed in a future release of gazebo-yarp-plugins.
  */
 
-
 class yarp::dev::GazeboYarpBaseStateDriver : public yarp::dev::IAnalogSensor,
-                                                    public yarp::dev::DeviceDriver,
-                                                    public yarp::dev::IPreciselyTimed
+                                             public yarp::dev::DeviceDriver,
+                                             public yarp::dev::IPreciselyTimed
 {
-        public:
-        GazeboYarpBaseStateDriver();
-        virtual ~GazeboYarpBaseStateDriver();
-
-
-        // Device Driver
-        virtual bool open(yarp::os::Searchable& config);
-        virtual bool close();
-
-        // Analog sensor
-        virtual int read(yarp::sig::Vector &out);
-
-        // Analog sensor unimplemented - not required in simulation
-        virtual int getState(int ch);
-        virtual int getChannels();
-        virtual int calibrateChannel(int ch);
-        virtual int calibrateChannel(int ch, double v);
-        virtual int calibrateSensor();
-        virtual int calibrateSensor(const yarp::sig::Vector& value);
-
-        // Precisely Timed
-        virtual yarp::os::Stamp getLastInputStamp(); 
-
-        /**
-         * Gets World pose, velocity and acceleration of the base link
-         * and updates the base state vector
-         */
-        void onUpdate(const gazebo::common::UpdateInfo& _info);
-
-        private:
-
-        gazebo::physics::Model* m_robot;                      ///< Pointer to the loaded robot model
-        gazebo::physics::LinkPtr m_baseLink;                  ///< Pointer to the desired base link from the model
-        std::string m_baseLinkName;                           ///< Base link name
-        const int m_stateDimensions = 18;                     ///< State dimensions to include 6D Pose, 6D velocity and 6D acceleration 
-        yarp::sig::Vector m_baseState;                        ///< Vector for the base state
-        yarp::os::Stamp m_stamp;                              ///< Current timestamp
-        std::mutex m_dataMutex;                               ///< Mutex for resource sharing
-        bool m_dataAvailable = false;                         ///< flag to check data is available
-        gazebo::event::ConnectionPtr m_updateConnection;      ///< Event Pointer to the callback for updating the Gazebo information
-
-};
-#endif 
+public:
+    GazeboYarpBaseStateDriver();
+    virtual ~GazeboYarpBaseStateDriver();
+
+    // Device Driver
+    virtual bool open(yarp::os::Searchable &config);
+    virtual bool close();
+
+    // Analog sensor
+    virtual int read(yarp::sig::Vector &out);
 
+    // Analog sensor unimplemented - not required in simulation
+    virtual int getState(int ch);
+    virtual int getChannels();
+    virtual int calibrateChannel(int ch);
+    virtual int calibrateChannel(int ch, double v);
+    virtual int calibrateSensor();
+    virtual int calibrateSensor(const yarp::sig::Vector &value);
+
+    // Precisely Timed
+    virtual yarp::os::Stamp getLastInputStamp();
+
+    /**
+     * Gets World pose, velocity and acceleration of the base link
+     * and updates the base state vector
+     */
+    void onUpdate(const gazebo::common::UpdateInfo &_info);
+
+private:
+    gazebo::physics::Model *m_robot;                 ///< Pointer to the loaded robot model
+    gazebo::physics::LinkPtr m_baseLink;             ///< Pointer to the desired base link from the model
+    std::string m_baseLinkName;                      ///< Base link name
+    const int m_stateDimensions = 18;                ///< State dimensions to include 6D Pose, 6D velocity and 6D acceleration
+    yarp::sig::Vector m_baseState;                   ///< Vector for the base state
+    yarp::os::Stamp m_stamp;                         ///< Current timestamp
+    std::mutex m_dataMutex;                          ///< Mutex for resource sharing
+    bool m_dataAvailable = false;                    ///< flag to check data is available
+    gazebo::event::ConnectionPtr m_updateConnection; ///< Event Pointer to the callback for updating the Gazebo information
+};
+#endif