Combining Atlas code with old gazebo_plugins

gazebo_plugins/include/gazebo_plugins/PubQueue.h

@@ -0,0 +1,191 @@
+/*
+ * Copyright 2012 Open Source Robotics Foundation
+ *
+ * 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
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * 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.
+ *
+ */
+
+#ifndef ROS_PUBQUEUE_H
+#define ROS_PUBQUEUE_H
+
+#include <boost/thread.hpp>
+#include <boost/bind.hpp>
+#include <deque>
+#include <list>
+#include <vector>
+
+#include <ros/ros.h>
+
+
+/// \brief Container for a (ROS publisher, outgoing message) pair.  
+/// We'll have queues of these.  Templated on a ROS message type.
+template<class T>
+class PubMessagePair
+{
+  public:
+    /// \brief The outgoing message.
+    T msg_;
+    /// \brief The publisher to use to publish the message.
+    ros::Publisher pub_;
+    PubMessagePair(T& msg, ros::Publisher& pub) :
+      msg_(msg), pub_(pub) {}
+};
+
+/// \brief A queue of outgoing messages.  Instead of calling publish() directly,
+/// you can push() messages here to defer ROS serialization and locking.
+/// Templated on a ROS message type.
+template<class T>
+class PubQueue
+{
+  public:
+    typedef boost::shared_ptr<std::deque<boost::shared_ptr<
+      PubMessagePair<T> > > > QueuePtr;
+    typedef boost::shared_ptr<PubQueue<T> > Ptr;
+
+  private:
+    /// \brief Our queue of outgoing messages.
+    QueuePtr queue_;
+    /// \brief Mutex to control access to the queue.
+    boost::shared_ptr<boost::mutex> queue_lock_;
+    /// \brief Function that will be called when a new message is pushed on.
+    boost::function<void()> notify_func_;
+
+  public:
+    PubQueue(QueuePtr queue, 
+             boost::shared_ptr<boost::mutex> queue_lock,
+             boost::function<void()> notify_func) :
+      queue_(queue), queue_lock_(queue_lock), notify_func_(notify_func) {}
+    ~PubQueue() {}
+
+    /// \brief Push a new message onto the queue.
+    /// \param[in] msg The outgoing message
+    /// \param[in] pub The ROS publisher to use to publish the message
+    void push(T& msg, ros::Publisher& pub)
+    {
+      boost::shared_ptr<PubMessagePair<T> > el(new PubMessagePair<T>(msg, pub));
+      boost::mutex::scoped_lock lock(*queue_lock_);
+      queue_->push_back(el);
+      notify_func_();
+    }
+
+    /// \brief Pop all waiting messages off the queue.
+    /// \param[out] els Place to store the popped messages
+    void pop(std::vector<boost::shared_ptr<PubMessagePair<T> > >& els)
+    {
+      boost::mutex::scoped_lock lock(*queue_lock_);
+      while(!queue_->empty())
+      {
+        els.push_back(queue_->front());
+        queue_->pop_front();
+      }
+    }
+};
+
+/// \brief A collection of PubQueue objects, potentially of different types.
+/// This class is the programmer's interface to this queuing system.
+class PubMultiQueue
+{
+  private:
+    /// \brief List of functions to be called to service our queues.
+    std::list<boost::function<void()> > service_funcs_;
+    /// \brief Mutex to lock access to service_funcs_
+    boost::mutex service_funcs_lock_;
+    /// \brief If started, the thread that will call the service functions
+    boost::thread service_thread_;
+    /// \brief Condition variable used to block and resume service_thread_
+    boost::condition_variable service_cond_var_;
+    /// \brief Mutex to accompany service_cond_var_
+    boost::mutex service_cond_var_lock_;
+
+    /// \brief Service a given queue by popping outgoing message off it and
+    /// publishing them.
+    template <class T> 
+    void serviceFunc(boost::shared_ptr<PubQueue<T> > pq)
+    {
+      std::vector<boost::shared_ptr<PubMessagePair<T> > > els;
+      pq->pop(els);
+      for(typename std::vector<boost::shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
+          it != els.end();
+          ++it)
+      {
+        (*it)->pub_.publish((*it)->msg_);
+      }
+    }
+
+  public:
+    PubMultiQueue() {}
+    ~PubMultiQueue() 
+    {
+      if(service_thread_.joinable())
+      {
+        notifyServiceThread();
+        service_thread_.join();
+      }
+    }
+
+    /// \brief Add a new queue.  Call this once for each published topic (or at
+    /// least each type of publish message).
+    /// \return Pointer to the newly created queue, good for calling push() on.
+    template <class T>
+    boost::shared_ptr<PubQueue<T> > addPub()
+    {
+      typename PubQueue<T>::QueuePtr queue(new std::deque<boost::shared_ptr<PubMessagePair<T> > >);
+      boost::shared_ptr<boost::mutex> queue_lock(new boost::mutex);
+      boost::shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this)));
+      boost::function<void()> f = boost::bind(&PubMultiQueue::serviceFunc<T>, this, pq);
+      {
+        boost::mutex::scoped_lock lock(service_funcs_lock_);
+        service_funcs_.push_back(f);
+      }
+      return pq;
+    }
+
+    /// \brief Service each queue one time.
+    void spinOnce()
+    {
+      boost::mutex::scoped_lock lock(service_funcs_lock_);
+      for(std::list<boost::function<void()> >::iterator it = service_funcs_.begin();
+          it != service_funcs_.end();
+          ++it)
+      {
+        (*it)();
+      }
+    }
+
+    /// \brief Service all queues indefinitely, waiting on a condition variable
+    /// in between cycles.
+    void spin()
+    {
+      while(ros::ok())
+      {
+        boost::unique_lock<boost::mutex> lock(service_cond_var_lock_);
+        service_cond_var_.wait(lock);
+        spinOnce();
+      }
+    }
+
+    /// \brief Start a thread to call spin().
+    void startServiceThread()
+    {
+      service_thread_ = boost::thread(boost::bind(&PubMultiQueue::spin, this));
+    }
+
+    /// \brief Wake up the queue serive thread (e.g., after having pushed a
+    /// message onto one of the queues).
+    void notifyServiceThread()
+    {
+      service_cond_var_.notify_one();
+    }
+};
+
+#endif

gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser_old.h

@@ -0,0 +1,152 @@
+/*
+ *  Gazebo - Outdoor Multi-Robot Simulator
+ *  Copyright (C) 2003
+ *     Nate Koenig & Andrew Howard
+ *
+ *  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.
+ *
+ *  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.
+ *
+ *  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
+ *
+ */
+/*
+ * 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>
+
+// ros messages stuff
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <sensor_msgs/Image.h>
+#include "image_transport/image_transport.h"
+
+// 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"
+
+// dynamic reconfigure stuff
+#include <dynamic_reconfigure/server.h>
+
+// boost stuff
+#include "boost/thread/mutex.hpp"
+
+namespace gazebo
+{
+  class GazeboRosGpuLaser : public GpuRayPlugin
+  {
+    /// \brief Constructor
+    /// \param parent The parent entity, must be a Model or a Sensor
+    public: GazeboRosGpuLaser();
+
+    /// \brief Destructor
+    public: ~GazeboRosGpuLaser();
+
+    /// \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
+    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_;
+
+    private: double point_cloud_cutoff_;
+
+    /// \brief ROS image topic name
+    private: std::string laser_topic_name_;
+
+    // overload with our own
+    private: common::Time sensor_update_time_;
+
+    protected: ros::NodeHandle* rosnode_;
+    private: std::string robot_namespace_;
+
+    protected: ros::CallbackQueue queue_;
+    protected: void QueueThread();
+    protected: boost::thread callback_queue_thread_;
+
+    protected: ros::WallTime last_publish_;
+    // protected: std::ofstream timelog_;
+    // protected: unsigned int logCount_;
+
+    protected: std::string frame_name_;
+    protected: double update_rate_;
+    protected: double update_period_;
+  };
+
+}
+#endif
+