Gazebo YARP plugin for iCub skin

plugins/CMakeLists.txt

@@ -27,6 +27,7 @@ add_subdirectory(contactloadcellarray)
 add_subdirectory(modelposepublisher)
 add_subdirectory(basestate)
 add_subdirectory(configurationoverride)
+add_subdirectory(skin)
 
 if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
   add_subdirectory(robotinterface)

plugins/skin/CMakeLists.txt

@@ -0,0 +1,13 @@
+PROJECT(Plugin_Skin)
+
+include(AddGazeboYarpPluginTarget)
+
+
+add_gazebo_yarp_plugin_target(LIBRARY_NAME skin
+                              INCLUDE_DIRS include/gazebo include/yarp/dev
+                              SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS}
+                              LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} RayPlugin  ${Boost_LIBRARIES}
+                              HEADERS include/gazebo/GazeboYarpSkin.hh
+                              SOURCES src/GazeboYarpSkin.cc
+                                 )
+

plugins/skin/include/gazebo/GazeboYarpSkin.hh

@@ -0,0 +1,97 @@
+#ifndef GAZEBOYARP_SKIN_HH
+#define GAZEBOYARP_SKIN_HH
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/transport/Node.hh>
+#include <gazebo/transport/Subscriber.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo/msgs/msgs.hh>
+
+#include <GazeboYarpPlugins/Handler.hh>
+#include <GazeboYarpPlugins/common.h>
+#include <GazeboYarpPlugins/ConfHelpers.hh>
+
+#include <yarp/os/Bottle.h>
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/Network.h>
+#include <yarp/os/Port.h>
+#include <yarp/os/Time.h>
+
+#include <string>
+#include <iostream>
+#include <regex>
+#include <math.h>
+#include <cstring>
+
+#define N_BODY_PARTS 7
+#define N_TAXELS_TORSO 768
+#define N_TAXELS_ARM 768
+#define N_TAXELS_FOREARM 384
+#define N_TAXELS_HAND 192
+#define MAX_TAXEL_FORCE 0.1
+#define TAXEL_DEFAULT_VALUE 0.0
+
+using yarp::os::Bottle;
+using yarp::os::Network;
+using yarp::os::Port;
+
+namespace gazebo
+{
+    /// \class GazeboYarpSkin
+    /// Gazebo Plugin emulating the iCub skin in Gazebo.
+    /// 
+    /// This model plugin subscribes to ~/physics/contacts Gazebo topic,
+    /// filters out skin contacts using regular expression on contact names,
+    /// and forwards all detected skin contacts to corresponding YARP ports.
+
+    class GazeboYarpSkin : public ModelPlugin
+    {
+    public:
+        GazeboYarpSkin();
+        virtual ~GazeboYarpSkin();
+        virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+        void OnContact(ConstContactsPtr &_msg);
+
+    private:
+        physics::ModelPtr model;
+        event::ConnectionPtr updateConnection;
+        transport::SubscriberPtr sub;
+        common::Time lastSendTime;
+        std::vector<double> skin[N_BODY_PARTS];
+        int taxelCounts[N_BODY_PARTS] = {
+            N_TAXELS_ARM,
+            N_TAXELS_FOREARM,
+            N_TAXELS_HAND,
+            N_TAXELS_ARM,
+            N_TAXELS_FOREARM,
+            N_TAXELS_HAND,
+            N_TAXELS_TORSO
+        };
+        std::string portNames[N_BODY_PARTS] = {
+            "/icubSim/skin/left_arm_comp",
+            "/icubSim/skin/left_forearm_comp",
+            "/icubSim/skin/left_hand_comp",
+            "/icubSim/skin/right_arm_comp",
+            "/icubSim/skin/right_forearm_comp",
+            "/icubSim/skin/right_hand_comp",
+            "/icubSim/skin/torso_comp"
+        };
+        std::string collisionPatterns[N_BODY_PARTS] = {
+            "iCub::left_upper_arm_skin::left_upper_arm_skin_collision_([0-9]+).*",
+            "iCub::left_forearm_skin::left_forearm_skin_collision_([0-9]+).*",
+            "iCub::left_hand_skin::left_hand_skin_collision_([0-9]+).*",
+            "iCub::right_upper_arm_skin::right_upper_arm_skin_collision_([0-9]+).*",
+            "iCub::right_forearm_skin::right_forearm_skin_collision_([0-9]+).*",
+            "iCub::right_hand_skin::right_hand_skin_collision_([0-9]+).*",
+            "iCub::chest_skin::chest_skin_collision_([0-9]+).*"
+        };
+        Port outputPorts[N_BODY_PARTS];
+        virtual void OnUpdate();
+        void updateLastSendTime();
+  };
+}
+#endif
+

