Merge pull request #81 from eborghi10/master

Fix ground truth Tf changing parent frame
avidbots · May 3, 2022 · c8e8747 · c8e8747
2 parents c0826a4 + 5bcd916
commit c8e8747
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/flatland_plugins/src/diff_drive.cpp b/flatland_plugins/src/diff_drive.cpp
@@ -65,6 +65,8 @@ void DiffDrive::OnInitialize(const YAML::Node& config) {
   enable_twist_pub_ = reader.Get<bool>("enable_twist_pub", true);
   std::string body_name = reader.Get<std::string>("body");
   std::string odom_frame_id = reader.Get<std::string>("odom_frame_id", "odom");
+  std::string ground_truth_frame_id =
+      reader.Get<std::string>("ground_truth_frame_id", odom_frame_id);
 
   std::string twist_topic = reader.Get<std::string>("twist_sub", "cmd_vel");
   std::string odom_topic =
@@ -127,12 +129,15 @@ void DiffDrive::OnInitialize(const YAML::Node& config) {
   }
 
   // init the values for the messages
-  ground_truth_msg_.header.frame_id = odom_frame_id;
+  ground_truth_msg_.header.frame_id = ground_truth_frame_id;
   ground_truth_msg_.child_frame_id =
       tf::resolve("", GetModel()->NameSpaceTF(body_->name_));
   ground_truth_msg_.twist.covariance.fill(0);
   ground_truth_msg_.pose.covariance.fill(0);
+  // Odometry message initially is similar to ground truth except for the
+  // parent frame ID
   odom_msg_ = ground_truth_msg_;
+  odom_msg_.header.frame_id = odom_frame_id;
 
   // copy from std::array to boost array
   for (unsigned int i = 0; i < 36; i++) {