//////////////////////////////////////////////////////////////////////////////// // 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; }