Adds range plugin for infrared and ultrasound sensors from PAL Robotics

gazebo_plugins/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
   polled_camera
   diagnostic_updater
   camera_info_manager
+  std_msgs
 )
 
 include (FindPkgConfig)
@@ -125,6 +126,7 @@ catkin_package(
   image_transport 
   rosconsole
   camera_info_manager
+  std_msgs
   DEPENDS 
     gazebo 
     SDF
@@ -249,6 +251,9 @@ target_link_libraries(gazebo_ros_hand_of_god ${GAZEBO_LIBRARIES} ${SDFormat_LIBR
 add_library(gazebo_ros_ft_sensor src/gazebo_ros_ft_sensor.cpp)
 target_link_libraries(gazebo_ros_ft_sensor ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
+add_library(gazebo_ros_range src/gazebo_ros_range.cpp)
+target_link_libraries(gazebo_ros_range ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} RayPlugin)
+
 ##
 ## Add your new plugin here
 ##
@@ -291,6 +296,7 @@ install(TARGETS
   gazebo_ros_planar_move
   pub_joint_trajectory_test  
   gazebo_ros_gpu_laser
+  gazebo_ros_range
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   )

gazebo_plugins/include/gazebo_plugins/gazebo_ros_range.h

@@ -0,0 +1,145 @@
+/*
+ * Software License Agreement (Modified BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PAL Robotics, S.L. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jose Capriles. */
+
+#ifndef GAZEBO_ROS_RANGE_H
+#define GAZEBO_ROS_RANGE_H
+
+
+#include <string>
+
+#include <boost/bind.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <ros/advertise_options.h>
+#include <sensor_msgs/Range.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/RayPlugin.hh>
+
+#include <sdf/Param.hh>
+
+namespace gazebo
+{
+
+class GazeboRosRange : public RayPlugin
+{
+
+    /// \brief Constructor
+    public: GazeboRosRange();
+
+    /// \brief Destructor
+    public: ~GazeboRosRange();
+
+    /// \brief Load the plugin
+    /// \param take in SDF root element
+    public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
+
+    /// \brief Update the controller
+    protected: virtual void OnNewLaserScans();
+
+    /// \brief Put range data to the ROS topic
+    private: void PutRangeData(common::Time &_updateTime);
+
+    /// \brief Keep track of number of connctions
+    private: int range_connect_count_;
+    private: void RangeConnect();
+    private: void RangeDisconnect();
+
+    // Pointer to the model
+    private: physics::WorldPtr world_;
+    /// \brief The parent sensor
+    private: sensors::SensorPtr parent_sensor_;
+    private: sensors::RaySensorPtr parent_ray_sensor_;
+
+    /// \brief pointer to ros node
+    private: ros::NodeHandle* rosnode_;
+    private: ros::Publisher pub_;
+
+    /// \brief ros message
+    private: sensor_msgs::Range range_msg_;
+
+    /// \brief topic name
+    private: std::string topic_name_;
+
+    /// \brief frame transform name, should match link name
+    private: std::string frame_name_;
+
+    /// \brief radiation type : ultrasound or infrared
+    private: std::string radiation_;
+
+    /// \brief sensor field of view
+    private: double fov_;
+    /// \brief Gaussian noise
+    private: double gaussian_noise_;
+
+    /// \brief Gaussian noise generator
+    private: double GaussianKernel(double mu, double sigma);
+
+    /// \brief mutex to lock access to fields that are used in message callbacks
+    private: boost::mutex lock_;
+
+    /// \brief hack to mimic hokuyo intensity cutoff of 100
+    private: double hokuyo_min_intensity_;
+
+    /// update rate of this sensor
+    private: double update_rate_;
+    private: double update_period_;
+    private: common::Time last_update_time_;
+
+    /// \brief for setting ROS name space
+    private: std::string robot_namespace_;
+
+    private: ros::CallbackQueue range_queue_;
+    private: void RangeQueueThread();
+    private: 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;
+};
+}
+#endif // GAZEBO_ROS_RANGE_H

gazebo_plugins/package.xml

@@ -41,6 +41,7 @@
   <build_depend>polled_camera</build_depend>
   <build_depend>diagnostic_updater</build_depend>
   <build_depend>camera_info_manager</build_depend>
+  <build_depend>std_msgs</build_depend>
 
   <run_depend>gazebo</run_depend>
   <run_depend>gazebo_msgs</run_depend>
@@ -66,5 +67,6 @@
   <run_depend>cv_bridge</run_depend>
   <run_depend>polled_camera</run_depend>
   <run_depend>camera_info_manager</run_depend>
+  <run_depend>std_msgs</run_depend>
 
 </package>