Combining Atlas code with old gazebo_plugins

gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h

@@ -1,152 +1,95 @@
 /*
- *  Gazebo - Outdoor Multi-Robot Simulator
- *  Copyright (C) 2003
- *     Nate Koenig & Andrew Howard
+ * Copyright 2012 Open Source Robotics Foundation
  *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
  *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
+ *     http://www.apache.org/licenses/LICENSE-2.0
  *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
  *
- */
-/*
- * Desc: A dynamic controller plugin that publishes a ROS point cloud or laser scan topic for generic ray sensor.
- * Author: Mihai Emanuel Dolha
- * Date: 29 March 2012
- * SVN: $Id$
- */
-#ifndef GAZEBO_ROS_GPU_LASER_HH
-#define GAZEBO_ROS_GPU_LASER_HH
-
-// ros stuff
-#include <ros/ros.h>
-#include <ros/callback_queue.h>
-#include <ros/advertise_options.h>
+*/
 
-#include <pcl_ros/point_cloud.h>
-#include <pcl/point_types.h>
+#ifndef GAZEBO_ROS_LASER_HH
+#define GAZEBO_ROS_LASER_HH
 
-// ros messages stuff
-#include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/LaserScan.h>
+#include <string>
 
-#include <sensor_msgs/Image.h>
-#include "image_transport/image_transport.h"
+#include <boost/bind.hpp>
+#include <boost/thread.hpp>
 
-// gazebo stuff
-#include "sdf/interface/Param.hh"
-#include "physics/physics.hh"
-#include "transport/TransportTypes.hh"
-#include "msgs/MessageTypes.hh"
-#include "common/Time.hh"
-#include "sensors/SensorTypes.hh"
-#include "plugins/GpuRayPlugin.hh"
+#include <ros/ros.h>
+#include <ros/advertise_options.h>
+#include <sensor_msgs/LaserScan.h>
 
-// dynamic reconfigure stuff
-#include <dynamic_reconfigure/server.h>
+#include <gazebo/sdf/interface/Param.hh>
+#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>
 
-// boost stuff
-#include "boost/thread/mutex.hpp"
+#include "PubQueue.h"
 
 namespace gazebo
 {
-  class GazeboRosGpuLaser : public GpuRayPlugin
+  class GazeboRosLaser : public GpuRayPlugin
   {
     /// \brief Constructor
-    /// \param parent The parent entity, must be a Model or a Sensor
-    public: GazeboRosGpuLaser();
+    public: GazeboRosLaser();
 
     /// \brief Destructor
-    public: ~GazeboRosGpuLaser();
+    public: ~GazeboRosLaser();
 
     /// \brief Load the plugin
     /// \param take in SDF root element
     public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
 
-    public: void Init();
-
-    /// \brief Update the controller
-    protected: virtual void OnNewLaserFrame(const float *_image,
-                   unsigned int _width, unsigned int _height,
-                   unsigned int _depth, const std::string &_format);
-
-    /// \brief Update the controller
-    //protected: virtual void OnNewImageFrame(const unsigned char *_image,
-    //               unsigned int _width, unsigned int _height,
-    //               unsigned int _depth, unsigned int cam);
-    //protected: void PutCameraData(const unsigned char *_src, unsigned int w, unsigned int h, unsigned int d, image_transport::Publisher *pub_);
-    //
-    /////// \brief ROS image message
-    //protected: image_transport::Publisher image_pub_;
-    //protected: image_transport::Publisher image2_pub_;
-    //protected: image_transport::Publisher image3_pub_;
-    //protected: image_transport::Publisher image4_pub_;
-    //private: image_transport::ImageTransport* itnode_;
-    ///// \brief Keep track of number of connctions
-    //protected: int imageConnectCount;
-    //private: void ImageConnect();
-    //private: void ImageDisconnect();
-
-    protected: void PublishLaserScan(const float *_scan, unsigned int _width);
-
-    protected: void PublishPointCloud(const float *_scan, unsigned int _width, unsigned int _height);
-
-    /// \brief Gaussian noise
-    private: double gaussian_noise_;
-
-    /// \brief Gaussian noise generator
-    private: double GaussianKernel(double mu,double sigma);
-
-    /// \brief hack to mimic hokuyo intensity cutoff of 100
-    //private: ParamT<double> *hokuyoMinIntensityP;
-    private: double hokuyo_min_intensity_;
-
-    /// \brief Keep track of number of connctions for point clouds
+    /// \brief Keep track of number of connctions
     private: int laser_connect_count_;
     private: void LaserConnect();
     private: void LaserDisconnect();
 
-    /// \brief A pointer to the ROS node.  A node will be instantiated if it does not exist.
-    private: ros::Publisher laser_scan_pub_;
-
-    /// \brief PCL point cloud message
-    private: pcl::PointCloud<pcl::PointXYZI> point_cloud_msg_;
-
-    private: sensor_msgs::LaserScan laser_scan_msg_;
+    // Pointer to the model
+    private: std::string world_name_;
+    private: physics::WorldPtr world_;
+    /// \brief The parent sensor
+    private: sensors::GpuRaySensorPtr parent_ray_sensor_;
 
-    private: double point_cloud_cutoff_;
+    /// \brief pointer to ros node
+    private: ros::NodeHandle* rosnode_;
+    private: ros::Publisher pub_;
+    private: PubQueue<sensor_msgs::LaserScan>::Ptr pub_queue_;
 
-    /// \brief ROS image topic name
-    private: std::string laser_topic_name_;
+    /// \brief topic name
+    private: std::string topic_name_;
 
-    // overload with our own
-    private: common::Time sensor_update_time_;
+    /// \brief frame transform name, should match link name
+    private: std::string frame_name_;
 
-    protected: ros::NodeHandle* rosnode_;
+    /// \brief for setting ROS name space
     private: std::string robot_namespace_;
 
-    protected: ros::CallbackQueue queue_;
-    protected: void QueueThread();
-    protected: boost::thread callback_queue_thread_;
+    // deferred load in case ros is blocking
+    private: sdf::ElementPtr sdf;
+    private: void LoadThread();
+    private: boost::thread deferred_load_thread_;
+    private: unsigned int seed;
 
-    protected: ros::WallTime last_publish_;
-    // protected: std::ofstream timelog_;
-    // protected: unsigned int logCount_;
+    private: gazebo::transport::NodePtr gazebo_node_;
+    private: gazebo::transport::SubscriberPtr laser_scan_sub_;
+    private: void OnScan(ConstLaserScanStampedPtr &_msg);
 
-    protected: std::string frame_name_;
-    protected: double update_rate_;
-    protected: double update_period_;
+    /// \brief prevents blocking
+    private: PubMultiQueue pmq;
   };
-
 }
 #endif
-