gazebo_ros_sonar.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_gazebo_plugins/gazebo_ros_sonar.h>
00030 #include <gazebo/common/Events.hh>
00031 #include <gazebo/physics/physics.hh>
00032 #include <gazebo/sensors/RaySensor.hh>
00033 
00034 #include <limits>
00035 
00036 namespace gazebo {
00037 
00038 GazeboRosSonar::GazeboRosSonar()
00039 {
00040 }
00041 
00043 // Destructor
00044 GazeboRosSonar::~GazeboRosSonar()
00045 {
00046   updateTimer.Disconnect(updateConnection);
00047   sensor_->SetActive(false);
00048   node_handle_->shutdown();
00049   delete node_handle_;
00050 }
00051 
00053 // Load the controller
00054 void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
00055 {
00056   // Get then name of the parent sensor
00057   sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(_sensor);
00058   if (!sensor_)
00059   {
00060     gzthrow("GazeboRosSonar requires a Ray Sensor as its parent");
00061     return;
00062   }
00063 
00064   // Get the world name.
00065   std::string worldName = sensor_->GetWorldName();
00066   world = physics::get_world(worldName);
00067 
00068   // load parameters
00069   if (!_sdf->HasElement("robotNamespace"))
00070     namespace_.clear();
00071   else
00072     namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00073 
00074   if (!_sdf->HasElement("frameId"))
00075     frame_id_ = "";
00076   else
00077     frame_id_ = _sdf->GetElement("frameId")->GetValueString();
00078 
00079   if (!_sdf->HasElement("topicName"))
00080     topic_ = "sonar";
00081   else
00082     topic_ = _sdf->GetElement("topicName")->GetValueString();
00083 
00084   sensor_model_.Load(_sdf);
00085 
00086   range_.header.frame_id = frame_id_;
00087   range_.radiation_type = sensor_msgs::Range::ULTRASOUND;
00088   range_.field_of_view = std::min(fabs((sensor_->GetAngleMax() - sensor_->GetAngleMin()).GetAsRadian()), fabs((sensor_->GetVerticalAngleMax() - sensor_->GetVerticalAngleMin()).GetAsRadian()));
00089   range_.max_range = sensor_->GetRangeMax();
00090   range_.min_range = sensor_->GetRangeMin();
00091 
00092   // start ros node
00093   if (!ros::isInitialized())
00094   {
00095     int argc = 0;
00096     char** argv = NULL;
00097     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00098   }
00099 
00100   node_handle_ = new ros::NodeHandle(namespace_);
00101   publisher_ = node_handle_->advertise<sensor_msgs::Range>(topic_, 1);
00102 
00103   Reset();
00104 
00105   // connect Update function
00106   updateTimer.setUpdateRate(10.0);
00107   updateTimer.Load(world, _sdf);
00108   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosSonar::Update, this));
00109 
00110   // activate RaySensor
00111   sensor_->SetActive(true);
00112 }
00113 
00114 void GazeboRosSonar::Reset()
00115 {
00116   updateTimer.Reset();
00117   sensor_model_.reset();
00118 }
00119 
00121 // Update the controller
00122 void GazeboRosSonar::Update()
00123 {
00124   common::Time sim_time = world->GetSimTime();
00125   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00126 
00127   // activate RaySensor if it is not yet active
00128   if (!sensor_->IsActive()) sensor_->SetActive(true);
00129 
00130   range_.header.stamp.sec  = (world->GetSimTime()).sec;
00131   range_.header.stamp.nsec = (world->GetSimTime()).nsec;
00132 
00133   // find ray with minimal range
00134   range_.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
00135   int num_ranges = sensor_->GetLaserShape()->GetSampleCount() * sensor_->GetLaserShape()->GetVerticalSampleCount();
00136   for(int i = 0; i < num_ranges; ++i) {
00137     double ray = sensor_->GetLaserShape()->GetRange(i);
00138     if (ray < range_.range) range_.range = ray;
00139   }
00140 
00141   // add Gaussian noise (and limit to min/max range)
00142   if (range_.range < range_.max_range) {
00143     range_.range += sensor_model_.update(dt);
00144     if (range_.range < range_.min_range) range_.range = range_.min_range;
00145     if (range_.range > range_.max_range) range_.range = range_.max_range;
00146   }
00147 
00148   publisher_.publish(range_);
00149 }
00150 
00151 // Register this plugin with the simulator
00152 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosSonar)
00153 
00154 } // namespace gazebo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher and Johannes Meyer
autogenerated on Mon Jul 15 2013 16:46:30