diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt index 838882c25..cd156ffe2 100644 --- a/plugins/CMakeLists.txt +++ b/plugins/CMakeLists.txt @@ -30,6 +30,7 @@ endif() add_subdirectory(worldinterface) add_subdirectory(linkattacher) add_subdirectory(lasersensor) +add_subdirectory(doublelaser) add_subdirectory(depthCamera) diff --git a/plugins/doublelaser/CMakeLists.txt b/plugins/doublelaser/CMakeLists.txt new file mode 100644 index 000000000..8c7b78185 --- /dev/null +++ b/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}) diff --git a/plugins/doublelaser/include/gazebo/DoubleLaser.hh b/plugins/doublelaser/include/gazebo/DoubleLaser.hh new file mode 100644 index 000000000..ce8c37dcb --- /dev/null +++ b/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 + */ + +#ifndef GAZEBOYARP_DOUBLELASER_HH +#define GAZEBOYARP_DOUBLELASER_HH + +#include + +#include + +#include + +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 diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc new file mode 100644 index 000000000..6d4e9f476 --- /dev/null +++ b/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 +#include +#include + +#include + +#include +#include +#include +#include + + +/** + * @file DoubleLaser.cc + * @authors: Valentina Gaggero + */ + +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("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" <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)); + + } + +} diff --git a/plugins/lasersensor/src/LaserSensor.cc b/plugins/lasersensor/src/LaserSensor.cc index 73445bfc6..eea5a2087 100644 --- a/plugins/lasersensor/src/LaserSensor.cc +++ b/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;