void GazeboRosSonar::Update()

in simulation_ws/src/rover/hector_gazebo_plugins/src/gazebo_ros_sonar.cpp [144:188]

• 37 lines of code
• 7 McCabe index (conditional complexity)

void GazeboRosSonar::Update()
{
#if (GAZEBO_MAJOR_VERSION >= 8)
  common::Time sim_time = world->SimTime();
#else
  common::Time sim_time = world->GetSimTime();
#endif
  double dt = updateTimer.getTimeSinceLastUpdate().Double();

  // activate RaySensor if it is not yet active
  if (!sensor_->IsActive()) sensor_->SetActive(true);

#if (GAZEBO_MAJOR_VERSION >= 8)
  range_.header.stamp.sec  = (world->SimTime()).sec;
  range_.header.stamp.nsec = (world->SimTime()).nsec;
#else
  range_.header.stamp.sec  = (world->GetSimTime()).sec;
  range_.header.stamp.nsec = (world->GetSimTime()).nsec;
#endif

  // find ray with minimal range
  range_.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
#if (GAZEBO_MAJOR_VERSION > 6)
  int num_ranges = sensor_->LaserShape()->GetSampleCount() * sensor_->LaserShape()->GetVerticalSampleCount();
#else
  int num_ranges = sensor_->GetLaserShape()->GetSampleCount() * sensor_->GetLaserShape()->GetVerticalSampleCount();
#endif
  for(int i = 0; i < num_ranges; ++i) {
#if (GAZEBO_MAJOR_VERSION > 6)
    double ray = sensor_->LaserShape()->GetRange(i);
#else
    double ray = sensor_->GetLaserShape()->GetRange(i);
#endif
    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_);
}