leica_plugin.h

Go to the documentation of this file.

/*
   Desc: based on GazeboRosGpuLaser plugin for simulating ray sensors in Gazebo
   Author: Mihai Emanuel Dolha
   Date: 29 March 2012
*/

#pragma once
#ifndef _LEICA_PLUGIN_H
#define _LEICA_PLUGIN_H

#include <string>

#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <thread>

#include <sdf/sdf.hh>

#include <ros/ros.h>
#include <ros/advertise_options.h>
#include <sensor_msgs/LaserScan.h>
#include "std_msgs/Float32.h"
#include "sensor_msgs/RegionOfInterest.h"

#include <gazebo/physics/physics.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <gazebo/msgs/MessageTypes.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/Events.hh>
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/plugins/GpuRayPlugin.hh>
#include <gazebo_plugins/PubQueue.h>

#include <condition_variable>

namespace gazebo
{
class LeicaPlugin : public GpuRayPlugin
{
public:
    LeicaPlugin();

    ~LeicaPlugin();

    void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
    
    void configureScanWindow(const sensor_msgs::RegionOfInterestConstPtr &_msg);

private:
    void connectToLaser();

    void disconnectFromLaser();

    void gazeboMsgToROSMsg(ConstLaserScanStampedPtr &_msg);

    void callbacksQueueHelperThread();

    void gazeboLoaderThread();
    
    unsigned int seed_;

    bool start_laser_;

    int laser_connect_count_;

    std::string world_name_;
    
    physics::WorldPtr world_;

    std::string topic_name_;

    std::string frame_name_;

    std::string tf_prefix_;

    std::string namespace_;
    
    std::mutex mutex_;

    std::condition_variable condition_variable_;
    
    std::thread queue_helper_thread_;

    sensors::GpuRaySensorPtr parent_ray_sensor_;

    ros::NodeHandle *nh_;

    ros::Publisher pub_;

    ros::Subscriber sub_;

    ros::CallbackQueue cb_queue_;

    gazebo::transport::NodePtr gazebo_node_;

    gazebo::transport::SubscriberPtr laser_scan_sub_;

    PubQueue<sensor_msgs::LaserScan>::Ptr pub_queue_;

    PubMultiQueue pmq_;

    sdf::ElementPtr sdf_;

    boost::thread deferred_load_thread_;
};
}
#endif