gazebo_ros_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: Ros Laser controller.
20  * Author: Nathan Koenig
21  * Date: 01 Feb 2007
22  */
23 
24 #include <algorithm>
25 #include <string>
26 #include <assert.h>
27 
28 #include <gazebo/physics/World.hh>
29 #include <gazebo/physics/HingeJoint.hh>
30 #include <gazebo/sensors/Sensor.hh>
31 #include <sdf/sdf.hh>
32 #include <sdf/Param.hh>
33 #include <gazebo/common/Exception.hh>
34 #include <gazebo/sensors/RaySensor.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 
42 
43 namespace gazebo
44 {
45 // Register this plugin with the simulator
46 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosLaser)
47 
48 // Constructor
50 GazeboRosLaser::GazeboRosLaser()
51 {
52  this->seed = 0;
53 }
54 
56 // Destructor
58 {
59  this->rosnode_->shutdown();
60  delete this->rosnode_;
61 }
62 
64 // Load the controller
65 void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
66 {
67  // load plugin
68  RayPlugin::Load(_parent, this->sdf);
69  // Get the world name.
70  std::string worldName = _parent->WorldName();
71  this->world_ = physics::get_world(worldName);
72  // save pointers
73  this->sdf = _sdf;
74 
76  this->parent_ray_sensor_ =
77  dynamic_pointer_cast<sensors::RaySensor>(_parent);
78 
79  if (!this->parent_ray_sensor_)
80  gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent");
81 
82  this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Laser");
83 
84  if (!this->sdf->HasElement("frameName"))
85  {
86  ROS_INFO_NAMED("laser", "Laser plugin missing <frameName>, defaults to /world");
87  this->frame_name_ = "/world";
88  }
89  else
90  this->frame_name_ = this->sdf->Get<std::string>("frameName");
91 
92 
93  if (!this->sdf->HasElement("topicName"))
94  {
95  ROS_INFO_NAMED("laser", "Laser plugin missing <topicName>, defaults to /world");
96  this->topic_name_ = "/world";
97  }
98  else
99  this->topic_name_ = this->sdf->Get<std::string>("topicName");
100 
101  this->laser_connect_count_ = 0;
102 
103  // Make sure the ROS node for Gazebo has already been initialized
104  if (!ros::isInitialized())
105  {
106  ROS_FATAL_STREAM_NAMED("laser", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
107  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
108  return;
109  }
110 
111  ROS_INFO_NAMED("laser", "Starting Laser Plugin (ns = %s)", this->robot_namespace_.c_str() );
112  // ros callback queue for processing subscription
113  this->deferred_load_thread_ = boost::thread(
114  boost::bind(&GazeboRosLaser::LoadThread, this));
115 
116 }
117 
119 // Load the controller
121 {
122  this->gazebo_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
123  this->gazebo_node_->Init(this->world_name_);
124 
125  this->pmq.startServiceThread();
126 
127  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
128 
129  this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
130  if(this->tf_prefix_.empty()) {
131  this->tf_prefix_ = this->robot_namespace_;
132  boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/"));
133  }
134  ROS_INFO_NAMED("laser", "Laser Plugin (ns = %s) <tf_prefix_>, set to \"%s\"",
135  this->robot_namespace_.c_str(), this->tf_prefix_.c_str());
136 
137  // resolve tf prefix
138  this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_);
139 
140  if (this->topic_name_ != "")
141  {
143  ros::AdvertiseOptions::create<sensor_msgs::LaserScan>(
144  this->topic_name_, 1,
145  boost::bind(&GazeboRosLaser::LaserConnect, this),
146  boost::bind(&GazeboRosLaser::LaserDisconnect, this),
147  ros::VoidPtr(), NULL);
148  this->pub_ = this->rosnode_->advertise(ao);
149  this->pub_queue_ = this->pmq.addPub<sensor_msgs::LaserScan>();
150  }
151 
152  // Initialize the controller
153 
154  // sensor generation off by default
155  this->parent_ray_sensor_->SetActive(false);
156 }
157 
159 // Increment count
161 {
162  this->laser_connect_count_++;
163  if (this->laser_connect_count_ == 1)
164  this->laser_scan_sub_ =
165  this->gazebo_node_->Subscribe(this->parent_ray_sensor_->Topic(),
166  &GazeboRosLaser::OnScan, this);
167 }
168 
170 // Decrement count
172 {
173  this->laser_connect_count_--;
174  if (this->laser_connect_count_ == 0)
175  this->laser_scan_sub_.reset();
176 }
177 
179 // Convert new Gazebo message to ROS message and publish it
180 void GazeboRosLaser::OnScan(ConstLaserScanStampedPtr &_msg)
181 {
182  // We got a new message from the Gazebo sensor. Stuff a
183  // corresponding ROS message and publish it.
184  sensor_msgs::LaserScan laser_msg;
185  laser_msg.header.stamp = ros::Time(_msg->time().sec(), _msg->time().nsec());
186  laser_msg.header.frame_id = this->frame_name_;
187  laser_msg.angle_min = _msg->scan().angle_min();
188  laser_msg.angle_max = _msg->scan().angle_max();
189  laser_msg.angle_increment = _msg->scan().angle_step();
190  laser_msg.time_increment = 0; // instantaneous simulator scan
191  laser_msg.scan_time = 0; // not sure whether this is correct
192  laser_msg.range_min = _msg->scan().range_min();
193  laser_msg.range_max = _msg->scan().range_max();
194  laser_msg.ranges.resize(_msg->scan().ranges_size());
195  std::copy(_msg->scan().ranges().begin(),
196  _msg->scan().ranges().end(),
197  laser_msg.ranges.begin());
198  laser_msg.intensities.resize(_msg->scan().intensities_size());
199  std::copy(_msg->scan().intensities().begin(),
200  _msg->scan().intensities().end(),
201  laser_msg.intensities.begin());
202  this->pub_queue_->push(laser_msg, this->pub_);
203 }
204 }
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
std::string getPrefixParam(ros::NodeHandle &nh)
ROSCPP_DECL bool isInitialized()
std::string tf_prefix_
tf prefix
gazebo::transport::SubscriberPtr laser_scan_sub_
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 Apr 6 2021 02:19:39