Merge pull request #379 from robotology/devel

Release gazebo-yarp-plugins 3.1.0

CMakeLists.txt

@@ -6,9 +6,9 @@ cmake_minimum_required(VERSION 2.8.7)
 PROJECT(GazeboYARPPlugins)
 
 # Project version
-set(${PROJECT_NAME}_MAJOR_VERSION 2)
-set(${PROJECT_NAME}_MINOR_VERSION 3)
-set(${PROJECT_NAME}_PATCH_VERSION 72)
+set(${PROJECT_NAME}_MAJOR_VERSION 3)
+set(${PROJECT_NAME}_MINOR_VERSION 1)
+set(${PROJECT_NAME}_PATCH_VERSION 0)
 set(${PROJECT_NAME}_VERSION
     ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION})
 
@@ -17,7 +17,7 @@ set(${PROJECT_NAME}_VERSION
 include(GNUInstallDirs)
 
 # Finding dependencies
-find_package(YARP 2.3.69.9 REQUIRED)
+find_package(YARP 3.0.101 REQUIRED)
 find_package(Boost REQUIRED serialization system)
 find_package(Protobuf REQUIRED)
 find_package(Gazebo REQUIRED)

README.md

@@ -13,5 +13,7 @@ Quick start
 -----------
 * [Downloading and Installing gazebo-yarp-plugins](http://robotology.gitlab.io/docs/gazebo-yarp-plugins/master/install.html)
 * [Use the gazebo-yarp-plugins in Gazebo Models](http://robotology.gitlab.io/docs/gazebo-yarp-plugins/master/embed_plugins.html)
+* [Use the gazebo-yarp-plugins as a C++ library using CMake](http://robotology.gitlab.io/docs/gazebo-yarp-plugins/master/use_as_library.html)
 * [Troubleshooting](http://robotology.gitlab.io/docs/gazebo-yarp-plugins/master/troubleshooting.html)
 * [Contributing](http://robotology.gitlab.io/docs/gazebo-yarp-plugins/master/contributing.html)
+

cmake/AddGazeboYarpPluginTarget.cmake

@@ -63,6 +63,7 @@ target_include_directories(${GAZEBO_PLUGIN_LIBRARY_NAME} PUBLIC
 
 # Add install target
 install(TARGETS ${GAZEBO_PLUGIN_LIBRARY_NAME}
+        EXPORT GazeboYARPPlugins
         RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}"
         LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}")
 

doc/embed_plugins.md

@@ -41,7 +41,7 @@ but are nevertheless useful for simulating complex scenarios.
 | Project an image stream on a simulated surface | `gazebo_yarp_videotexture` | Visual  | gazebo::VideoTexture |
 | Expose a YARP RPC interface to create/manipulate objects programatically. | `gazebo_yarp_worldinterface` | Model |  gazebo::WorldInterface |
 | Publish the absolute pose of the root link of a model. | `gazebo_yarp_modelposepublisher` | Model | gazebo::GazeboYarpModelPosePublisher |
-
+| Exposes the absolute pose, velocity and acceleration of a desired link through YARP analog server device| `gazebo_basestate` | Model | gazebo::GazeboYarpBaseState | yarp::dev::GazeboYarpBaseStateDriver |
 
 ## Using the plugins in Gazebo Models
 In Gazebo, the simulated models are described using the [SDF (simulation description format)]((http://gazebosim.org/sdf.html), an XML-based file format.

doc/main.md

@@ -14,5 +14,6 @@ Quick start
 -----------------------
 \li \ref install
 \li \ref embed_plugins
+\li \ref use_as_library
 \li \ref troubleshooting
 \li \ref contributing

doc/release/v3_0_0.md

@@ -0,0 +1,13 @@
+
+YARP 3.0.0 (UNRELEASED) Release Notes                                 {#v3_0_0}
+=====================================
+
+
+
+Important Changes
+-----------------
+
+
+### Build System
+* The `GazeboYARPPlugins` CMake configuration file now export targets for the `gazebo_yarp_singleton` and the `gazebo_yarp_lib_common` C++ libraries.
+  See `doc/use_as_library.md` for more info.

doc/use_as_library.md

@@ -0,0 +1,22 @@
+
+
+# Use the gazebo-yarp-plugins as a C++ library using CMake {#use_as_library}
+
+# Use the gazebo-yarp-plugins as a C++ library using CMake
+
+You can search the `gazebo-yarp-plugins` as a CMake package, using the `GazeboYARPPlugins` CMake package name.
+
+~~~cmake
+find_package(GazeboYARPPlugins REQUIRED)
+~~~
+
+If the `GazeboYARPPlugins` CMake package, the following imported targets will be defined:
+
+| Target name        | Description          |
+|:------------------:|:--------------------:|
+| `GazeboYARPPlugins::gazebo_yarp_lib_common` | `INTERFACE` library containing just the `GazeboYarpPlugins/common.h` header file. |
+| `GazeboYARPPlugins::gazebo_yarp_singleton` | Library containing the `GazeboYarpPlugins::Handler` singleton library. |
+| `GazeboYARPPlugins::gazebo_yarp_rpc_worldinterface` | The [thrift-generated YARP RPC interface](http://www.yarp.it/thrift_tutorial_simple.html) to the `gazebo_yarp_worldinterface` plugin. |
+| `GazeboYARPPlugins::gazebo_yarp_rpc_clock` | The [thrift-generated YARP RPC interface](http://www.yarp.it/thrift_tutorial_simple.html) to the `gazebo_yarp_clock` plugin. |
+
+Furthermore every plugin location is encoded in the `GazeboYARPPlugins::gazebo_yarp_pluginname` CMake imported target.

libraries/common/CMakeLists.txt

@@ -12,5 +12,10 @@ else()
     add_library(gazebo_yarp_lib_common INTERFACE)
     target_include_directories(gazebo_yarp_lib_common INTERFACE
       $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}/include>
     )
+
+    install(TARGETS gazebo_yarp_lib_common EXPORT GazeboYARPPlugins)
+    install(FILES include/GazeboYarpPlugins/common.h
+            DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/GazeboYarpPlugins)
 endif()

libraries/singleton/CMakeLists.txt

@@ -8,12 +8,19 @@ PROJECT(Library_Singleton)
 
 include(AddGazeboYarpPluginTarget)
 
+set(Singleton_HEADERS include/GazeboYarpPlugins/Handler.hh
+                      include/GazeboYarpPlugins/ConfHelpers.hh)
+
 add_gazebo_yarp_plugin_target(LIBRARY_NAME singleton
                               INCLUDE_DIRS include/GazeboYarpPlugins
-                              SYSTEM_INCLUDE_DIRS ${YARP_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS}
+                              SYSTEM_INCLUDE_DIRS ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS}
                               LINKED_LIBRARIES ${YARP_LIBRARIES} ${SDFORMAT_LIBRARIES} ${GAZEBO_LIBRARIES} ${PROTOBUF_LIBRARIES} ${Boost_LIBRARIES}
-                              HEADERS include/GazeboYarpPlugins/Handler.hh
-                                      include/GazeboYarpPlugins/ConfHelpers.hh
+                              HEADERS ${Singleton_HEADERS}
                               SOURCES src/Handler.cc
                                       src/ConfHelpers.cc)
 
+install(FILES ${Singleton_HEADERS}
+        DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/GazeboYarpPlugins)
+
+
+

plugins/CMakeLists.txt

@@ -34,3 +34,4 @@ add_subdirectory(depthCamera)
 
 add_subdirectory(contactloadcellarray)
 add_subdirectory(modelposepublisher)
+add_subdirectory(basestate)

plugins/basestate/CMakeLists.txt

@@ -0,0 +1,28 @@
+# Copyright (C) 2018 Istituto Italiano di Tecnologia ADVR & iCub Facility & RBCS Department
+# Authors: see AUTHORS file.
+# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+
+cmake_minimum_required(VERSION 2.8.7)
+
+include(AddGazeboYarpPluginTarget)
+
+set(LIB_COMMON_NAME gazebo_yarp_lib_common)
+
+if(CMAKE_VERSION VERSION_LESS 3.0.0)
+    get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS)
+    unset(LIB_COMMON_NAME)
+endif()
+
+
+add_gazebo_yarp_plugin_target(LIBRARY_NAME basestate
+                              INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}
+                              INCLUDE_DIRS include/gazebo 
+                                           include/yarp/dev
+                              SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS}
+                              LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${SDFORMAT_LIBRARIES}
+                              HEADERS include/gazebo/BaseState.hh
+                                      include/yarp/dev/BaseStateDriver.h
+                              SOURCES src/BaseState.cc
+                                      src/BaseStateDriver.cpp)
+
+

plugins/basestate/include/gazebo/BaseState.hh

@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR
+ * Authors: see AUTHORS file.
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT
+ */
+
+#ifndef GAZEBO_YARP_BASESTATE_HH
+#define GAZEBO_YARP_BASESTATE_HH
+
+#include <string>
+
+#include <gazebo/common/Plugin.hh>
+#include <GazeboYarpPlugins/Handler.hh>
+#include <GazeboYarpPlugins/common.h>
+#include <GazeboYarpPlugins/ConfHelpers.hh>
+
+#include <yarp/dev/Wrapper.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/PolyDriverList.h>
+
+namespace yarp
+{
+    namespace dev
+    {
+        class IMultipleWrapper;
+    }
+}
+
+namespace gazebo
+{
+    class GazeboYarpBaseState : public ModelPlugin
+    {
+    public:
+        GazeboYarpBaseState();
+        virtual ~GazeboYarpBaseState();
+
+        /**
+         * Loads robot model, reads configuration, 
+         * opens network wrapper device and opens device driver
+         */
+        void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+
+    private:
+        yarp::dev::PolyDriver m_deviceDriver;           ///< Device driver for getting base link state
+        yarp::dev::PolyDriver m_networkDevice;          ///< Network wrapper device to send state through YARP server
+        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
+
+    };
+}
+
+#endif
+

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

@@ -0,0 +1,143 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR
+ * Authors: see AUTHORS file.
+ * 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
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/common/common.hh>
+
+#include <gazebo/physics/Link.hh>
+#include <gazebo/physics/physics.hh>
+
+#if GAZEBO_MAJOR_VERSION >= 8
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Matrix3.hh>
+#include <ignition/math.hh>
+#else 
+#include <gazebo/math/Pose.hh>
+#include <gazebo/math/Vector3.hh>
+#include <gazebo/math/Matrix3.hh>
+#endif
+
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/dev/IAnalogSensor.h>
+#include <yarp/dev/PreciselyTimed.h>
+
+#include <yarp/os/Stamp.h>
+#include <yarp/os/Mutex.h>
+#include <yarp/os/LockGuard.h>
+
+#include <yarp/sig/Vector.h>
+
+#include <GazeboYarpPlugins/Handler.hh>
+#include <GazeboYarpPlugins/common.h>
+
+
+namespace yarp
+{
+    namespace dev
+    {
+        class GazeboYarpBaseStateDriver;
+    }
+}
+
+/**
+ * YARP device driver to give the gazebo state information of a desired base link of the robot
+ * 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
+ * 
+ * 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>
+ *  ```
+ * 
+ * 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    |    
+ *
+ * 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
+ * ```
+ */
+
+
+class yarp::dev::GazeboYarpBaseStateDriver : public yarp::dev::IAnalogSensor,
+                                                    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
+        yarp::os::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 
+