Extended GPS plugin to support yaw delta between map and world

docs/included_plugins/gps.rst

@@ -50,6 +50,10 @@ This plugin provides a simple simulation of a perfectly-accurate GPS receiver.
       # to (0, 0) in world frame
       ref_lon: 0.0
 
+      # optional, defaults to 0.0, angle, in CCW+ radians between ENU coordinates (GPS) and map/world
+      ref_yaw_radians: 0.0
+
+
       # 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]

flatland_plugins/include/flatland_plugins/gps.h

@@ -30,6 +30,7 @@ class Gps : public ModelPlugin
                           /// frame
   double ref_lon_rad_;    ///< longitude in radians corresponding to (0, 0) in map
                           /// frame
+  double ref_yaw_rad_;    ///< Yaw offset (CCW+) that the map's X axis deviates from East (towards north)
   double ref_ecef_x_;     ///< ECEF coordinates of reference lat and lon at zero
                           /// altitude
   double ref_ecef_y_;     ///< ECEF coordinates of reference lat and lon at zero
@@ -42,13 +43,17 @@ class Gps : public ModelPlugin
   static double WGS84_A;   ///< Earth's major axis length
   static double WGS84_E2;  ///< Square of Earth's first eccentricity
 
+
+
   rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr
     fix_publisher_;                                                ///< GPS fix topic publisher
   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;  ///< broadcast GPS frame
   geometry_msgs::msg::TransformStamped gps_tf_;                    ///< tf from body to GPS frame
   sensor_msgs::msg::NavSatFix gps_fix_;  ///< message for publishing output
   UpdateTimer update_timer_;             ///< for controlling update rate
 
+  Eigen::Matrix3f m_enu_to_world_;  ///< rotation matrix ENU coordinates to map coordinates
+
   Eigen::Matrix3f m_body_to_gps_;  ///< tf from body to GPS
 
   /**

flatland_plugins/src/gps.cpp

@@ -21,6 +21,11 @@ void Gps::OnInitialize(const YAML::Node & config)
   double s = sin(origin_.theta);
   double x = origin_.x, y = origin_.y;
   m_body_to_gps_ << c, -s, x, s, c, y, 0, 0, 1;
+
+  c = cos(ref_yaw_rad_);
+  s = sin(ref_yaw_rad_);
+  m_enu_to_world_ << c, -s, 0, s, c, 0, 0, 0, 1;
+
 }
 
 void Gps::BeforePhysicsStep(const Timekeeper & timekeeper)
@@ -62,7 +67,7 @@ void Gps::UpdateFix()
   const b2Transform & t = body_->GetPhysicsBody()->GetTransform();
   Eigen::Matrix3f m_world_to_body;
   m_world_to_body << t.q.c, -t.q.s, t.p.x, t.q.s, t.q.c, t.p.y, 0, 0, 1;
-  Eigen::Matrix3f m_world_to_gps = m_world_to_body * m_body_to_gps_;
+  Eigen::Matrix3f m_world_to_gps = m_enu_to_world_ * m_world_to_body * m_body_to_gps_;
   b2Vec2 gps_pos(m_world_to_gps(0, 2), m_world_to_gps(1, 2));
 
   /* Convert simulation position into ECEF coordinates */
@@ -104,6 +109,7 @@ void Gps::ParseParameters(const YAML::Node & config)
   update_rate_ = reader.Get<double>("update_rate", 10.0);
   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);
   ComputeReferenceEcef();
   origin_ = reader.GetPose("origin", Pose(0, 0, 0));