Double laser plugin

plugins/CMakeLists.txt

@@ -30,6 +30,7 @@ endif()
 add_subdirectory(worldinterface)
 add_subdirectory(linkattacher)
 add_subdirectory(lasersensor)
+add_subdirectory(doublelaser)
 add_subdirectory(depthCamera)
 
 

plugins/doublelaser/CMakeLists.txt

@@ -0,0 +1,29 @@
+# Copyright (C) 2007-2015 Istituto Italiano di Tecnologia ADVR & iCub Facility & RBCS Department
+# Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro, Alessandro Settimi and Francesco Romano
+# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+
+cmake_minimum_required(VERSION 2.8.7)
+
+PROJECT(Plugin_DoubleLaser)
+
+message("sono nel doublelaser!!")
+
+include(AddGazeboYarpPluginTarget)
+
+set(doubleLaser_source      src/DoubleLaser.cc  )
+
+set(doubleLaser_headers     include/gazebo/DoubleLaser.hh )
+
+
+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 doublelaser
+                              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} RayPlugin ${Boost_LIBRARIES}
+                              HEADERS ${doubleLaser_headers}
+                              SOURCES ${doubleLaser_source})

plugins/doublelaser/include/gazebo/DoubleLaser.hh

@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2013-2015 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
+ */
+
+/**
+ * @file DoubleLaser.h
+ * @authors: Valentina Gaggero <[email protected]>
+ */
+
+#ifndef GAZEBOYARP_DOUBLELASER_HH
+#define GAZEBOYARP_DOUBLELASER_HH
+
+#include <gazebo/common/Plugin.hh>
+
+#include <string>
+
+#include <yarp/dev/PolyDriverList.h>
+
+namespace yarp {
+    namespace dev {
+        class IMultipleWrapper;
+    }
+}
+
+namespace gazebo
+{
+/// \class GazeboYarpDoubleLaser
+/// Gazebo Plugin emulating the yarp double laser device in Gazebo.
+///.
+/// It can be configurated using the yarpConfigurationFile sdf tag,
+/// that contains a Gazebo URI pointing at a yarp .ini configuration file
+/// containing the configuration parameters of the doublelaser
+///
+
+
+class GazeboYarpDoubleLaser : public ModelPlugin
+{
+public:
+    GazeboYarpDoubleLaser();
+    virtual ~GazeboYarpDoubleLaser();
+
+    /**
+     * Saves the gazebo pointer, creates the device driver
+     */
+    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+
+private:
+
+    bool readConfigurationFromFile(physics::ModelPtr _parent, sdf::ElementPtr _sdf); //Getting .ini configuration file from sdf
+    yarp::dev::PolyDriver m_wrapper_rangeFinder;
+    yarp::dev::IMultipleWrapper* m_iWrap_rangeFinder;
+
+     yarp::dev::PolyDriver m_driver_doublelaser;
+     yarp::dev::IMultipleWrapper* m_iWrap_doublelaser;
+
+    yarp::dev::PolyDriverList m_lasers;  //contains pointers of front and back laser
+
+    yarp::os::Property m_parameters;
+
+    std::string m_sensorName;
+
+    yarp::dev::PolyDriver * m_driver_laserFront;
+    yarp::dev::PolyDriver * m_driver_laserBack;
+};
+
+}
+
+#endif

plugins/doublelaser/src/DoubleLaser.cc

