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..7a193a221 --- /dev/null +++ b/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}) diff --git a/plugins/doublelaser/include/gazebo/DoubleLaser.hh b/plugins/doublelaser/include/gazebo/DoubleLaser.hh new file mode 100644 index 000000000..75daf33cc --- /dev/null +++ b/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 + */ + +#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 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 diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc new file mode 100644 index 000000000..cd5130a29 --- /dev/null +++ b/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 +#include +#include + +#include + +#include +#include +#include +#include + + +/** + * @file DoubleLaser.h + * @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 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)); + + } + +}