GazeboRosVelodyneLaser.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015-2018, Dataspeed Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Dataspeed Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #ifndef GAZEBO_ROS_VELODYNE_LASER_H_
36 #define GAZEBO_ROS_VELODYNE_LASER_H_
37 
38 // Use the same source code for CPU and GPU plugins
39 #ifndef GAZEBO_GPU_RAY
40 #define GAZEBO_GPU_RAY 0
41 #endif
42 
43 // Custom Callback Queue
44 #include <ros/ros.h>
45 #include <ros/callback_queue.h>
46 #include <ros/advertise_options.h>
47 
48 #include <sdf/Param.hh>
49 #include <gazebo/physics/physics.hh>
50 #include <gazebo/transport/TransportTypes.hh>
51 #include <gazebo/msgs/MessageTypes.hh>
52 
53 #include <gazebo/common/Time.hh>
54 #include <gazebo/common/Plugin.hh>
55 #include <gazebo/sensors/SensorTypes.hh>
56 #if GAZEBO_GPU_RAY
57 #include <gazebo/plugins/GpuRayPlugin.hh>
58 #else
59 #include <gazebo/plugins/RayPlugin.hh>
60 #endif
61 
62 #include <boost/algorithm/string/trim.hpp>
63 #include <boost/bind.hpp>
64 #include <boost/thread.hpp>
65 #include <boost/thread/mutex.hpp>
66 #include <boost/thread/lock_guard.hpp>
67 
68 #if GAZEBO_GPU_RAY
69 #define GazeboRosVelodyneLaser GazeboRosVelodyneGpuLaser
70 #define RayPlugin GpuRayPlugin
71 #define RaySensorPtr GpuRaySensorPtr
72 #endif
73 
74 namespace gazebo
75 {
76 
77  class GazeboRosVelodyneLaser : public RayPlugin
78  {
81  public: GazeboRosVelodyneLaser();
82 
84  public: ~GazeboRosVelodyneLaser();
85 
88  public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
89 
91  private: void ConnectCb();
92 
94  private: sensors::RaySensorPtr parent_ray_sensor_;
95 
97  private: ros::NodeHandle* nh_;
98 
101 
103  private: std::string topic_name_;
104 
106  private: std::string frame_name_;
107 
109  private: double min_intensity_;
110 
112  private: double min_range_;
113 
115  private: double max_range_;
116 
118  private: double gaussian_noise_;
119 
121  private: static double gaussianKernel(double mu, double sigma)
122  {
123  // using Box-Muller transform to generate two independent standard normally distributed normal variables
124  // see wikipedia
125  double U = (double)rand() / (double)RAND_MAX; // normalized uniform random variable
126  double V = (double)rand() / (double)RAND_MAX; // normalized uniform random variable
127  return sigma * (sqrt(-2.0 * ::log(U)) * cos(2.0 * M_PI * V)) + mu;
128  }
129 
131  private: boost::mutex lock_;
132 
134  private: std::string robot_namespace_;
135 
136  // Custom Callback Queue
138  private: void laserQueueThread();
139  private: boost::thread callback_laser_queue_thread_;
140 
141  // Subscribe to gazebo laserscan
142  private: gazebo::transport::NodePtr gazebo_node_;
143  private: gazebo::transport::SubscriberPtr sub_;
144  private: void OnScan(const ConstLaserScanStampedPtr &_msg);
145 
146  };
147 
148 } // namespace gazebo
149 
150 #endif /* GAZEBO_ROS_VELODYNE_LASER_H_ */
151 
void OnScan(const ConstLaserScanStampedPtr &_msg)
ros::NodeHandle * nh_
Pointer to ROS node.
sensors::RaySensorPtr parent_ray_sensor_
The parent ray sensor.
gazebo::transport::NodePtr gazebo_node_
static double gaussianKernel(double mu, double sigma)
Gaussian noise generator.
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
void ConnectCb()
Subscribe on-demand.
ros::Publisher pub_
ROS publisher.
double gaussian_noise_
Gaussian noise.
gazebo::transport::SubscriberPtr sub_
double min_range_
Minimum range to publish.
std::string robot_namespace_
For setting ROS name space.
double max_range_
Maximum range to publish.
std::string frame_name_
frame transform name, should match link name
double min_intensity_
the intensity beneath which points will be filtered
boost::mutex lock_
A mutex to lock access.


velodyne_gazebo_plugins
Author(s): Kevin Hallenbeck
autogenerated on Mon Mar 11 2019 02:27:05