////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosSonar::Update()
{
common::Time sim_time = world->GetSimTime();
double dt = updateTimer.getTimeSinceLastUpdate().Double();
// activate RaySensor if it is not yet active
if (!sensor_->IsActive()) sensor_->SetActive(true);
range_.header.stamp.sec = (world->GetSimTime()).sec;
range_.header.stamp.nsec = (world->GetSimTime()).nsec;
// find ray with minimal range
range_.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
int num_ranges = sensor_->GetLaserShape()->GetSampleCount() * sensor_->GetLaserShape()->GetVerticalSampleCount();
for(int i = 0; i < num_ranges; ++i) {
double ray = sensor_->GetLaserShape()->GetRange(i);
if (ray < range_.range) range_.range = ray;
}
// add Gaussian noise (and limit to min/max range)
if (range_.range < range_.max_range) {
range_.range = sensor_model_(range_.range, dt);
if (range_.range < range_.min_range) range_.range = range_.min_range;
if (range_.range > range_.max_range) range_.range = range_.max_range;
}
publisher_.publish(range_);
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosMagnetic::Update()
{
common::Time sim_time = world->GetSimTime();
double dt = updateTimer.getTimeSinceLastUpdate().Double();
math::Pose pose = link->GetWorldPose();
math::Vector3 field = sensor_model_(pose.rot.RotateVectorReverse(magnetic_field_world_), dt);
magnetic_field_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
magnetic_field_.vector.x = field.x;
magnetic_field_.vector.y = field.y;
magnetic_field_.vector.z = field.z;
publisher_.publish(magnetic_field_);
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosMagnetic::Update()
{
common::Time sim_time = world->GetSimTime();
double dt = (sim_time - last_time).Double();
if (last_time + update_period > sim_time) return;
math::Pose pose = link->GetWorldPose();
math::Vector3 field = sensor_model_(pose.rot.RotateVectorReverse(magnetic_field_world_), dt);
magnetic_field_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
magnetic_field_.vector.x = field.x;
magnetic_field_.vector.y = field.y;
magnetic_field_.vector.z = field.z;
publisher_.publish(magnetic_field_);
// save last time stamp
last_time = sim_time;
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosBaro::Update()
{
common::Time sim_time = world->GetSimTime();
double dt = (sim_time - last_time).Double();
if (last_time + update_period > sim_time) return;
math::Pose pose = link->GetWorldPose();
double height = sensor_model_(pose.pos.z, dt);
if (height_publisher_) {
#ifdef USE_MAV_MSGS
double previous_height = height_.height;
height_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
height_.height = height;
height_.height_variance = sensor_model_.gaussian_noise*sensor_model_.gaussian_noise + sensor_model_.drift*sensor_model_.drift;
height_.climb = dt > 0.0 ? (height_.height - previous_height) / dt : 0.0;
height_.climb_variance = sensor_model_.gaussian_noise*sensor_model_.gaussian_noise;
#else
height_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
height_.point.z = height;
#endif
height_publisher_.publish(height_);
}
if (altimeter_publisher_) {
altimeter_.header = height_.header;
altimeter_.altitude = height + elevation_;
altimeter_.pressure = pow((1.0 - altimeter_.altitude / 44330.0), 5.263157) * qnh_;
altimeter_.qnh = qnh_;
altimeter_publisher_.publish(altimeter_);
}
// save last time stamp
last_time = sim_time;
}