[devices] Add YarpSensorBridge test device

CMakeLists.txt

@@ -75,6 +75,7 @@ include(AddBipedalLocomotionLibrary)
 include(InstallIniFiles)
 
 add_subdirectory(src)
+add_subdirectory(devices)
 
 #Creates the interface target BipedalLocomotion::Framework that depends on all the targets.
 #By including the file "BipedalLocomotion/Framework.h" it is possible to include all the headers automatically.

devices/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+add_subdirectory(YarpSensorBridgeTestDevice)
+

devices/YarpSensorBridgeTestDevice/CMakeLists.txt

@@ -0,0 +1,46 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+if(FRAMEWORK_COMPILE_YarpImplementation)
+  cmake_minimum_required(VERSION 3.5)
+  set(PLUGIN_NAME YarpSensorBridgeTestDevice)
+
+  set(${PLUGIN_NAME}_SRC src/YarpSensorBridgeTestDevice.cpp)
+  set(${PLUGIN_NAME}_HDR include/BipedalLocomotion/YarpSensorBridgeTestDevice.h)
+
+  yarp_prepare_plugin(${PLUGIN_NAME} CATEGORY device
+                                     TYPE BipedalLocomotion::YarpSensorBridgeTestDevice
+                                     INCLUDE ${${PLUGIN_NAME}_HDR}
+                                     DEFAULT ON)
+
+  yarp_add_plugin(${PLUGIN_NAME} ${${PLUGIN_NAME}_SRC} ${${PLUGIN_NAME}_HDR})
+
+  target_link_libraries(${PLUGIN_NAME} PUBLIC ${YARP_LIBRARIES}
+                                              ${iDynTree_LIBRARIES}
+                                              BipedalLocomotion::YarpUtilities
+                                              BipedalLocomotion::ParametersHandlerYarpImplementation
+                                              BipedalLocomotion::RobotInterfaceYarpImplementation)
+  target_compile_features(${PLUGIN_NAME} PUBLIC cxx_std_17)
+
+  # Specify include directories for both compilation and installation process.
+  # The $<INSTALL_PREFIX> generator expression is useful to ensure to create
+  # relocatable configuration files, see https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#creating-relocatable-packages
+  target_include_directories(${PLUGIN_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "<INSTALLINTERFACE:<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
+
+  # Specify installation targets, typology and destination folders.
+  yarp_install(TARGETS ${PLUGIN_NAME}
+               COMPONENT runtime
+               LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}/
+               ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}/)
+  yarp_install(FILES YarpSensorBridgeTestDevice.ini
+               COMPONENT runtime
+               DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}/)
+
+  add_subdirectory(app)
+
+  message(STATUS "Created device ${PLUGIN_NAME}.")
+
+endif()
+

devices/YarpSensorBridgeTestDevice/YarpSensorBridgeTestDevice.ini

@@ -0,0 +1,4 @@
+[plugin YarpSensorBridgeTestDevice]
+type device
+name YarpSensorBridgeTestDevice
+library YarpSensorBridgeTestDevice

devices/YarpSensorBridgeTestDevice/app/CMakeLists.txt

