From 9dd84d97f88f9636977ad4e14b53f234a8469d4c Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Wed, 4 Dec 2024 21:05:06 +0000 Subject: [PATCH] Added position covariance to GPS parameters and fixed IMU tf pub rate --- docs/included_plugins/gps.rst | 3 + .../include/flatland_plugins/gps.h | 2 + flatland_plugins/src/gps.cpp | 4 + flatland_plugins/src/imu.cpp | 114 +++++++++--------- 4 files changed, 66 insertions(+), 57 deletions(-) diff --git a/docs/included_plugins/gps.rst b/docs/included_plugins/gps.rst index bdec59ef..cba09d53 100644 --- a/docs/included_plugins/gps.rst +++ b/docs/included_plugins/gps.rst @@ -57,3 +57,6 @@ This plugin provides a simple simulation of a perfectly-accurate GPS receiver. # optional, default to [0, 0, 0], in the form of [x, y, yaw], the position # and orientation to place GPS antenna relative to specified model body origin: [0, 0, 0] + + # 3x3 covariance matrix for x/y/z + position_covariance: [1e-2, 0, 0, 0, 1e-2, 0, 0, 0, 1e-2] diff --git a/flatland_plugins/include/flatland_plugins/gps.h b/flatland_plugins/include/flatland_plugins/gps.h index 308a6955..ed848ea9 100644 --- a/flatland_plugins/include/flatland_plugins/gps.h +++ b/flatland_plugins/include/flatland_plugins/gps.h @@ -56,6 +56,8 @@ class Gps : public ModelPlugin Eigen::Matrix3f m_body_to_gps_; ///< tf from body to GPS + std::array position_covariance_; + /** * @brief Initialization for the plugin * @param[in] config Plugin YAML Node diff --git a/flatland_plugins/src/gps.cpp b/flatland_plugins/src/gps.cpp index 39b294ce..d05483b2 100644 --- a/flatland_plugins/src/gps.cpp +++ b/flatland_plugins/src/gps.cpp @@ -97,6 +97,8 @@ void Gps::UpdateFix() } gps_fix_.latitude = lat_rad * 180.0 * M_1_PI; gps_fix_.altitude = 0.0; + gps_fix_.position_covariance_type = gps_fix_.COVARIANCE_TYPE_DIAGONAL_KNOWN; + gps_fix_.position_covariance = position_covariance_; } void Gps::ParseParameters(const YAML::Node & config) @@ -110,6 +112,8 @@ void Gps::ParseParameters(const YAML::Node & config) ref_lat_rad_ = M_PI / 180.0 * reader.Get("ref_lat", 0.0); ref_lon_rad_ = M_PI / 180.0 * reader.Get("ref_lon", 0.0); ref_yaw_rad_ = reader.Get("ref_yaw_radians", 0.0); + position_covariance_ = reader.GetArray( + "position_cavariance", std::array{1e-2, 0, 0, 0, 1e-2, 0, 0, 0, 1e-2}); ComputeReferenceEcef(); origin_ = reader.GetPose("origin", Pose(0, 0, 0)); diff --git a/flatland_plugins/src/imu.cpp b/flatland_plugins/src/imu.cpp index 02e73367..e0a45b7b 100644 --- a/flatland_plugins/src/imu.cpp +++ b/flatland_plugins/src/imu.cpp @@ -180,7 +180,9 @@ void Imu::OnInitialize(const YAML::Node& config) { } void Imu::AfterPhysicsStep(const Timekeeper& timekeeper) { - bool publish = update_timer_.CheckUpdate(timekeeper); + if (!update_timer_.CheckUpdate(timekeeper)) { + return; // return if we're not ready to publish + } b2Body* b2body = body_->physics_body_; @@ -190,68 +192,66 @@ void Imu::AfterPhysicsStep(const Timekeeper& timekeeper) { b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0)); float angular_vel = b2body->GetAngularVelocity(); - if (publish) { - // get the state of the body and publish the data - - ground_truth_msg_.header.stamp = timekeeper.GetSimTime(); - tf2::Quaternion q; - q.setRPY(0, 0, angle); - ground_truth_msg_.orientation.x = q.x(); - ground_truth_msg_.orientation.y = q.y(); - ground_truth_msg_.orientation.z = q.z(); - ground_truth_msg_.orientation.w = q.w(); - ground_truth_msg_.angular_velocity.z = angular_vel; - - double global_acceleration_x = - (linear_vel_local.x - linear_vel_local_prev.x) * pub_rate_; - double global_acceleration_y = - (linear_vel_local.y - linear_vel_local_prev.y) * pub_rate_; - - double global_acceleration_z = 0.0; - if (enable_imu_gravity_) { - global_acceleration_z = -9.81; - } - - ground_truth_msg_.linear_acceleration.x = - cos(angle) * global_acceleration_x + sin(angle) * global_acceleration_y; - ground_truth_msg_.linear_acceleration.y = - sin(angle) * global_acceleration_x + cos(angle) * global_acceleration_y; - - ground_truth_msg_.linear_acceleration.z = global_acceleration_z; - - // ROS_INFO_STREAM_THROTTLE( - // 1, "" << linear_vel_local.x << " " << linear_vel_local_prev.x << " " - //<< pub_rate_); - // add the noise to odom messages - imu_msg_.header.stamp = timekeeper.GetSimTime(); - - tf2::Quaternion q2; - q2.setRPY(0, 0, angle + noise_gen_[2](rng_)); - imu_msg_.orientation.x = q.x(); - imu_msg_.orientation.y = q.y(); - imu_msg_.orientation.z = q.z(); - imu_msg_.orientation.w = q.w(); - - imu_msg_.angular_velocity = ground_truth_msg_.angular_velocity; - imu_msg_.angular_velocity.z += noise_gen_[5](rng_); - - imu_msg_.linear_acceleration = ground_truth_msg_.linear_acceleration; - imu_msg_.linear_acceleration.x += noise_gen_[6](rng_); - imu_msg_.linear_acceleration.y += noise_gen_[7](rng_); - imu_msg_.linear_acceleration.z += noise_gen_[8](rng_); - - if (enable_imu_pub_) { - ground_truth_pub_->publish(ground_truth_msg_); - imu_pub_->publish(imu_msg_); - } - linear_vel_local_prev = linear_vel_local; + // get the state of the body and publish the data + + ground_truth_msg_.header.stamp = timekeeper.GetSimTime(); + tf2::Quaternion q; + q.setRPY(0, 0, angle); + ground_truth_msg_.orientation.x = q.x(); + ground_truth_msg_.orientation.y = q.y(); + ground_truth_msg_.orientation.z = q.z(); + ground_truth_msg_.orientation.w = q.w(); + ground_truth_msg_.angular_velocity.z = angular_vel; + + double global_acceleration_x = + (linear_vel_local.x - linear_vel_local_prev.x) * pub_rate_; + double global_acceleration_y = + (linear_vel_local.y - linear_vel_local_prev.y) * pub_rate_; + + double global_acceleration_z = 0.0; + if (enable_imu_gravity_) { + global_acceleration_z = -9.81; + } + + ground_truth_msg_.linear_acceleration.x = + cos(angle) * global_acceleration_x + sin(angle) * global_acceleration_y; + ground_truth_msg_.linear_acceleration.y = + sin(angle) * global_acceleration_x + cos(angle) * global_acceleration_y; + + ground_truth_msg_.linear_acceleration.z = global_acceleration_z; + + // ROS_INFO_STREAM_THROTTLE( + // 1, "" << linear_vel_local.x << " " << linear_vel_local_prev.x << " " + //<< pub_rate_); + // add the noise to odom messages + imu_msg_.header.stamp = timekeeper.GetSimTime(); + + tf2::Quaternion q2; + q2.setRPY(0, 0, angle + noise_gen_[2](rng_)); + imu_msg_.orientation.x = q.x(); + imu_msg_.orientation.y = q.y(); + imu_msg_.orientation.z = q.z(); + imu_msg_.orientation.w = q.w(); + + imu_msg_.angular_velocity = ground_truth_msg_.angular_velocity; + imu_msg_.angular_velocity.z += noise_gen_[5](rng_); + + imu_msg_.linear_acceleration = ground_truth_msg_.linear_acceleration; + imu_msg_.linear_acceleration.x += noise_gen_[6](rng_); + imu_msg_.linear_acceleration.y += noise_gen_[7](rng_); + imu_msg_.linear_acceleration.z += noise_gen_[8](rng_); + + if (enable_imu_pub_) { + ground_truth_pub_->publish(ground_truth_msg_); + imu_pub_->publish(imu_msg_); } + linear_vel_local_prev = linear_vel_local; if (broadcast_tf_) { imu_tf_.header.stamp = timekeeper.GetSimTime(); tf_broadcaster_->sendTransform(imu_tf_); } } -} +} // end namespace PLUGINLIB_EXPORT_CLASS(flatland_plugins::Imu, flatland_server::ModelPlugin)