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> 47 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosLaser)
71 GpuRayPlugin::Load(_parent, this->
sdf);
73 std::string worldName = _parent->WorldName();
74 this->
world_ = physics::get_world(worldName);
80 dynamic_pointer_cast<sensors::GpuRaySensor>(_parent);
83 gzthrow(
"GazeboRosLaser controller requires a Ray Sensor as its parent");
87 if (!this->
sdf->HasElement(
"frameName"))
89 ROS_INFO_NAMED(
"gpu_laser",
"GazeboRosLaser plugin missing <frameName>, defaults to /world");
95 if (!this->
sdf->HasElement(
"topicName"))
97 ROS_INFO_NAMED(
"gpu_laser",
"GazeboRosLaser plugin missing <topicName>, defaults to /world");
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)");
125 this->
gazebo_node_ = gazebo::transport::NodePtr(
new gazebo::transport::Node());
135 boost::trim_right_if(this->
tf_prefix_,boost::is_any_of(
"/"));
137 ROS_INFO_NAMED(
"gpu_laser",
"GPU Laser Plugin (ns = %s) <tf_prefix_>, set to \"%s\"",
146 ros::AdvertiseOptions::create<sensor_msgs::LaserScan>(
189 sensor_msgs::LaserScan laser_msg;
190 laser_msg.header.stamp =
ros::Time(_msg->time().sec(), _msg->time().nsec());
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;
196 laser_msg.scan_time = 0;
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());
void push(T &msg, ros::Publisher &pub)
Push a new message onto the queue.
#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)...
~GazeboRosLaser()
Destructor.
void OnScan(ConstLaserScanStampedPtr &_msg)
std::string topic_name_
topic name
void startServiceThread()
Start a thread to call spin().
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