gazebo_ros_gpu_laser.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 /*
19  Desc: GazeboRosGpuLaser plugin for simulating ray sensors in Gazebo
20  Author: Mihai Emanuel Dolha
21  Date: 29 March 2012
22  */
23 
24 #include <algorithm>
25 #include <string>
26 #include <assert.h>
27 
28 #include <sdf/sdf.hh>
29 
30 #include <gazebo/physics/World.hh>
31 #include <gazebo/physics/HingeJoint.hh>
32 #include <gazebo/sensors/Sensor.hh>
33 #include <gazebo/common/Exception.hh>
34 #include <gazebo/sensors/GpuRaySensor.hh>
35 #include <gazebo/sensors/SensorTypes.hh>
36 #include <gazebo/transport/transport.hh>
37 
38 #include <tf/tf.h>
39 #include <tf/transform_listener.h>
40 
43 
44 namespace gazebo
45 {
46 // Register this plugin with the simulator
47 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosLaser)
48 
49 // Constructor
52 {
53  this->seed = 0;
54 }
55 
57 // Destructor
59 {
60  ROS_DEBUG_STREAM_NAMED("gpu_laser","Shutting down GPU Laser");
61  this->rosnode_->shutdown();
62  delete this->rosnode_;
63  ROS_DEBUG_STREAM_NAMED("gpu_laser","Unloaded");
64 }
65 
67 // Load the controller
68 void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
69 {
70  // load plugin
71  GpuRayPlugin::Load(_parent, this->sdf);
72  // Get the world name.
73  std::string worldName = _parent->WorldName();
74  this->world_ = physics::get_world(worldName);
75  // save pointers
76  this->sdf = _sdf;
77 
79  this->parent_ray_sensor_ =
80  dynamic_pointer_cast<sensors::GpuRaySensor>(_parent);
81 
82  if (!this->parent_ray_sensor_)
83  gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent");
84 
85  this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Laser");
86 
87  if (!this->sdf->HasElement("frameName"))
88  {
89  ROS_INFO_NAMED("gpu_laser", "GazeboRosLaser plugin missing <frameName>, defaults to /world");
90  this->frame_name_ = "/world";
91  }
92  else
93  this->frame_name_ = this->sdf->Get<std::string>("frameName");
94 
95  if (!this->sdf->HasElement("topicName"))
96  {
97  ROS_INFO_NAMED("gpu_laser", "GazeboRosLaser plugin missing <topicName>, defaults to /world");
98  this->topic_name_ = "/world";
99  }
100  else
101  this->topic_name_ = this->sdf->Get<std::string>("topicName");
102 
103  this->laser_connect_count_ = 0;
104 
105 
106  // Make sure the ROS node for Gazebo has already been initialized
107  if (!ros::isInitialized())
108  {
109  ROS_FATAL_STREAM_NAMED("gpu_laser", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
110  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
111  return;
112  }
113 
114  ROS_INFO_NAMED("gpu_laser", "Starting GazeboRosLaser Plugin (ns = %s)", this->robot_namespace_.c_str() );
115  // ros callback queue for processing subscription
116  this->deferred_load_thread_ = boost::thread(
117  boost::bind(&GazeboRosLaser::LoadThread, this));
118 
119 }
120 
122 // Load the controller
124 {
125  this->gazebo_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
126  this->gazebo_node_->Init(this->world_name_);
127 
128  this->pmq.startServiceThread();
129 
130  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
131 
132  this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
133  if(this->tf_prefix_.empty()) {
134  this->tf_prefix_ = this->robot_namespace_;
135  boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/"));
136  }
137  ROS_INFO_NAMED("gpu_laser", "GPU Laser Plugin (ns = %s) <tf_prefix_>, set to \"%s\"",
138  this->robot_namespace_.c_str(), this->tf_prefix_.c_str());
139 
140  // resolve tf prefix
141  this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_);
142 
143  if (this->topic_name_ != "")
144  {
146  ros::AdvertiseOptions::create<sensor_msgs::LaserScan>(
147  this->topic_name_, 1,
148  boost::bind(&GazeboRosLaser::LaserConnect, this),
149  boost::bind(&GazeboRosLaser::LaserDisconnect, this),
150  ros::VoidPtr(), NULL);
151  this->pub_ = this->rosnode_->advertise(ao);
152  this->pub_queue_ = this->pmq.addPub<sensor_msgs::LaserScan>();
153  }
154 
155  // Initialize the controller
156 
157  // sensor generation off by default
158  this->parent_ray_sensor_->SetActive(false);
159 
160  ROS_INFO_STREAM_NAMED("gpu_laser","LoadThread function completed");
161 }
162 
164 // Increment count
166 {
167  this->laser_connect_count_++;
168  if (this->laser_connect_count_ == 1)
169  this->laser_scan_sub_ =
170  this->gazebo_node_->Subscribe(this->parent_ray_sensor_->Topic(),
171  &GazeboRosLaser::OnScan, this);
172 }
173 
175 // Decrement count
177 {
178  this->laser_connect_count_--;
179  if (this->laser_connect_count_ == 0)
180  this->laser_scan_sub_.reset();
181 }
182 
184 // Convert new Gazebo message to ROS message and publish it
185 void GazeboRosLaser::OnScan(ConstLaserScanStampedPtr &_msg)
186 {
187  // We got a new message from the Gazebo sensor. Stuff a
188  // corresponding ROS message and publish it.
189  sensor_msgs::LaserScan laser_msg;
190  laser_msg.header.stamp = ros::Time(_msg->time().sec(), _msg->time().nsec());
191  laser_msg.header.frame_id = this->frame_name_;
192  laser_msg.angle_min = _msg->scan().angle_min();
193  laser_msg.angle_max = _msg->scan().angle_max();
194  laser_msg.angle_increment = _msg->scan().angle_step();
195  laser_msg.time_increment = 0; // instantaneous simulator scan
196  laser_msg.scan_time = 0; // not sure whether this is correct
197  laser_msg.range_min = _msg->scan().range_min();
198  laser_msg.range_max = _msg->scan().range_max();
199  laser_msg.ranges.resize(_msg->scan().ranges_size());
200  std::copy(_msg->scan().ranges().begin(),
201  _msg->scan().ranges().end(),
202  laser_msg.ranges.begin());
203  laser_msg.intensities.resize(_msg->scan().intensities_size());
204  std::copy(_msg->scan().intensities().begin(),
205  _msg->scan().intensities().end(),
206  laser_msg.intensities.begin());
207  this->pub_queue_->push(laser_msg, this->pub_);
208 }
209 }
void push(T &msg, ros::Publisher &pub)
Push a new message onto the queue.
Definition: PubQueue.h:73
#define ROS_INFO_NAMED(name,...)
ros::NodeHandle * rosnode_
pointer to ros node
#define ROS_DEBUG_STREAM_NAMED(name, args)
std::string getPrefixParam(ros::NodeHandle &nh)
ROSCPP_DECL bool isInitialized()
std::string tf_prefix_
tf prefix
gazebo::transport::SubscriberPtr laser_scan_sub_
#define ROS_INFO_STREAM_NAMED(name, args)
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
boost::thread deferred_load_thread_
std::string resolve(const std::string &prefix, const std::string &frame_name)
PubQueue< sensor_msgs::LaserScan >::Ptr pub_queue_
std::string frame_name_
frame transform name, should match link name
std::string robot_namespace_
for setting ROS name space
#define ROS_FATAL_STREAM_NAMED(name, args)
gazebo::transport::NodePtr gazebo_node_
PubMultiQueue pmq
prevents blocking
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
boost::shared_ptr< PubQueue< T > > addPub()
Add a new queue. Call this once for each published topic (or at least each type of publish message)...
Definition: PubQueue.h:143
void OnScan(ConstLaserScanStampedPtr &_msg)
std::string topic_name_
topic name
void startServiceThread()
Start a thread to call spin().
Definition: PubQueue.h:181
std::string GetRobotNamespace(const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo=NULL)
Reads the name space tag of a sensor plugin.
#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
sensors::GpuRaySensorPtr parent_ray_sensor_
The parent sensor.
int laser_connect_count_
Keep track of number of connctions.
boost::shared_ptr< void > VoidPtr


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27