plugins/skin/src/GazeboYarpSkin.cc

@@ -0,0 +1,120 @@
+#include "GazeboYarpSkin.hh"
+
+using namespace gazebo;
+using namespace transport;
+
+using yarp::os::Bottle;
+using yarp::os::Network;
+using yarp::os::Port;
+
+GZ_REGISTER_MODEL_PLUGIN(GazeboYarpSkin)
+
+namespace gazebo {
+
+GazeboYarpSkin::GazeboYarpSkin() : ModelPlugin()
+{  
+}
+
+GazeboYarpSkin::~GazeboYarpSkin()
+{
+    // Close YARP ports
+    for (int i = 0; i < N_BODY_PARTS; i++) {
+        this->outputPorts[i].close();
+    }
+}
+
+void GazeboYarpSkin::updateLastSendTime()
+{
+    this->lastSendTime = this->model->GetWorld()->RealTime();
+}
+
+void GazeboYarpSkin::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
+{
+    // Initialize YARP network
+    // yarp::os::Network::init();
+    // if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) {
+    //     yError() << "GazeboYarpSkin::Load error: yarp network does not seem to be available, is the yarpserver running?";
+    //     return;
+    // }
+
+    // Store reference to the model
+    this->model = _parent;
+
+    // Set time of last sent skin data
+    this->updateLastSendTime();
+
+    // Setup YARP ports
+    for (int i = 0; i < N_BODY_PARTS; i++) {
+        this->outputPorts[i].open(this->portNames[i]);
+        this->skin[i].resize(this->taxelCounts[i]);
+        std::fill(this->skin[i].begin(), this->skin[i].end(), TAXEL_DEFAULT_VALUE);
+    }
+
+    // Listen to the update event
+    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+        std::bind(&GazeboYarpSkin::OnUpdate, this)
+    );
+
+    // Subscribe to ~/physics/contacts Gazebo topic
+    NodePtr node(new Node());
+    node->Init();
+    this->sub = node->Subscribe("~/physics/contacts", &GazeboYarpSkin::OnContact, this);
+}
+
+void GazeboYarpSkin::OnContact(ConstContactsPtr &_msg)
+{
+    if (_msg->contact_size() > 0) {
+        int timeSec = _msg->time().sec();
+        int timeNsec = _msg->time().nsec();
+
+        for (unsigned int i = 0; i < _msg->contact_size(); ++i) {
+            std::cmatch cm;
+            std::string collisionName1 = _msg->contact(i).collision1();
+            std::string collisionName2 = _msg->contact(i).collision2();
+
+            for (int j = 0; j < N_BODY_PARTS; j++) {
+                std::regex_match(collisionName1.c_str(), cm, std::regex(this->collisionPatterns[j]));
+                if (!cm.size()) {
+                    std::regex_match(collisionName2.c_str(), cm, std::regex(this->collisionPatterns[j]));
+                }
+                if (cm.size()) {
+                    int taxelNumber = stoi(cm[1]);
+                    msgs::Vector3d f = _msg->contact(i).wrench(0).body_1_wrench().force();
+                    double force = sqrt(f.x() * f.x() + f.y() * f.y() + f.z() * f.z());
+                    double taxelValue = std::min(
+                        255.0,
+                        ((force / MAX_TAXEL_FORCE) * 255.0)
+                    );
+                    this->skin[j][taxelNumber] = taxelValue;
+                }
+            }
+        }
+    }
+}
+
+void GazeboYarpSkin::OnUpdate()
+{
+    // Get time difference from last skin data
+    common::Time diff = this->model->GetWorld()->RealTime() - this->lastSendTime;
+
+    // Send skin data at 10 Hz
+    // Note: for 50 Hz, use nsec value 20000000
+    if (diff.sec > 0 || diff.nsec >= 100000000) {
+        // Update last time skin update was sent
+        this->updateLastSendTime();
+
+        // Send YARP data
+        for (int i = 0; i < N_BODY_PARTS; i++) {
+            Bottle bot;
+            for (int j = 0; j < this->taxelCounts[i]; j++) {
+                bot.addFloat64(this->skin[i][j]);
+            }
+            this->outputPorts[i].write(bot);
+
+            // Reset taxel data
+            std::fill(this->skin[i].begin(), this->skin[i].end(), TAXEL_DEFAULT_VALUE);
+        }
+    }
+}
+
+}