From 753288d4a9943091b0080fba22864f5525220fa5 Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Fri, 29 Mar 2019 16:21:33 +0100 Subject: [PATCH] Developed Double laser plugin --- plugins/doublelaser/CMakeLists.txt | 7 +- .../doublelaser/include/gazebo/DoubleLaser.hh | 40 +-- .../include/yarp/dev/DoubleLaserDriver.h | 132 --------- plugins/doublelaser/src/DoubleLaser.cc | 269 +++++++++++------- plugins/doublelaser/src/DoubleLaserDriver.cpp | 159 ----------- 5 files changed, 188 insertions(+), 419 deletions(-) delete mode 100644 plugins/doublelaser/include/yarp/dev/DoubleLaserDriver.h delete mode 100644 plugins/doublelaser/src/DoubleLaserDriver.cpp diff --git a/plugins/doublelaser/CMakeLists.txt b/plugins/doublelaser/CMakeLists.txt index 03765e57b..7a193a221 100644 --- a/plugins/doublelaser/CMakeLists.txt +++ b/plugins/doublelaser/CMakeLists.txt @@ -10,12 +10,9 @@ message("sono nel doublelaser!!") include(AddGazeboYarpPluginTarget) -set(doubleLaser_source src/DoubleLaser.cc - src/DoubleLaserDriver.cpp - ) +set(doubleLaser_source src/DoubleLaser.cc ) -set(doubleLaser_headers include/gazebo/DoubleLaser.hh - include/yarp/dev/DoubleLaserDriver.h ) +set(doubleLaser_headers include/gazebo/DoubleLaser.hh ) set(LIB_COMMON_NAME gazebo_yarp_lib_common) diff --git a/plugins/doublelaser/include/gazebo/DoubleLaser.hh b/plugins/doublelaser/include/gazebo/DoubleLaser.hh index e97facfa5..75daf33cc 100644 --- a/plugins/doublelaser/include/gazebo/DoubleLaser.hh +++ b/plugins/doublelaser/include/gazebo/DoubleLaser.hh @@ -4,6 +4,11 @@ * 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 + */ + #ifndef GAZEBOYARP_DOUBLELASER_HH #define GAZEBOYARP_DOUBLELASER_HH @@ -22,20 +27,15 @@ namespace yarp { namespace gazebo { /// \class GazeboYarpDoubleLaser -/// Gazebo Plugin emulating the yarp controlBoard device in Gazebo. +/// 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 -/// containt the configuration parameters of the controlBoard -/// -/// The gazebo plugin is the "main" of the yarp device, -/// so what it should is to initialize the device, copy the -/// gazebo pointer, and return +/// containing the configuration parameters of the doublelaser /// -/// The device will receive the gazebo pointer, parse the model, -/// and wait for yarp connections and the gazebo wait event. -/// -class GazeboYarpDoubleLaser : public SensorPlugin + + +class GazeboYarpDoubleLaser : public ModelPlugin { public: GazeboYarpDoubleLaser(); @@ -44,23 +44,25 @@ public: /** * Saves the gazebo pointer, creates the device driver */ - void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); + 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::PolyDriverList m_doublelaser; //in this list there is only the doublelaser device - yarp::dev::PolyDriver m_driver_doublelaser; - yarp::dev::IMultipleWrapper* m_iWrap_doublelaser; //the wrapper interface of doublelaser - - - yarp::dev::PolyDriverList m_lasers;// the contains the pointer two laser (front and back) + + 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; }; } diff --git a/plugins/doublelaser/include/yarp/dev/DoubleLaserDriver.h b/plugins/doublelaser/include/yarp/dev/DoubleLaserDriver.h deleted file mode 100644 index e42bbdaa1..000000000 --- a/plugins/doublelaser/include/yarp/dev/DoubleLaserDriver.h +++ /dev/null @@ -1,132 +0,0 @@ -/* - * 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 - */ - -#ifndef GAZEBOYARP_DOUBLELASERDRIVER_HH -#define GAZEBOYARP_DOUBLELASERDRIVER_HH - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - - -#include -#include - -#include -#include -#include -#include - - - -namespace yarp { - namespace dev { - class GazeboYarpDoubleLaserDriver; - }; - } - -namespace gazebo { - namespace common { - class UpdateInfo; - } - - namespace physics { - class Model; - class Joint; - typedef boost::shared_ptr JointPtr; - } - - namespace event { - class Connection; - typedef boost::shared_ptr ConnectionPtr; - } -} - -extern const std::string YarpLaserSensorScopedName; - -class yarp::dev::GazeboYarpDoubleLaserDriver: - public yarp::dev::DeviceDriver, - public yarp::dev::IRangefinder2D, - public yarp::dev::IMultipleWrapper -{ -public: - - GazeboYarpDoubleLaserDriver(); - virtual ~GazeboYarpDoubleLaserDriver(); - - /** - * Gazebo stuff - */ - - /** - * Callback for the WorldUpdateBegin Gazebo event. - */ - void onUpdate(const gazebo::common::UpdateInfo&); - - /** - * Callback for the WorldReset Gazebo event. - */ - void onReset(); - - - /** - * Yarp interfaces start here - */ - - bool open(yarp::os::Searchable& config); - bool close(); - - //IMultipleWrapper interface - bool attachAll(const PolyDriverList &p) override; - bool detachAll() override; - - //IRangefinder2D interface - virtual bool getRawData(yarp::sig::Vector &data) override; - virtual bool getLaserMeasurement(std::vector &data) override; - virtual bool getDeviceStatus (Device_status &status) override; - virtual bool getDeviceInfo (std::string &device_info) override; // - virtual bool getDistanceRange (double& min, double& max) override; - virtual bool setDistanceRange (double min, double max) override; - virtual bool getScanLimits (double& min, double& max) override; - virtual bool setScanLimits (double min, double max) override; - virtual bool getHorizontalResolution (double& step) override; - virtual bool setHorizontalResolution (double step) override; - virtual bool getScanRate (double& rate) override; - virtual bool setScanRate (double rate) override; - -private: - - std::string m_deviceName; - gazebo::sensors::RaySensor* m_parentSensor; - - /** - * Connection to the WorldUpdateBegin Gazebo event - */ - gazebo::event::ConnectionPtr m_updateConnection; - - /** - * Connection to the WorldReset Gazebo event - */ - gazebo::event::ConnectionPtr m_resetConnection; - - yarp::os::Property m_pluginParameters; /**< Contains the parameters of the device contained in the yarpConfigurationFile .ini file */ - - bool m_started; - int m_clock; - gazebo::common::Time m_previousTime; - - yarp::os::Mutex m_mutex; //MI SERVE?????? -}; - -#endif //GAZEBOYARP_CONTROLBOARDDRIVER_HH diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc index 55b332e3d..cd5130a29 100644 --- a/plugins/doublelaser/src/DoubleLaser.cc +++ b/plugins/doublelaser/src/DoubleLaser.cc @@ -5,23 +5,29 @@ */ #include "DoubleLaser.hh" -#include "DoubleLaserDriver.h" #include #include #include -#include +#include + #include #include #include #include +/** + * @file DoubleLaser.h + * @authors: Valentina Gaggero + */ + using namespace std; + namespace gazebo { -GZ_REGISTER_SENSOR_PLUGIN(GazeboYarpDoubleLaser) +GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser) GazeboYarpDoubleLaser::GazeboYarpDoubleLaser() : m_iWrap_rangeFinder(0), m_iWrap_doublelaser(0) {} @@ -33,7 +39,7 @@ GZ_REGISTER_SENSOR_PLUGIN(GazeboYarpDoubleLaser) m_iWrap_doublelaser->detachAll(); m_iWrap_rangeFinder = 0; } - + if (m_iWrap_rangeFinder) { m_iWrap_rangeFinder->detachAll(); @@ -55,64 +61,90 @@ GZ_REGISTER_SENSOR_PLUGIN(GazeboYarpDoubleLaser) 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("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" <SetActive(true); - - m_sensorName = _sensor->ScopedName(); - - yError() << "GazeboYarpDoubleLaser : my name is " << m_sensorName; - - //Insert the pointer in the singleton handler for retriving it in the yarp driver - GazeboYarpPlugins::Handler::getHandler()->setSensor(_sensor.get()); + m_sensorName = _parent->GetScopedName(); + //3) load configuration from sdf + if(!readConfigurationFromFile( _parent, _sdf)) + return; - // Add the gazebo_controlboard device driver to the factory. - yarp::dev::Drivers::factory().add(new yarp::dev::DriverCreatorOf("gazebo_doubleLaser", "Rangefinder2DWrapper", "GazeboYarpDoubleLaserDriver")); + // 4) Insert the pointer in the singleton handler for retriving it in the yarp driver + GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent)); - //Getting .ini configuration file from sdf - bool configuration_loaded = false; - ::yarp::os::Property wrapper_properties; - ::yarp::os::Property driver_properties; - - configuration_loaded = GazeboYarpPlugins::loadConfigSensorPlugin(_sensor,_sdf,driver_properties); - - - if (!configuration_loaded) + //5) open wrapper Rangefinder2DWrapper + yarp::os::Property wrapper_parameters; + if(!m_parameters.check("WRAPPER")) { - yError() << "GazeboYarpDoubleLaser::Load error: unabble to load configuration"; + yError() << "GazeboYarpDoubleLaser: [WRAPPER] group is missing in configuration file"; return; - }; - - wrapper_properties = driver_properties; - + } - driver_properties.put(YarpLaserSensorScopedName.c_str(), m_sensorName.c_str()); - - //Open the wrapper - wrapper_properties.put("device","Rangefinder2DWrapper"); - if( m_wrapper_rangeFinder.open(wrapper_properties) ) + wrapper_parameters.fromString(m_parameters.findGroup("WRAPPER").toString()); + if(m_parameters.check("ROS")) { - yError() << "GazeboYarpDoubleLaser : ho aperto il wrapper Rangefinder2DWrapper "; - } + 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; @@ -123,83 +155,112 @@ GZ_REGISTER_SENSOR_PLUGIN(GazeboYarpDoubleLaser) yError("GazeboYarpDoubleLaser : wrapper interface not found, load failed. Rangefinder2DWrapper"); return; } - - //Open the driver DoubleLaser - driver_properties.put("device","gazebo_doubleLaser"); - if( m_driver_doublelaser.open(driver_properties) ) + + // 6) Open the driver DoubleLaser + yarp::os::Property doublelaser_dev_parameters; + if(!m_parameters.check("onSimulator")) { - - yError() << "GazeboYarpDoubleLaser : ho aperto il driver doubleLaser "; + yError() << "GazeboYarpDoubleLaser: onSimulator parameter is missing in configuration file"; + return; } - else + doublelaser_dev_parameters.put("onSimulator", m_parameters.find("onSimulator").asBool()); + + doublelaser_dev_parameters.put("device", "cerDoubleLidar"); + + if(!m_parameters.check("LASERFRONT-CFG")) { - yError()<<"GazeboYarpDoubleLaser Plugin failed: error in opening yarp driver doubleLaser"; + yError() << "GazeboYarpDoubleLaser: LASERFRONT-CFG group is missing in configuration file"; return; } - - - //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_doublelaser->attachAll(listofdoubellaser)) + + doublelaser_dev_parameters.addGroup("LASERFRONT-CFG").fromString(m_parameters.findGroup("LASERFRONT-CFG").toString()); + + if(!m_parameters.check("LASERBACK-CFG")) { - yError() << "GazeboYarpDoubleLaser: ho fatto attach di double laser a rangefinder wrapper. OK"; + yError() << "GazeboYarpDoubleLaser: LASERBACK-CFG group is missing in configuration file"; + return; } - else + 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: ERRORE mentre facevo attach di double laser a rangefinder wrapper."; + yError() << "GazeboYarpDoubleLaser: cannot find LASERFRONT-CFG.sensorName parameter"; + return; } - - //cerco i due sensori laser - yarp::dev::PolyDriverDescriptor laser1, laser2; - std::string scopedDeviceName_laser1 = m_sensorName + "::" + "laser_sensor"; - std::string scopedDeviceName_laser2 = m_sensorName + "::" + "laser_sensor2"; - laser1.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName_laser1); - if(laser1.poly == nullptr) + yarp::os::Bottle &back_name = m_parameters.findGroup("LASERBACK-CFG").findGroup("sensorName"); + if(back_name.isNull()) { - yError()<<"GazeboYarpDoubleLaser: non trovo laser1"; + yError() << "GazeboYarpDoubleLaser: cannot find LASERBACK-CFG.sensorName parameter"; return; } - laser2.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName_laser2); - if(laser2.poly == nullptr) + + + 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: non trovo laser2"; + yError() << "GazeboYarpDoubleLaser: cannot find laserFront device"; return; } - - - yError()<<"GazeboYarpDoubleLaser: ho trovato tutti e due i laser!! OK"; - - GazeboYarpPlugins::Handler::getHandler()->setDevice("laser_sensor", laser1.poly); - m_lasers.push(laser1); - - GazeboYarpPlugins::Handler::getHandler()->setDevice("laser_sensor2", laser2.poly); - m_lasers.push(laser2); - - - - if (!m_driver_doublelaser.view(m_iWrap_doublelaser)) - { - yError("GazeboYarpDoubleLaser : doublelaser device's wrapper interface not found, load failed."); + + m_driver_laserBack = GazeboYarpPlugins::Handler::getHandler()->getDevice(laserBack_name); + if(m_driver_laserBack == nullptr) + { + yError() << "GazeboYarpDoubleLaser: cannot find laserBack device"; return; - } - - yError("GazeboYarpDoubleLaser : ho fatto la view doublelaser."); - - if(m_iWrap_doublelaser->attachAll(m_lasers)) - { - yError("GazeboYarpDoubleLaser : ho fatto attach a tutti i laserOK."); - } - else - { - yError("GazeboYarpDoubleLaser : ERRORE mentre facevo attach a tutti i laser."); - } - //Insert the pointer in the singleton handler for retriving it in the yarp driver - GazeboYarpPlugins::Handler::getHandler()->setSensor(_sensor.get()); + } + + 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)); } diff --git a/plugins/doublelaser/src/DoubleLaserDriver.cpp b/plugins/doublelaser/src/DoubleLaserDriver.cpp deleted file mode 100644 index 1769e6ff9..000000000 --- a/plugins/doublelaser/src/DoubleLaserDriver.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/* - * 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 -#include "DoubleLaserDriver.h" -#include -#include - -#include -#include - - -#include -#include - -using namespace yarp::os; -using namespace yarp::sig; -using namespace yarp::dev; - - -GazeboYarpDoubleLaserDriver::GazeboYarpDoubleLaserDriver() : m_deviceName("") -{ - yWarning () <<"....GazeboYarpDoubleLaserDriver::sono nel costruttore..."; -} - -GazeboYarpDoubleLaserDriver::~GazeboYarpDoubleLaserDriver() {} - - -void GazeboYarpDoubleLaserDriver::onUpdate(const gazebo::common::UpdateInfo& _info) -{ - yWarning () <<"....GazeboYarpDoubleLaserDriver::onUpdate..."; -} - -void GazeboYarpDoubleLaserDriver::onReset() -{ -} - - -bool GazeboYarpDoubleLaserDriver::attachAll(const PolyDriverList &p) -{;} -bool GazeboYarpDoubleLaserDriver::detachAll() -{;} - -bool GazeboYarpDoubleLaserDriver::open(yarp::os::Searchable& config) -{ - yarp::os::LockGuard guard(m_mutex); - - //Get gazebo pointers - std::string sensorScopedName(config.find(YarpLaserSensorScopedName.c_str()).asString().c_str()); - - yError() << "GazeboYarpDoubleLaserDriver: sto cercando il sensore chiamato" << sensorScopedName ; - - m_parentSensor = dynamic_cast(GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName)); - m_started = true; - - if (!m_parentSensor) - { - yError() << "Error, sensor" << sensorScopedName << "was not found" ; - return false ; - } - - //Connect the driver to the gazebo simulation - this->m_updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboYarpDoubleLaserDriver::onUpdate, this, _1)); - - //DEVO AGGIUNGERLO??? - // Connect the onReset method to the WorldReset event callback - this->m_resetConnection = - gazebo::event::Events::ConnectWorldReset(boost::bind(&GazeboYarpDoubleLaserDriver::onReset, this)); - - //AGGIUNGI QUI ROBA PER LEGGERE LA CONFIGURAZIONE DEI LASER! - -} - -bool GazeboYarpDoubleLaserDriver::close() -{ - this->m_updateConnection.reset(); - return true; -} - - - -bool GazeboYarpDoubleLaserDriver::getDistanceRange(double& min, double& max) -{ - yarp::os::LockGuard guard(m_mutex); - return true; -} - -bool GazeboYarpDoubleLaserDriver::setDistanceRange(double min, double max) -{ - yarp::os::LockGuard guard(m_mutex); - return true; -} - -bool GazeboYarpDoubleLaserDriver::getScanLimits(double& min, double& max) -{ - yarp::os::LockGuard guard(m_mutex); - return true; -} - -bool GazeboYarpDoubleLaserDriver::setScanLimits(double min, double max) -{ - yarp::os::LockGuard guard(m_mutex); - yWarning("setScanLimits not yet implemented"); - return true; -} - -bool GazeboYarpDoubleLaserDriver::getHorizontalResolution(double& step) -{ - yarp::os::LockGuard guard(m_mutex); - return true; -} - -bool GazeboYarpDoubleLaserDriver::setHorizontalResolution(double step) -{ - yarp::os::LockGuard guard(m_mutex); - return true; -} - -bool GazeboYarpDoubleLaserDriver::getScanRate(double& rate) -{ - yarp::os::LockGuard guard(m_mutex); - yWarning("getScanRate not yet implemented"); - return true; -} - -bool GazeboYarpDoubleLaserDriver::setScanRate(double rate) -{ - yarp::os::LockGuard guard(m_mutex); - yWarning("setScanRate not yet implemented"); - return false; -} - - -bool GazeboYarpDoubleLaserDriver::getRawData(yarp::sig::Vector &out) -{ - yarp::os::LockGuard guard(m_mutex); - return true; -} - -bool GazeboYarpDoubleLaserDriver::getLaserMeasurement(std::vector &data) -{ - yarp::os::LockGuard guard(m_mutex); - return true; -} -bool GazeboYarpDoubleLaserDriver::getDeviceStatus(Device_status &status) -{ - yarp::os::LockGuard guard(m_mutex); - return true; -} - -bool GazeboYarpDoubleLaserDriver::getDeviceInfo (std::string &device_info) -{ - yarp::os::LockGuard guard(m_mutex); - return true; -} -