gazebo_ros_gpu_laser.h
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  * Desc: A dynamic controller plugin that publishes a ROS point cloud or laser scan topic for generic ray sensor.
00023  * Author: Mihai Emanuel Dolha
00024  * Date: 29 March 2012
00025  * SVN: $Id$
00026  */
00027 #ifndef GAZEBO_ROS_GPU_LASER_HH
00028 #define GAZEBO_ROS_GPU_LASER_HH
00029 
00030 // ros stuff
00031 #include <ros/ros.h>
00032 #include <ros/callback_queue.h>
00033 #include <ros/advertise_options.h>
00034 
00035 #include <pcl_ros/point_cloud.h>
00036 #include <pcl/point_types.h>
00037 
00038 // ros messages stuff
00039 #include <sensor_msgs/PointCloud2.h>
00040 #include <sensor_msgs/LaserScan.h>
00041 
00042 #include <sensor_msgs/Image.h>
00043 #include "image_transport/image_transport.h"
00044 
00045 // gazebo stuff
00046 #include "sdf/interface/Param.hh"
00047 #include "physics/physics.h"
00048 #include "transport/TransportTypes.hh"
00049 #include "msgs/MessageTypes.hh"
00050 #include "common/Time.hh"
00051 #include "sensors/SensorTypes.hh"
00052 #include "plugins/GpuRayPlugin.hh"
00053 
00054 // dynamic reconfigure stuff
00055 #include <dynamic_reconfigure/server.h>
00056 
00057 // boost stuff
00058 #include "boost/thread/mutex.hpp"
00059 
00060 namespace gazebo
00061 {
00062   class GazeboRosGpuLaser : public GpuRayPlugin
00063   {
00066     public: GazeboRosGpuLaser();
00067 
00069     public: ~GazeboRosGpuLaser();
00070 
00073     public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
00074 
00075     public: void Init();
00076 
00078     protected: virtual void OnNewLaserFrame(const float *_image,
00079                    unsigned int _width, unsigned int _height,
00080                    unsigned int _depth, const std::string &_format);
00081 
00083     //protected: virtual void OnNewImageFrame(const unsigned char *_image,
00084     //               unsigned int _width, unsigned int _height,
00085     //               unsigned int _depth, unsigned int cam);
00086     //protected: void PutCameraData(const unsigned char *_src, unsigned int w, unsigned int h, unsigned int d, image_transport::Publisher *pub_);
00087     //
00089     //protected: image_transport::Publisher image_pub_;
00090     //protected: image_transport::Publisher image2_pub_;
00091     //protected: image_transport::Publisher image3_pub_;
00092     //protected: image_transport::Publisher image4_pub_;
00093     //private: image_transport::ImageTransport* itnode_;
00095     //protected: int imageConnectCount;
00096     //private: void ImageConnect();
00097     //private: void ImageDisconnect();
00098 
00099     protected: void PublishLaserScan(const float *_scan, unsigned int _width);
00100 
00101     protected: void PublishPointCloud(const float *_scan, unsigned int _width, unsigned int _height);
00102 
00104     private: double gaussian_noise_;
00105 
00107     private: double GaussianKernel(double mu,double sigma);
00108 
00110     //private: ParamT<double> *hokuyoMinIntensityP;
00111     private: double hokuyo_min_intensity_;
00112 
00114     private: int laser_connect_count_;
00115     private: void LaserConnect();
00116     private: void LaserDisconnect();
00117 
00119     private: ros::Publisher laser_scan_pub_;
00120 
00122     private: pcl::PointCloud<pcl::PointXYZI> point_cloud_msg_;
00123 
00124     private: sensor_msgs::LaserScan laser_scan_msg_;
00125 
00126     private: double point_cloud_cutoff_;
00127 
00129     private: std::string laser_topic_name_;
00130 
00131     // overload with our own
00132     private: common::Time sensor_update_time_;
00133 
00134     protected: ros::NodeHandle* rosnode_;
00135     private: std::string robot_namespace_;
00136 
00137     protected: ros::CallbackQueue queue_;
00138     protected: void QueueThread();
00139     protected: boost::thread callback_queue_thread_;
00140 
00141     protected: ros::WallTime last_publish_;
00142     // protected: std::ofstream timelog_;
00143     // protected: unsigned int logCount_;
00144 
00145     protected: std::string frame_name_;
00146     protected: double update_rate_;
00147     protected: double update_period_;
00148   };
00149 
00150 }
00151 #endif
00152 


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Sun Jan 5 2014 11:34:58