diff --git a/CHANGELOG.md b/CHANGELOG.md index fac215bb2..210428a22 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/plugins/basestate/include/gazebo/BaseState.hh b/plugins/basestate/include/gazebo/BaseState.hh index e68646b14..61384d9bf 100644 --- a/plugins/basestate/include/gazebo/BaseState.hh +++ b/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; }; } diff --git a/plugins/basestate/include/yarp/dev/BaseStateDriver.h b/plugins/basestate/include/yarp/dev/BaseStateDriver.h index 4f788824d..2d260da3d 100644 --- a/plugins/basestate/include/yarp/dev/BaseStateDriver.h +++ b/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, * ``` * * model://path-to-the-configuration-file * * ``` - * + * + * 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: + * ``` + * + * (disableImplicitNetworkWrapper) (yarpDeviceName basestate_device) (baseLink torso_link) + * + * ``` + * + * + * 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 diff --git a/plugins/basestate/src/BaseState.cc b/plugins/basestate/src/BaseState.cc index 187a96d7b..f691f9971 100644 --- a/plugins/basestate/src/BaseState.cc +++ b/plugins/basestate/src/BaseState.cc @@ -24,6 +24,12 @@ namespace gazebo GazeboYarpBaseState::~GazeboYarpBaseState() { + if (m_deviceRegistered) + { + GazeboYarpPlugins::Handler::getHandler()->removeDevice(m_scopedDeviceName); + m_deviceRegistered = false; + } + if (m_networkWrapper) { m_networkWrapper->detachAll(); @@ -61,10 +67,9 @@ namespace gazebo GazeboYarpPlugins::Handler::getHandler()->setRobot(boost::get_pointer(_parent)); m_robot = _parent->GetScopedName(); - - ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::GazeboYarpBaseStateDriver> - ("gazebo_basestate", "analogServer", "GazeboYarpBaseState")); - + + ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf<::yarp::dev::GazeboYarpBaseStateDriver>("gazebo_basestate", "", "GazeboYarpBaseState")); + // Getting .ini configuration file parameters from sdf bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_parent, _sdf, m_config); @@ -74,85 +79,126 @@ namespace gazebo return; } - yarp::os::Bottle networkDeviceProp = m_config.findGroup("WRAPPER"); - if (networkDeviceProp.isNull()) - { - yError() << "GazeboYarpBaseState plugin failed: [WRAPPER] group not found in config file"; - return; - } - - yarp::os::Bottle deviceDriverProp = m_config.findGroup("DRIVER"); - if (deviceDriverProp.isNull()) - { - yError() << "GazeboYarpBaseState plugin failed: [DRIVER] group not found in config file"; - return; - } - - if (networkDeviceProp.find("device").isNull()) - { - yError() << "GazeboYarpBaseState plugin failed: Missing parameter \"device\" under [WRAPPER] in config"; - return; - } - - if (networkDeviceProp.find("device").asString() != "analogServer") - { - yError() << "GazeboYarpBaseState plugin failed: \"device\" under [WRAPPER] should be set to \"analogSensor\" network wrapper."; - return; - } - - if (deviceDriverProp.find("device").isNull()) - { - yError() << "GazeboYarpBaseState plugin failed: Missing parameter \"device\" under [DRIVER] in config"; - return; - } - - if (deviceDriverProp.find("device").asString() != "gazebo_basestate") - { - yError() << "GazeboYarpBaseState plugin failed: \"device\" under [DRIVER] should be set to \"gazebo_basestate\"."; - return; - } - - if (deviceDriverProp.find("baseLink").isNull()) - { - yError() << "GazeboYarpBaseState plugin failed: Missing parameter \"baseLink\" under [DRIVER] in config"; - return; - } - - yarp::os::Bottle& robotConf = deviceDriverProp.addList(); - robotConf.addString("robot"); - robotConf.addString(m_robot.data()); - - if (deviceDriverProp.find("robot").isNull()) - { - yError() << "GazeboYarpBaseState plugin failed: robot name not passed to the driver"; - return; - } - - if (!m_networkDevice.open(networkDeviceProp)) - { - yError() << "GazeboYarpBaseState plugin failed: error opening network wrapper device"; - return; - } - - if (!m_deviceDriver.open(deviceDriverProp)) - { - yError() << "GazeboYarpBaseState plugin failed: error opening yarp device driver"; - return; - } - - yarp::dev::PolyDriverList driver_list; - if (!m_networkDevice.view(m_networkWrapper)) - { - yError() << "GazeboYarpBaseState plugin failed: error in loading wrapper"; - return; + bool disable_wrapper = m_config.check("disableImplicitNetworkWrapper"); + + // New behaviour to use + if (disable_wrapper) + { + if (m_config.find("yarpDeviceName").isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: Missing parameter \"yarpDeviceName\" under in config"; + return; + } + + if (m_config.find("baseLink").isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: Missing parameter \"baseLink\" under in config"; + return; + } + + m_config.put("device", "gazebo_basestate"); + m_config.put("robot", m_robot); + + if (!m_deviceDriver.open(m_config)) + { + yError() << "GazeboYarpBaseState plugin failed: error opening yarp device driver"; + return; + } + + m_scopedDeviceName = m_robot + "::" + m_config.find("yarpDeviceName").asString(); + + if (!GazeboYarpPlugins::Handler::getHandler()->setDevice(m_scopedDeviceName, &m_deviceDriver)) + { + yError() << "GazeboYarpForceTorque: failed setting scopedDeviceName(=" << m_scopedDeviceName << ")"; + return; + } + m_deviceRegistered = true; + yInfo() << "Registered YARP device with instance name:" << m_scopedDeviceName; } - - driver_list.push(&m_deviceDriver, "dummy"); - - if (!m_networkWrapper->attachAll(driver_list)) - { - yError() << "GazeboYarpBaseState plugin failed: error attaching devices"; - return; + + // Legacy behaviour that will be removed + if (!disable_wrapper) + { + yarp::os::Bottle networkDeviceProp = m_config.findGroup("WRAPPER"); + if (networkDeviceProp.isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: [WRAPPER] group not found in config file"; + return; + } + + yarp::os::Bottle deviceDriverProp = m_config.findGroup("DRIVER"); + if (deviceDriverProp.isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: [DRIVER] group not found in config file"; + return; + } + + if (networkDeviceProp.find("device").isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: Missing parameter \"device\" under [WRAPPER] in config"; + return; + } + + if (networkDeviceProp.find("device").asString() != "analogServer") + { + yError() << "GazeboYarpBaseState plugin failed: \"device\" under [WRAPPER] should be set to \"analogSensor\" network wrapper."; + return; + } + + if (deviceDriverProp.find("device").isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: Missing parameter \"device\" under [DRIVER] in config"; + return; + } + + if (deviceDriverProp.find("device").asString() != "gazebo_basestate") + { + yError() << "GazeboYarpBaseState plugin failed: \"device\" under [DRIVER] should be set to \"gazebo_basestate\"."; + return; + } + + if (deviceDriverProp.find("baseLink").isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: Missing parameter \"baseLink\" under [DRIVER] in config"; + return; + } + + yarp::os::Bottle &robotConf = deviceDriverProp.addList(); + robotConf.addString("robot"); + robotConf.addString(m_robot.data()); + + if (deviceDriverProp.find("robot").isNull()) + { + yError() << "GazeboYarpBaseState plugin failed: robot name not passed to the driver"; + return; + } + + if (!m_networkDevice.open(networkDeviceProp)) + { + yError() << "GazeboYarpBaseState plugin failed: error opening network wrapper device"; + return; + } + + if (!m_deviceDriver.open(deviceDriverProp)) + { + yError() << "GazeboYarpBaseState plugin failed: error opening yarp device driver"; + return; + } + + yarp::dev::PolyDriverList driver_list; + if (!m_networkDevice.view(m_networkWrapper)) + { + yError() << "GazeboYarpBaseState plugin failed: error in loading wrapper"; + return; + } + + driver_list.push(&m_deviceDriver, "dummy"); + + if (!m_networkWrapper->attachAll(driver_list)) + { + yError() << "GazeboYarpBaseState plugin failed: error attaching devices"; + return; + } } } } diff --git a/plugins/robotinterface/README.md b/plugins/robotinterface/README.md index 774030746..24a43eafa 100644 --- a/plugins/robotinterface/README.md +++ b/plugins/robotinterface/README.md @@ -88,3 +88,4 @@ Furthermore if the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` optio | `gazebo_yarp_multicamera` | This plugin can create a YARP device that exposes a multicamera interface. | | `gazebo_yarp_imu` | This plugin can create a YARP device that exposes a imu interface. | | `gazebo_yarp_forcetorque` | This plugin can create a YARP device that exposes a force-torque interface. | +| `gazebo_yarp_basestate` | This plugin can create a YARP device that expose the position, velocity and acceleration of the specified link. | diff --git a/tutorial/model/basestate/basestate_nws.xml b/tutorial/model/basestate/basestate_nws.xml new file mode 100644 index 000000000..1949d1e7b --- /dev/null +++ b/tutorial/model/basestate/basestate_nws.xml @@ -0,0 +1,17 @@ + + + + + + + + /basestate + 50 + + + basestate_plugin_device + + + + + diff --git a/tutorial/model/basestate/model.config b/tutorial/model/basestate/model.config new file mode 100644 index 000000000..2ee8ccb02 --- /dev/null +++ b/tutorial/model/basestate/model.config @@ -0,0 +1,16 @@ + + + + BaseState + 1.0 + model.sdf + + + Alessandro Croci + alessandro.croci@iit.it + + + + A simple model to demonstrate to gazebo_yarp_basestate. + + diff --git a/tutorial/model/basestate/model.sdf b/tutorial/model/basestate/model.sdf new file mode 100644 index 000000000..797f7f5fe --- /dev/null +++ b/tutorial/model/basestate/model.sdf @@ -0,0 +1,29 @@ + + + + 0 0 0.1 0 0 0 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + + (yarpDeviceName basestate_plugin_device) (baseLink link) (disableImplicitNetworkWrapper) + + + model://basestate/basestate_nws.xml + + +