gazebo_ros_sonar.cpp
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Flight Systems and Automatic Control group,
13 // TU Darmstadt, nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
30 #include <gazebo/common/Events.hh>
31 #include <gazebo/physics/physics.hh>
32 #include <gazebo/sensors/RaySensor.hh>
33 
34 #include <limits>
35 
36 #include <gazebo/gazebo_config.h>
37 
38 namespace gazebo {
39 
41 {
42 }
43 
45 // Destructor
47 {
49  sensor_->SetActive(false);
50 
52 
54  delete node_handle_;
55 }
56 
58 // Load the controller
59 void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
60 {
61  // Get then name of the parent sensor
62 #if (GAZEBO_MAJOR_VERSION > 6)
63  sensor_ = std::dynamic_pointer_cast<sensors::RaySensor>(_sensor);
64 #else
65  sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(_sensor);
66 #endif
67  if (!sensor_)
68  {
69  gzthrow("GazeboRosSonar requires a Ray Sensor as its parent");
70  return;
71  }
72 
73  // Get the world name.
74 #if (GAZEBO_MAJOR_VERSION > 6)
75  std::string worldName = sensor_->WorldName();
76 #else
77  std::string worldName = sensor_->GetWorldName();
78 #endif
79  world = physics::get_world(worldName);
80 
81  // default parameters
82  namespace_.clear();
83  topic_ = "sonar";
84  frame_id_ = "/sonar_link";
85 
86  // load parameters
87  if (_sdf->HasElement("robotNamespace"))
88  namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
89 
90  if (_sdf->HasElement("frameId"))
91  frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();
92 
93  if (_sdf->HasElement("topicName"))
94  topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
95 
96  sensor_model_.Load(_sdf);
97 
98  range_.header.frame_id = frame_id_;
99  range_.radiation_type = sensor_msgs::Range::ULTRASOUND;
100 #if (GAZEBO_MAJOR_VERSION > 6)
101  range_.field_of_view = std::min(fabs((sensor_->AngleMax() - sensor_->AngleMin()).Radian()), fabs((sensor_->VerticalAngleMax() - sensor_->VerticalAngleMin()).Radian()));
102  range_.max_range = sensor_->RangeMax();
103  range_.min_range = sensor_->RangeMin();
104 #else
105  range_.field_of_view = std::min(fabs((sensor_->GetAngleMax() - sensor_->GetAngleMin()).Radian()), fabs((sensor_->GetVerticalAngleMax() - sensor_->GetVerticalAngleMin()).Radian()));
106  range_.max_range = sensor_->GetRangeMax();
107  range_.min_range = sensor_->GetRangeMin();
108 #endif
109 
110  // Make sure the ROS node for Gazebo has already been initialized
111  if (!ros::isInitialized())
112  {
113  ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
114  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
115  return;
116  }
117 
119  publisher_ = node_handle_->advertise<sensor_msgs::Range>(topic_, 1);
120 
121  // setup dynamic_reconfigure server
122  dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_)));
124 
125  Reset();
126 
127  // connect Update function
129  updateTimer.Load(world, _sdf);
131 
132  // activate RaySensor
133  sensor_->SetActive(true);
134 }
135 
137 {
138  updateTimer.Reset();
140 }
141 
143 // Update the controller
145 {
146 #if (GAZEBO_MAJOR_VERSION >= 8)
147  common::Time sim_time = world->SimTime();
148 #else
149  common::Time sim_time = world->GetSimTime();
150 #endif
151  double dt = updateTimer.getTimeSinceLastUpdate().Double();
152 
153  // activate RaySensor if it is not yet active
154  if (!sensor_->IsActive()) sensor_->SetActive(true);
155 
156 #if (GAZEBO_MAJOR_VERSION >= 8)
157  range_.header.stamp.sec = (world->SimTime()).sec;
158  range_.header.stamp.nsec = (world->SimTime()).nsec;
159 #else
160  range_.header.stamp.sec = (world->GetSimTime()).sec;
161  range_.header.stamp.nsec = (world->GetSimTime()).nsec;
162 #endif
163 
164  // find ray with minimal range
165  range_.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
166 #if (GAZEBO_MAJOR_VERSION > 6)
167  int num_ranges = sensor_->LaserShape()->GetSampleCount() * sensor_->LaserShape()->GetVerticalSampleCount();
168 #else
169  int num_ranges = sensor_->GetLaserShape()->GetSampleCount() * sensor_->GetLaserShape()->GetVerticalSampleCount();
170 #endif
171  for(int i = 0; i < num_ranges; ++i) {
172 #if (GAZEBO_MAJOR_VERSION > 6)
173  double ray = sensor_->LaserShape()->GetRange(i);
174 #else
175  double ray = sensor_->GetLaserShape()->GetRange(i);
176 #endif
177  if (ray < range_.range) range_.range = ray;
178  }
179 
180  // add Gaussian noise (and limit to min/max range)
181  if (range_.range < range_.max_range) {
182  range_.range = sensor_model_(range_.range, dt);
183  if (range_.range < range_.min_range) range_.range = range_.min_range;
184  if (range_.range > range_.max_range) range_.range = range_.max_range;
185  }
186 
188 }
189 
190 // Register this plugin with the simulator
191 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosSonar)
192 
193 } // namespace gazebo
sensors::RaySensorPtr sensor_
ros::NodeHandle * node_handle_
virtual void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf, const std::string &_prefix="update")
Definition: update_timer.h:52
void publish(const boost::shared_ptr< M > &message) const
virtual void Load(sdf::ElementPtr _sdf, const std::string &prefix=std::string())
Definition: sensor_model.h:101
virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
ros::Publisher publisher_
ROSCPP_DECL bool isInitialized()
virtual void Reset()
Definition: update_timer.h:175
virtual event::ConnectionPtr Connect(const boost::function< void()> &_subscriber, bool connectToWorldUpdateBegin=true)
Definition: update_timer.h:85
#define ROS_FATAL_STREAM(args)
virtual void Disconnect(event::ConnectionPtr const &_c=event::ConnectionPtr())
Definition: update_timer.h:95
virtual void dynamicReconfigureCallback(SensorModelConfig &config, uint32_t level)
common::Time getTimeSinceLastUpdate() const
Definition: update_timer.h:132
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
event::ConnectionPtr updateConnection
virtual void reset()
Definition: sensor_model.h:191
physics::WorldPtr world
The parent World.
sensor_msgs::Range range_
void setUpdateRate(double rate)
Definition: update_timer.h:124
boost::shared_ptr< dynamic_reconfigure::Server< SensorModelConfig > > dynamic_reconfigure_server_


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Fri Feb 5 2021 03:48:30