gazebo_ros_laser.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /*
00019  * Desc: Ros Laser controller.
00020  * Author: Nathan Koenig
00021  * Date: 01 Feb 2007
00022  */
00023 
00024 #include <algorithm>
00025 #include <string>
00026 #include <assert.h>
00027 
00028 #include <gazebo/physics/World.hh>
00029 #include <gazebo/physics/HingeJoint.hh>
00030 #include <gazebo/sensors/Sensor.hh>
00031 #include <sdf/sdf.hh>
00032 #include <sdf/Param.hh>
00033 #include <gazebo/common/Exception.hh>
00034 #include <gazebo/sensors/RaySensor.hh>
00035 #include <gazebo/sensors/SensorTypes.hh>
00036 #include <gazebo/transport/transport.hh>
00037 
00038 #include <tf/tf.h>
00039 #include <tf/transform_listener.h>
00040 
00041 #include <gazebo_plugins/gazebo_ros_laser.h>
00042 
00043 namespace gazebo
00044 {
00045 // Register this plugin with the simulator
00046 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosLaser)
00047 
00048 
00049 // Constructor
00050 GazeboRosLaser::GazeboRosLaser()
00051 {
00052   this->seed = 0;
00053 }
00054 
00056 // Destructor
00057 GazeboRosLaser::~GazeboRosLaser()
00058 {
00059   this->rosnode_->shutdown();
00060   delete this->rosnode_;
00061 }
00062 
00064 // Load the controller
00065 void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00066 {
00067   // load plugin
00068   RayPlugin::Load(_parent, this->sdf);
00069   // Get the world name.
00070 # if GAZEBO_MAJOR_VERSION >= 7
00071   std::string worldName = _parent->WorldName();
00072 # else
00073   std::string worldName = _parent->GetWorldName();
00074 # endif
00075 
00076   this->world_ = physics::get_world(worldName);
00077   // save pointers
00078   this->sdf = _sdf;
00079 
00080   GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
00081   this->parent_ray_sensor_ =
00082     dynamic_pointer_cast<sensors::RaySensor>(_parent);
00083 
00084   if (!this->parent_ray_sensor_)
00085     gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent");
00086 
00087   this->robot_namespace_ =  GetRobotNamespace(_parent, _sdf, "Laser");
00088 
00089   if (!this->sdf->HasElement("frameName"))
00090   {
00091     ROS_INFO("Laser plugin missing <frameName>, defaults to /world");
00092     this->frame_name_ = "/world";
00093   }
00094   else
00095     this->frame_name_ = this->sdf->Get<std::string>("frameName");
00096 
00097 
00098   if (!this->sdf->HasElement("topicName"))
00099   {
00100     ROS_INFO("Laser plugin missing <topicName>, defaults to /world");
00101     this->topic_name_ = "/world";
00102   }
00103   else
00104     this->topic_name_ = this->sdf->Get<std::string>("topicName");
00105 
00106   this->laser_connect_count_ = 0;
00107 
00108     // Make sure the ROS node for Gazebo has already been initialized
00109   if (!ros::isInitialized())
00110   {
00111     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00112       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00113     return;
00114   }
00115 
00116   ROS_INFO ( "Starting Laser Plugin (ns = %s)!", this->robot_namespace_.c_str() );
00117   // ros callback queue for processing subscription
00118   this->deferred_load_thread_ = boost::thread(
00119     boost::bind(&GazeboRosLaser::LoadThread, this));
00120 
00121 }
00122 
00124 // Load the controller
00125 void GazeboRosLaser::LoadThread()
00126 {
00127   this->gazebo_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
00128   this->gazebo_node_->Init(this->world_name_);
00129 
00130   this->pmq.startServiceThread();
00131 
00132   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00133 
00134   this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
00135   if(this->tf_prefix_.empty()) {
00136       this->tf_prefix_ = this->robot_namespace_;
00137       boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/"));
00138   }
00139   ROS_INFO("Laser Plugin (ns = %s)  <tf_prefix_>, set to \"%s\"",
00140              this->robot_namespace_.c_str(), this->tf_prefix_.c_str());
00141 
00142   // resolve tf prefix
00143   this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_);
00144 
00145   if (this->topic_name_ != "")
00146   {
00147     ros::AdvertiseOptions ao =
00148       ros::AdvertiseOptions::create<sensor_msgs::LaserScan>(
00149       this->topic_name_, 1,
00150       boost::bind(&GazeboRosLaser::LaserConnect, this),
00151       boost::bind(&GazeboRosLaser::LaserDisconnect, this),
00152       ros::VoidPtr(), NULL);
00153     this->pub_ = this->rosnode_->advertise(ao);
00154     this->pub_queue_ = this->pmq.addPub<sensor_msgs::LaserScan>();
00155   }
00156 
00157   // Initialize the controller
00158 
00159   // sensor generation off by default
00160   this->parent_ray_sensor_->SetActive(false);
00161 }
00162 
00164 // Increment count
00165 void GazeboRosLaser::LaserConnect()
00166 {
00167   this->laser_connect_count_++;
00168   if (this->laser_connect_count_ == 1)
00169     this->laser_scan_sub_ =
00170 # if GAZEBO_MAJOR_VERSION >= 7
00171       this->gazebo_node_->Subscribe(this->parent_ray_sensor_->Topic(),
00172 # else
00173       this->gazebo_node_->Subscribe(this->parent_ray_sensor_->GetTopic(),
00174 # endif
00175                                     &GazeboRosLaser::OnScan, this);
00176 }
00177 
00179 // Decrement count
00180 void GazeboRosLaser::LaserDisconnect()
00181 {
00182   this->laser_connect_count_--;
00183   if (this->laser_connect_count_ == 0)
00184     this->laser_scan_sub_.reset();
00185 }
00186 
00188 // Convert new Gazebo message to ROS message and publish it
00189 void GazeboRosLaser::OnScan(ConstLaserScanStampedPtr &_msg)
00190 {
00191   // We got a new message from the Gazebo sensor.  Stuff a
00192   // corresponding ROS message and publish it.
00193   sensor_msgs::LaserScan laser_msg;
00194   laser_msg.header.stamp = ros::Time(_msg->time().sec(), _msg->time().nsec());
00195   laser_msg.header.frame_id = this->frame_name_;
00196   laser_msg.angle_min = _msg->scan().angle_min();
00197   laser_msg.angle_max = _msg->scan().angle_max();
00198   laser_msg.angle_increment = _msg->scan().angle_step();
00199   laser_msg.time_increment = 0;  // instantaneous simulator scan
00200   laser_msg.scan_time = 0;  // not sure whether this is correct
00201   laser_msg.range_min = _msg->scan().range_min();
00202   laser_msg.range_max = _msg->scan().range_max();
00203   laser_msg.ranges.resize(_msg->scan().ranges_size());
00204   std::copy(_msg->scan().ranges().begin(),
00205             _msg->scan().ranges().end(),
00206             laser_msg.ranges.begin());
00207   laser_msg.intensities.resize(_msg->scan().intensities_size());
00208   std::copy(_msg->scan().intensities().begin(),
00209             _msg->scan().intensities().end(),
00210             laser_msg.intensities.begin());
00211   this->pub_queue_->push(laser_msg, this->pub_);
00212 }
00213 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:22