added double laser plugins (WIP)

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,32 @@
+# 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
+ src/DoubleLaserDriver.cpp
+ )
+
+set(doubleLaser_headers include/gazebo/DoubleLaser.hh
+ include/yarp/dev/DoubleLaserDriver.h )
+
+
+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,68 @@
+/*
+ * 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_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 controlBoard 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
+///
+/// The device will receive the gazebo pointer, parse the model,
+/// and wait for yarp connections and the gazebo wait event.
+///
+class GazeboYarpDoubleLaser : public SensorPlugin
+{
+public:
+ GazeboYarpDoubleLaser();
+ virtual ~GazeboYarpDoubleLaser();
+
+ /**
+ * Saves the gazebo pointer, creates the device driver
+ */
+ void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
+
+private:
+ 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::os::Property m_parameters;
+
+ std::string m_sensorName;
+};
+
+}
+
+#endif

plugins/doublelaser/include/yarp/dev/DoubleLaserDriver.h

@@ -0,0 +1,132 @@
+/*
+ * 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 <yarp/os/Property.h>
+#include <yarp/dev/Drivers.h>
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/sig/Vector.h>
+#include <yarp/os/Time.h>
+#include <yarp/os/Semaphore.h>
+#include <yarp/os/Stamp.h>
+#include <yarp/dev/IRangefinder2D.h>
+#include <yarp/dev/Wrapper.h>
+
+#include <boost/shared_ptr.hpp>
+
+
+#include <gazebo/common/Time.hh>
+#include <gazebo/common/Plugin.hh>
+
+#include <string>
+#include <functional>
+#include <unordered_map>
+#include <vector>
+
+
+
+namespace yarp {
+ namespace dev {
+ class GazeboYarpDoubleLaserDriver;
+ };
+ }
+
+namespace gazebo {
+ namespace common {
+ class UpdateInfo;
+ }
+
+ namespace physics {
+ class Model;
+ class Joint;
+ typedef boost::shared_ptr<Joint> JointPtr;
+ }
+
+ namespace event {
+ class Connection;
+ typedef boost::shared_ptr<Connection> 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<LaserMeasurementData> &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