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 #include <gazebo/gazebo_config.h>
00037 
00038 namespace gazebo {
00039 
00040 GazeboRosSonar::GazeboRosSonar()
00041 {
00042 }
00043 
00045 // Destructor
00046 GazeboRosSonar::~GazeboRosSonar()
00047 {
00048   updateTimer.Disconnect(updateConnection);
00049   sensor_->SetActive(false);
00050 
00051   dynamic_reconfigure_server_.reset();
00052 
00053   node_handle_->shutdown();
00054   delete node_handle_;
00055 }
00056 
00058 // Load the controller
00059 void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
00060 {
00061   // Get then name of the parent sensor
00062 #if (GAZEBO_MAJOR_VERSION > 6)
00063   sensor_ = std::dynamic_pointer_cast<sensors::RaySensor>(_sensor);
00064 #else
00065   sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(_sensor);
00066 #endif
00067   if (!sensor_)
00068   {
00069     gzthrow("GazeboRosSonar requires a Ray Sensor as its parent");
00070     return;
00071   }
00072 
00073   // Get the world name.
00074 #if (GAZEBO_MAJOR_VERSION > 6)
00075   std::string worldName = sensor_->WorldName();
00076 #else
00077   std::string worldName = sensor_->GetWorldName();
00078 #endif
00079   world = physics::get_world(worldName);
00080 
00081   // default parameters
00082   namespace_.clear();
00083   topic_ = "sonar";
00084   frame_id_ = "/sonar_link";
00085 
00086   // load parameters
00087   if (_sdf->HasElement("robotNamespace"))
00088     namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
00089 
00090   if (_sdf->HasElement("frameId"))
00091     frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();
00092 
00093   if (_sdf->HasElement("topicName"))
00094     topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
00095 
00096   sensor_model_.Load(_sdf);
00097 
00098   range_.header.frame_id = frame_id_;
00099   range_.radiation_type = sensor_msgs::Range::ULTRASOUND;
00100 #if (GAZEBO_MAJOR_VERSION > 6)
00101   range_.field_of_view = std::min(fabs((sensor_->AngleMax() - sensor_->AngleMin()).Radian()), fabs((sensor_->VerticalAngleMax() - sensor_->VerticalAngleMin()).Radian()));
00102   range_.max_range = sensor_->RangeMax();
00103   range_.min_range = sensor_->RangeMin();
00104 #else
00105   range_.field_of_view = std::min(fabs((sensor_->GetAngleMax() - sensor_->GetAngleMin()).Radian()), fabs((sensor_->GetVerticalAngleMax() - sensor_->GetVerticalAngleMin()).Radian()));
00106   range_.max_range = sensor_->GetRangeMax();
00107   range_.min_range = sensor_->GetRangeMin();
00108 #endif
00109 
00110   // Make sure the ROS node for Gazebo has already been initialized
00111   if (!ros::isInitialized())
00112   {
00113     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00114       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00115     return;
00116   }
00117 
00118   node_handle_ = new ros::NodeHandle(namespace_);
00119   publisher_ = node_handle_->advertise<sensor_msgs::Range>(topic_, 1);
00120 
00121   // setup dynamic_reconfigure server
00122   dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_)));
00123   dynamic_reconfigure_server_->setCallback(boost::bind(&SensorModel::dynamicReconfigureCallback, &sensor_model_, _1, _2));
00124 
00125   Reset();
00126 
00127   // connect Update function
00128   updateTimer.setUpdateRate(10.0);
00129   updateTimer.Load(world, _sdf);
00130   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosSonar::Update, this));
00131 
00132   // activate RaySensor
00133   sensor_->SetActive(true);
00134 }
00135 
00136 void GazeboRosSonar::Reset()
00137 {
00138   updateTimer.Reset();
00139   sensor_model_.reset();
00140 }
00141 
00143 // Update the controller
00144 void GazeboRosSonar::Update()
00145 {
00146   common::Time sim_time = world->GetSimTime();
00147   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00148 
00149   // activate RaySensor if it is not yet active
00150   if (!sensor_->IsActive()) sensor_->SetActive(true);
00151 
00152   range_.header.stamp.sec  = (world->GetSimTime()).sec;
00153   range_.header.stamp.nsec = (world->GetSimTime()).nsec;
00154 
00155   // find ray with minimal range
00156   range_.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
00157 #if (GAZEBO_MAJOR_VERSION > 6)
00158   int num_ranges = sensor_->LaserShape()->GetSampleCount() * sensor_->LaserShape()->GetVerticalSampleCount();
00159 #else
00160   int num_ranges = sensor_->GetLaserShape()->GetSampleCount() * sensor_->GetLaserShape()->GetVerticalSampleCount();
00161 #endif
00162   for(int i = 0; i < num_ranges; ++i) {
00163 #if (GAZEBO_MAJOR_VERSION > 6)
00164     double ray = sensor_->LaserShape()->GetRange(i);
00165 #else
00166     double ray = sensor_->GetLaserShape()->GetRange(i);
00167 #endif
00168     if (ray < range_.range) range_.range = ray;
00169   }
00170 
00171   // add Gaussian noise (and limit to min/max range)
00172   if (range_.range < range_.max_range) {
00173     range_.range = sensor_model_(range_.range, dt);
00174     if (range_.range < range_.min_range) range_.range = range_.min_range;
00175     if (range_.range > range_.max_range) range_.range = range_.max_range;
00176   }
00177 
00178   publisher_.publish(range_);
00179 }
00180 
00181 // Register this plugin with the simulator
00182 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosSonar)
00183 
00184 } // namespace gazebo


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Sat Jun 8 2019 19:52:36