@@ -0,0 +1,25 @@
+# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+# Thanks to Stefano Dafarra <stefano.dafarra@iit.it> for this CMakeLists.txt
+
+# List the subdirectory (http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory)
+macro(SUBDIRLIST result curdir)
+  file(GLOB children RELATIVE ${curdir} ${curdir}/*)
+  set(dirlist "")
+  foreach(child ${children})
+    if(IS_DIRECTORY ${curdir}/${child})
+      list(APPEND dirlist ${child})
+    endif()
+  endforeach()
+  set(${result} ${dirlist})
+endmacro()
+
+# Get list of models
+subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/)
+# Install each model
+foreach (dir ${subdirs})
+  yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR})
+endforeach ()
+

devices/YarpSensorBridgeTestDevice/app/robots/iCubGazeboV2_5/interface/all_joints_mc.xml

@@ -0,0 +1,10 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="all_joints_mc" type="remotecontrolboardremapper">
+    <param name="remoteControlBoards">("/icubSim/head", "/icubSim/torso", "/icubSim/left_arm", "/icubSim/right_arm", "/icubSim/left_leg", "/icubSim/right_leg")</param>
+    <param name="axesNames">("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+    <param name="localPortPrefix">/baseestimation</param>
+</device>
+

devices/YarpSensorBridgeTestDevice/app/robots/iCubGazeboV2_5/interface/root_imu.xml

@@ -0,0 +1,9 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="root_link_imu_acc" type="genericSensorClient">
+    <param name="remote">/icubSim/xsens_inertial</param>
+    <param name="local">/baseestimation/xsens_inertial</param>
+</device>
+

devices/YarpSensorBridgeTestDevice/app/robots/iCubGazeboV2_5/interface/wbd_left_foot.xml

@@ -0,0 +1,9 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_foot_cartesian_wrench" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o</param>
+    <param name="local">/baseestimation/left_foot/cartesianEndEffectorWrench</param>
+</device>
+

devices/YarpSensorBridgeTestDevice/app/robots/iCubGazeboV2_5/interface/wbd_right_foot.xml

@@ -0,0 +1,9 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_foot_cartesian_wrench" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o</param>
+    <param name="local">/baseestimation/right_foot/cartesianEndEffectorWrench</param>
+</device>
+

devices/YarpSensorBridgeTestDevice/app/robots/iCubGazeboV2_5/launch-test-sensor-bridge.xml

@@ -0,0 +1,19 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="iCubGazeboV2_5" portprefix="icubSim" build="1" xmlns:xi="http://www.w3.org/2001/XInclude">
+    <devices>
+        <!-- interface -->
+        <xi:include href="interface/all_joints_mc.xml" />
+        <xi:include href="interface/root_imu.xml" />
+        <xi:include href="interface/wbd_left_foot.xml" />
+        <xi:include href="interface/wbd_right_foot.xml" />
+        <xi:include href="./test-sensor-bridge.xml" />
+    </devices>
+
+</robot>
+

devices/YarpSensorBridgeTestDevice/app/robots/iCubGazeboV2_5/test-sensor-bridge.xml

@@ -0,0 +1,47 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+
+<?xml version="1.0" encoding="UTF-8" ?>
+<device  xmlns:xi="http://www.w3.org/2001/XInclude" name="test-sensor-bridge" type="YarpSensorBridgeTestDevice">
+    <param name="robot">icubSim</param>
+    <param name="sampling_period_in_s">0.01</param>
+    <param name="port_prefix">/test-sensor-bridge</param>
+
+    <param name="joint_list">("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+
+    <group name="RobotSensorBridge">
+        <param name="check_for_nan">0</param>
+        <param name="stream_joint_states">1</param>
+        <param name="stream_inertials">1</param>
+        <param name="stream_cartesian_wrenches">1</param>
+        <param name="stream_forcetorque_sensors">0</param>
+        <param name="stream_cameras">0</param>
+
+        <group name="RemoteControlBoardRemapper">
+            <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+        </group>
+
+        <group name="InertialSensors">
+            <param name="imu_list">("root_link_imu_acc")</param>
+        </group>
+
+        <group name="CartesianWrenches">
+            <param name="cartesian_wrenches_list">("right_foot_cartesian_wrench", "left_foot_cartesian_wrench")</param>
+        </group>
+    </group>
+
+    <!-- ATTACH -->
+    <action phase="startup" level="15" type="attach">
+        <paramlist name="networks">
+            <elem name="all_joints">all_joints_mc</elem>
+            <elem name="root_link_imu_acc">root_link_imu_acc</elem>
+            <elem name="left_foot_cartesian_wrench">left_foot_cartesian_wrench</elem>
+            <elem name="right_foot_cartesian_wrench">right_foot_cartesian_wrench</elem>
+        </paramlist>
+    </action>
+
+    <action phase="shutdown" level="2" type="detach" />
+    <!-- FINISH ATTACH-->
+
+</device>

devices/YarpSensorBridgeTestDevice/include/BipedalLocomotion/YarpSensorBridgeTestDevice.h

@@ -0,0 +1,53 @@
+/**
+ * @file YarpSensorBridgeTestDevice.h
+ * @authors Prashanth Ramadoss
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_SENSOR_BRIDGE_TEST_DEVICE_H
+#define BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_SENSOR_BRIDGE_TEST_DEVICE_H
+
+#include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
+#include <BipedalLocomotion/RobotInterface/YarpSensorBridge.h>
+
+#include <yarp/os/PeriodicThread.h>
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/dev/Wrapper.h>
+#include <yarp/os/ResourceFinder.h>
+
+#include <mutex>
+#include <memory>
+
+
+namespace BipedalLocomotion
+{
+    class YarpSensorBridgeTestDevice;
+}
+
+class BipedalLocomotion::YarpSensorBridgeTestDevice : public yarp::dev::DeviceDriver,
+                                                      public yarp::dev::IMultipleWrapper,
+                                                      public yarp::os::PeriodicThread
+{
+public:
+    explicit YarpSensorBridgeTestDevice(double period, yarp::os::ShouldUseSystemClock useSystemClock = yarp::os::ShouldUseSystemClock::No);
+    YarpSensorBridgeTestDevice();
+    ~YarpSensorBridgeTestDevice();
+
+    virtual bool open(yarp::os::Searchable& config) final;
+    virtual bool close() final;
+    virtual bool attachAll(const yarp::dev::PolyDriverList & poly) final;
+    virtual bool detachAll() final;
+    virtual void run() final;
+
+private:
+    bool setupRobotSensorBridge(yarp::os::Searchable& config);
+    std::string m_robot{"test"};
+    std::string m_portPrefix{"/testYarpSensorBridge"};
+    std::unique_ptr<BipedalLocomotion::RobotInterface::YarpSensorBridge> m_robotSensorBridge;
+    std::mutex m_deviceMutex;
+};
+
+
+
+#endif //BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_SENSOR_BRIDGE_TEST_DEVICE_H

devices/YarpSensorBridgeTestDevice/src/YarpSensorBridgeTestDevice.cpp

@@ -0,0 +1,128 @@
+/**
+ * @file YarpSensorBridgeTestDevice.cpp
+ * @authors Prashanth Ramadoss
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <BipedalLocomotion/YarpSensorBridgeTestDevice.h>
+#include <BipedalLocomotion/YarpUtilities/Helper.h>
+#include <yarp/os/LogStream.h>
+
+using namespace BipedalLocomotion::YarpUtilities;
+using namespace BipedalLocomotion::ParametersHandler;
+using namespace BipedalLocomotion::RobotInterface;
+using namespace BipedalLocomotion;
+
+YarpSensorBridgeTestDevice::YarpSensorBridgeTestDevice(double period,
+                                                       yarp::os::ShouldUseSystemClock useSystemClock)
+: yarp::os::PeriodicThread(period, useSystemClock)
+{
+}
+
+
+YarpSensorBridgeTestDevice::YarpSensorBridgeTestDevice()
+        : yarp::os::PeriodicThread(0.01, yarp::os::ShouldUseSystemClock::No)
+{
+}
+
+YarpSensorBridgeTestDevice::~YarpSensorBridgeTestDevice()
+{
+}
+
+
+bool YarpSensorBridgeTestDevice::open(yarp::os::Searchable& config)
+{
+    YarpUtilities::getElementFromSearchable(config, "robot", m_robot);
+    YarpUtilities::getElementFromSearchable(config, "port_prefix", m_portPrefix);
+
+    double devicePeriod{0.01};
+
+    if (YarpUtilities::getElementFromSearchable(config, "sampling_period_in_s", devicePeriod))
+    {
+        setPeriod(devicePeriod);
+    }
+
+    if (!setupRobotSensorBridge(config))
+    {
+        return false;
+    }
+
+    return true;
+}
+
+bool YarpSensorBridgeTestDevice::setupRobotSensorBridge(yarp::os::Searchable& config)
+{
+    auto bridgeConfig = config.findGroup("RobotSensorBridge");
+    if (bridgeConfig.isNull())
+    {
+        yError() << "[YarpSensorBridgeTestDevice][setupRobotSensorBridge] Missing required group \"RobotSensorBridge\"";
+        return false;
+    }
+
+    std::shared_ptr<YarpImplementation> originalHandler = std::make_shared<YarpImplementation>();
+    originalHandler->set(bridgeConfig);
+
+    m_robotSensorBridge = std::make_unique<YarpSensorBridge>();
+    if (!m_robotSensorBridge->initialize(originalHandler))
+    {
+        yError() << "[YarpSensorBridgeTestDevice][setupRobotSensorBridge] Could not configure RobotSensorBridge";
+        return false;
+    }
+
+    return true;
+}
+
+
+bool YarpSensorBridgeTestDevice::attachAll(const yarp::dev::PolyDriverList & poly)
+{
+    if (!m_robotSensorBridge->setDriversList(poly))
+    {
+        yError() << "[YarpSensorBridgeTestDevice][attachAll] Could not attach drivers list to sensor bridge";
+        return false;
+    }
+
+    start();
+    return true;
+}
+
+
+void YarpSensorBridgeTestDevice::run()
+{
+    auto start = std::chrono::high_resolution_clock::now();
+    m_robotSensorBridge->advance();
+    auto end = std::chrono::high_resolution_clock::now();
+    double duration = std::chrono::duration_cast<std::chrono::microseconds>( end - start ).count();
+    yInfo() << "YarpSensorBridge advance() took " <<  duration << " us.";
+
+    Eigen::Matrix<double, 12, 1> imuMeasurement;
+    double time;
+    if (!m_robotSensorBridge->getIMUMeasurement("root_link_imu_acc", imuMeasurement, &time))
+    {
+        yError() << "Couldn't get IMU measurement";
+    }
+    else
+    {
+        std::cout << "IMU measurement: " << imuMeasurement.transpose() << std::endl;
+    }
+}
+
+
+bool YarpSensorBridgeTestDevice::detachAll()
+{
+    std::lock_guard<std::mutex> guard(m_deviceMutex);
+    if (isRunning())
+    {
+        stop();
+    }
+
+    return true;
+}
+
+
+bool YarpSensorBridgeTestDevice::close()
+{
+    std::lock_guard<std::mutex> guard(m_deviceMutex);
+    return true;
+}
+