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,28 @@
+#Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia - iCub Facility
+#Authors: see AUTHORS file.
+#CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT
+
+
+cmake_minimum_required(VERSION 2.8.7)
+
+PROJECT(Plugin_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) 2019 Fondazione Istituto Italiano di Tecnologia - iCub Facility
+ * 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.hh
+ * @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 configured 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,264 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia - iCub Facility
+ * 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.cc
+ * @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 retrieving 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 opening yarp driver wrapper Rangefinder2DWrapper";
+ return;
+ }
+
+ if (!m_wrapper_rangeFinder.view(m_iWrap_rangeFinder)) 
+ {
+ yError("GazeboYarpDoubleLaser : wrapper (Rangefinder2DWrapper) interface not found, load failed.");
+ 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 opening DoubleLaser yarp device ";
+ 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 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: error attaching wrapper to double laser device";
+ return;
+ }
+
+
+ //Insert the pointer in the singleton handler for retrieving it in the yarp driver
+ GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));
+
+ }
+
+}

plugins/lasersensor/src/LaserSensor.cc

@@ -97,6 +97,19 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
  return;
  }
 
+ if(!driver_properties.check("deviceId"))
+ {
+ yError()<<"GazeboYarpLaserSensor Plugin failed: cannot find deviceId parameter in ini file.";
+ return;
+ }
+ std::string deviceId = driver_properties.find("deviceId").asString();
+
+ if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver))
+ {
+ yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")";
+ return;
+ }
+
  //Attach the driver to the wrapper
  ::yarp::dev::PolyDriverList driver_list;