Added position covariance to GPS parameters and fixed IMU tf pub rate

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]

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<double, 9> position_covariance_;
+
   /**
    * @brief Initialization for the plugin
    * @param[in] config Plugin YAML Node

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<double>("ref_lat", 0.0);
   ref_lon_rad_ = M_PI / 180.0 * reader.Get<double>("ref_lon", 0.0);
   ref_yaw_rad_ = reader.Get<double>("ref_yaw_radians", 0.0);
+  position_covariance_ = reader.GetArray<double, 9>(
+      "position_cavariance", std::array<double, 9>{1e-2, 0, 0,  0, 1e-2, 0,  0, 0, 1e-2});
   ComputeReferenceEcef();
   origin_ = reader.GetPose("origin", Pose(0, 0, 0));
 

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)