@@ -0,0 +1,267 @@
+/*
+ * Copyright (C) 2013-2015 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
+ */
+
+#include "DoubleLaser.hh"
+#include <GazeboYarpPlugins/common.h>
+#include <GazeboYarpPlugins/Handler.hh>
+#include <GazeboYarpPlugins/ConfHelpers.hh>
+
+#include <gazebo/physics/Model.hh>
+
+#include <yarp/dev/Wrapper.h>
+#include <yarp/os/Network.h>
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+
+
+/**
+ * @file DoubleLaser.h
+ * @authors: Valentina Gaggero <[email protected]>
+ */
+
+using namespace std;
+
+namespace gazebo
+{
+
+GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
+
+    GazeboYarpDoubleLaser::GazeboYarpDoubleLaser() : m_iWrap_rangeFinder(0), m_iWrap_doublelaser(0)
+    {}
+
+    GazeboYarpDoubleLaser::~GazeboYarpDoubleLaser()
+    {
+        if (m_iWrap_doublelaser) 
+        {
+            m_iWrap_doublelaser->detachAll();
+            m_iWrap_rangeFinder = 0;
+        }
+
+        if (m_iWrap_rangeFinder) 
+        {
+            m_iWrap_rangeFinder->detachAll();
+            m_iWrap_rangeFinder = 0;
+        }
+
+        if (m_wrapper_rangeFinder.isValid()) 
+        {
+            m_wrapper_rangeFinder.close();
+        }
+
+
+        for (int n = 0; n < m_lasers.size(); n++) {
+            std::string scopedDeviceName = m_sensorName + "::" + m_lasers[n]->key.c_str();
+            GazeboYarpPlugins::Handler::getHandler()->removeDevice(scopedDeviceName);
+        }
+
+        GazeboYarpPlugins::Handler::getHandler()->removeSensor(m_sensorName);
+        yarp::os::Network::fini();
+    }
+
+
+    bool GazeboYarpDoubleLaser::readConfigurationFromFile(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
+    {
+        if (!_sdf->HasElement("yarpConfigurationFile"))
+        {
+            yError() << "GazeboYarpDoubleLaser: error: unable to load configuration";
+            return false;
+        }
+
+        std::string ini_file_name = _sdf->Get<std::string>("yarpConfigurationFile");
+        std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name);
+
+        if (ini_file_path == "")
+        {
+            yError() << "GazeboYarpDoubleLaser: ini file path is empty";
+            return false;
+        }
+
+        GazeboYarpPlugins::addGazeboEnviromentalVariablesModel(_parent,_sdf,m_parameters);
+
+        bool wipe = false; //in order to not clear m_parameters
+        if (! m_parameters.fromConfigFile(ini_file_path.c_str(),wipe))
+        {
+            yError()  << "GazeboYarpDoubleLaser: error reading parameters from config file= " << ini_file_name << "in" <<ini_file_path ;
+            return false;
+        }
+
+        m_parameters.put("gazebo_ini_file_path",ini_file_path.c_str());
+
+        return true;
+    }
+
+
+
+    /**
+     * Saves the gazebo pointer, creates the device driver
+     */
+    void GazeboYarpDoubleLaser::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
+    {
+        // 1) check params
+        if (!_parent)
+        {
+            gzerr << "GazeboYarpDoubleLaser plugin requires a parent.\n";
+            return;
+        }
+
+        //2) check yarp network
+        yarp::os::Network::init();
+
+        if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout))
+        {
+            yError() << "GazeboYarpDoubleLaser : yarp network does not seem to be available, is the yarpserver running?";
+            return;
+        }
+
+        m_sensorName = _parent->GetScopedName();
+
+        //3) load configuration from sdf
+        if(!readConfigurationFromFile( _parent, _sdf))
+            return;
+
+        // 4) Insert the pointer in the singleton handler for retriving it in the yarp driver
+        GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));
+
+
+        //5) open wrapper Rangefinder2DWrapper
+        yarp::os::Property wrapper_parameters;
+        if(!m_parameters.check("WRAPPER"))
+        {
+            yError() << "GazeboYarpDoubleLaser: [WRAPPER] group is missing in configuration file";
+            return;
+        }
+
+        wrapper_parameters.fromString(m_parameters.findGroup("WRAPPER").toString());
+        if(m_parameters.check("ROS"))
+        {
+            wrapper_parameters.addGroup("ROS").fromString(m_parameters.findGroup("ROS").toString());
+        }
+        else
+        {
+            yInfo() << "GazeboYarpDoubleLaser: ROS group is missing in configuration file";
+        }
+
+        if(! m_wrapper_rangeFinder.open(wrapper_parameters))
+        {
+            yError()<<"GazeboYarpDoubleLaser failed: error in opening yarp driver wrapper Rangefinder2DWrapper!!!";
+            return;
+        }
+
+        if (!m_wrapper_rangeFinder.view(m_iWrap_rangeFinder)) 
+        {
+            yError("GazeboYarpDoubleLaser : wrapper interface not found, load failed. Rangefinder2DWrapper");
+            return;
+        }
+
+        // 6) Open the driver DoubleLaser
+        yarp::os::Property doublelaser_dev_parameters;
+        if(!m_parameters.check("onSimulator"))
+        {
+            yError() << "GazeboYarpDoubleLaser: onSimulator parameter is missing in configuration file";
+            return;
+        }
+        doublelaser_dev_parameters.put("onSimulator", m_parameters.find("onSimulator").asBool());
+
+        doublelaser_dev_parameters.put("device", "cerDoubleLidar");
+
+        if(!m_parameters.check("LASERFRONT-CFG"))
+        {
+            yError() << "GazeboYarpDoubleLaser: LASERFRONT-CFG group is missing in configuration file";
+            return;
+        }
+
+        doublelaser_dev_parameters.addGroup("LASERFRONT-CFG").fromString(m_parameters.findGroup("LASERFRONT-CFG").toString());
+
+        if(!m_parameters.check("LASERBACK-CFG"))
+        {
+            yError() << "GazeboYarpDoubleLaser: LASERBACK-CFG group is missing in configuration file";
+            return;
+        }
+        doublelaser_dev_parameters.addGroup("LASERBACK-CFG").fromString(m_parameters.findGroup("LASERBACK-CFG").toString());
+
+        if(!m_driver_doublelaser.open(doublelaser_dev_parameters) )
+        {
+            yError()<<"GazeboYarpDoubleLaser: error in opening yarp driver doubleLaser";
+            return;
+        }
+
+        // 7 )finds device of laser front and laser back. the device names are written in configuration .ini file
+        yarp::os::Bottle &front_name = m_parameters.findGroup("LASERFRONT-CFG").findGroup("sensorName");
+        if(front_name.isNull())
+        {
+            yError() << "GazeboYarpDoubleLaser: cannot find LASERFRONT-CFG.sensorName parameter";
+            return;
+        }
+         yarp::os::Bottle &back_name = m_parameters.findGroup("LASERBACK-CFG").findGroup("sensorName");
+        if(back_name.isNull())
+        {
+            yError() << "GazeboYarpDoubleLaser: cannot find LASERBACK-CFG.sensorName parameter";
+            return;
+        }
+
+
+        std::string laserFront_name = front_name.find("sensorName").asString();
+        std::string laserBack_name = back_name.find("sensorName").asString();
+
+
+        m_driver_laserFront = GazeboYarpPlugins::Handler::getHandler()->getDevice(laserFront_name);
+        if(m_driver_laserFront == nullptr)
+        {
+            yError() << "GazeboYarpDoubleLaser: cannot find laserFront device";
+            return;
+        }
+
+        m_driver_laserBack = GazeboYarpPlugins::Handler::getHandler()->getDevice(laserBack_name);
+        if(m_driver_laserBack == nullptr)
+        {
+            yError() << "GazeboYarpDoubleLaser: cannot find laserBack device";
+            return;
+        }
+
+        yarp::dev::PolyDriverList listoflasers; //it will contain front and back laser device pointers
+        yarp::dev::PolyDriverDescriptor laserFront_desc;
+        yarp::dev::PolyDriverDescriptor laserBack_desc;
+
+        laserFront_desc.poly = m_driver_laserFront;
+        laserFront_desc.key = laserFront_name;
+        listoflasers.push(laserFront_desc);
+
+        laserBack_desc.poly = m_driver_laserBack;
+        laserBack_desc.key = laserBack_name;
+        listoflasers.push(laserBack_desc);
+
+        m_driver_doublelaser.view(m_iWrap_doublelaser);
+        if(!m_iWrap_doublelaser->attachAll(listoflasers))
+        {
+            yError() << "GazeboYarpDoubleLaser: error douring attaching double laser to front and back laser devices";
+            m_driver_doublelaser.close();
+            return;
+        }
+
+
+        // 8 )attach rangefinder wrapper to double laser
+        yarp::dev::PolyDriverList listofdoubellaser; //it will contain only double laser
+        yarp::dev::PolyDriverDescriptor doublelaser_desc;
+        doublelaser_desc.poly = &m_driver_doublelaser;
+
+        listofdoubellaser.push(doublelaser_desc);
+
+        if(m_iWrap_rangeFinder->attachAll(listofdoubellaser))
+        {
+            yError() << "GazeboYarpDoubleLaser: ho fatto attach di rangeFinder wrapper al double laser. OK";
+        }
+        else
+        {
+            yError() << "GazeboYarpDoubleLaser: ERRORE mentre facevo attach di rangeFinder wrapper al double laser.";
+        }
+
+
+        //Insert the pointer in the singleton handler for retrieving it in the yarp driver
+        GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));
+
+     }
+
+}