22 #include "gazebo/physics/physics.hh" 25 #include <gazebo/common/common.hh> 26 #include <gazebo/common/Plugin.hh> 27 #include <gazebo/gazebo.hh> 28 #include <gazebo/physics/physics.hh> 29 #include "gazebo/transport/transport.hh" 30 #include "gazebo/msgs/msgs.hh" 37 #include <boost/algorithm/string.hpp> 55 this->newLaserScansConnection.reset();
57 this->parentSensor.reset();
65 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
70 std::dynamic_pointer_cast<sensors::RaySensor>(_parent);
72 if (!this->parentSensor)
73 gzthrow(
"RayPlugin requires a Ray Sensor as its parent");
75 this->world = physics::get_world(this->parentSensor->WorldName());
77 this->newLaserScansConnection =
78 this->parentSensor->LaserShape()->ConnectNewLaserScans(
81 if (_sdf->HasElement(
"robotNamespace"))
82 namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
84 gzwarn <<
"[gazebo_lidar_plugin] Please specify a robotNamespace.\n";
86 node_handle_ = transport::NodePtr(
new transport::Node());
87 node_handle_->Init(namespace_);
89 const string scopedName = _parent->ParentName();
91 string topicName =
"~/" + scopedName +
"/lidar";
92 boost::replace_all(topicName,
"::",
"/");
94 lidar_pub_ = node_handle_->Advertise<lidar_msgs::msgs::lidar>(topicName, 10);
100 lidar_message.set_time_msec(0);
101 lidar_message.set_min_distance(parentSensor->RangeMin());
102 lidar_message.set_max_distance(parentSensor->RangeMax());
103 lidar_message.set_current_distance(parentSensor->Range(0));
105 lidar_pub_->Publish(lidar_message);
GZ_REGISTER_SENSOR_PLUGIN(GazeboGpsPlugin)
virtual void OnNewLaserScans()
Update callback.
static const bool kPrintOnPluginLoad
virtual ~GazeboLidarPlugin()
Destructor.
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.