29 #include <gazebo/physics/World.hh> 30 #include <gazebo/physics/HingeJoint.hh> 31 #include <gazebo/sensors/Sensor.hh> 33 #include <sdf/Param.hh> 34 #include <gazebo/common/Exception.hh> 35 #include <gazebo/sensors/RaySensor.hh> 36 #include <gazebo/sensors/SensorTypes.hh> 37 #include <gazebo/transport/Node.hh> 39 #include <geometry_msgs/Point32.h> 40 #include <sensor_msgs/ChannelFloat32.h> 44 #define EPSILON_DIFF 0.000001 49 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBlockLaser)
76 RayPlugin::Load(_parent, _sdf);
82 std::string worldName = _parent->WorldName();
83 this->
world_ = physics::get_world(worldName);
85 #if GAZEBO_MAJOR_VERSION >= 8 91 this->
node_ = transport::NodePtr(
new transport::Node());
92 this->
node_->Init(worldName);
98 gzthrow(
"GazeboRosBlockLaser controller requires a Ray Sensor as its parent");
101 if (_sdf->HasElement(
"robotNamespace"))
102 this->
robot_namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>() +
"/";
104 if (!_sdf->HasElement(
"frameName"))
106 ROS_INFO_NAMED(
"block_laser",
"Block laser plugin missing <frameName>, defaults to /world");
110 this->
frame_name_ = _sdf->GetElement(
"frameName")->Get<std::string>();
112 if (!_sdf->HasElement(
"topicName"))
114 ROS_INFO_NAMED(
"block_laser",
"Block laser plugin missing <topicName>, defaults to /world");
118 this->
topic_name_ = _sdf->GetElement(
"topicName")->Get<std::string>();
120 if (!_sdf->HasElement(
"gaussianNoise"))
122 ROS_INFO_NAMED(
"block_laser",
"Block laser plugin missing <gaussianNoise>, defaults to 0.0");
126 this->
gaussian_noise_ = _sdf->GetElement(
"gaussianNoise")->Get<
double>();
128 if (!_sdf->HasElement(
"hokuyoMinIntensity"))
130 ROS_INFO_NAMED(
"block_laser",
"Block laser plugin missing <hokuyoMinIntensity>, defaults to 101");
138 if (!_sdf->HasElement(
"updateRate"))
140 ROS_INFO_NAMED(
"block_laser",
"Block laser plugin missing <updateRate>, defaults to 0");
144 this->
update_rate_ = _sdf->GetElement(
"updateRate")->Get<
double>();
153 ROS_FATAL_STREAM_NAMED(
"block_laser",
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 154 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
168 this->
cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
213 common::Time sensor_update_time = this->
parent_sensor_->LastUpdateTime();
216 ROS_WARN_NAMED(
"block_laser",
"Negative sensor update time difference detected.");
228 ROS_INFO_NAMED(
"block_laser",
"gazebo_ros_block_laser topic name not set");
240 double r1, r2, r3, r4, r;
258 double yDiff = maxAngle.Radian() - minAngle.Radian();
259 double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian();
266 this->
cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
273 boost::mutex::scoped_lock sclock(this->
lock);
276 this->
cloud_msg_.header.stamp.sec = _updateTime.sec;
277 this->
cloud_msg_.header.stamp.nsec = _updateTime.nsec;
279 for (j = 0; j<verticalRangeCount; j++)
282 vb = (verticalRangeCount == 1) ? 0 : (
double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
283 vja = (int) floor(vb);
284 vjb = std::min(vja + 1, verticalRayCount - 1);
287 assert(vja >= 0 && vja < verticalRayCount);
288 assert(vjb >= 0 && vjb < verticalRayCount);
290 for (i = 0; i<rangeCount; i++)
293 hb = (rangeCount == 1)? 0 : (
double) i * (rayCount - 1) / (rangeCount - 1);
294 hja = (int) floor(hb);
295 hjb = std::min(hja + 1, rayCount - 1);
298 assert(hja >= 0 && hja < rayCount);
299 assert(hjb >= 0 && hjb < rayCount);
302 j1 = hja + vja * rayCount;
303 j2 = hjb + vja * rayCount;
304 j3 = hja + vjb * rayCount;
305 j4 = hjb + vjb * rayCount;
307 r1 = std::min(this->
parent_ray_sensor_->LaserShape()->GetRange(j1) , maxRange-minRange);
308 r2 = std::min(this->
parent_ray_sensor_->LaserShape()->GetRange(j2) , maxRange-minRange);
309 r3 = std::min(this->
parent_ray_sensor_->LaserShape()->GetRange(j3) , maxRange-minRange);
310 r4 = std::min(this->
parent_ray_sensor_->LaserShape()->GetRange(j4) , maxRange-minRange);
314 r = (1-vb)*((1 - hb) * r1 + hb * r2)
315 + vb *((1 - hb) * r3 + hb * r4);
330 double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.Radian();
331 double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.Radian();
339 double diffRange = maxRange - minRange;
340 double diff = diffRange - r;
344 geometry_msgs::Point32 point;
346 point.x = r * cos(pAngle) * cos(yAngle);
347 point.y = r * cos(pAngle) * sin(yAngle);
348 point.z = r * sin(pAngle);
354 geometry_msgs::Point32 point;
381 double U = (double)rand()/(double)RAND_MAX;
382 double V = (double)rand()/(double)RAND_MAX;
383 double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
396 static const double timeout = 0.01;
406 this->
sim_time_ = msgs::Convert( _msg->sim_time() );
408 ignition::math::Pose3d pose;
409 pose.Pos().X() = 0.5*sin(0.01*this->
sim_time_.Double());
410 gzdbg <<
"plugin simTime [" << this->
sim_time_.Double() <<
"] update pose [" << pose.Pos().X() <<
"]\n";
int laser_connect_count_
Keep track of number of connctions.
void PutLaserData(common::Time &_updateTime)
Put laser data to the ROS topic.
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
ROSCPP_DECL bool isInitialized()
double hokuyo_min_intensity_
hack to mimic hokuyo intensity cutoff of 100
ros::NodeHandle * rosnode_
pointer to ros node
boost::thread callback_laser_queue_thread_
ros::CallbackQueue laser_queue_
boost::mutex lock
A mutex to lock access to fields that are used in message callbacks.
std::string robot_namespace_
for setting ROS name space
std::string resolve(const std::string &prefix, const std::string &frame_name)
#define ROS_DEBUG_NAMED(name,...)
#define ROS_FATAL_STREAM_NAMED(name, args)
std::string frame_name_
frame transform name, should match link name
std::string topic_name_
topic name
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
~GazeboRosBlockLaser()
Destructor.
sensors::SensorPtr parent_sensor_
The parent sensor.
double gaussian_noise_
Gaussian noise.
sensors::RaySensorPtr parent_ray_sensor_
common::Time last_update_time_
bool getParam(const std::string &key, std::string &s) const
virtual void OnNewLaserScans()
Update the controller.
#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
double update_rate_
update rate of this sensor
boost::shared_ptr< void > VoidPtr
void OnStats(const boost::shared_ptr< msgs::WorldStatistics const > &_msg)
sensor_msgs::PointCloud cloud_msg_
ros message