imu plugin now compiles

avidbots · Nov 16, 2024 · a687c8c · a687c8c
1 parent 28091d8
commit a687c8c
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 5 deletions.
diff --git a/flatland_plugins/include/flatland_plugins/diff_drive.h b/flatland_plugins/include/flatland_plugins/diff_drive.h
@@ -51,6 +51,7 @@
 #include <tf2_ros/transform_broadcaster.h>
 
 #include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <random>
 
@@ -68,7 +69,7 @@ class DiffDrive : public flatland_server::ModelPlugin
   rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr ground_truth_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_pub_;
   Body * body_;
   geometry_msgs::msg::TwistStamped::SharedPtr twist_msg_ = std::make_shared<geometry_msgs::msg::TwistStamped>();
   nav_msgs::msg::Odometry odom_msg_;

diff --git a/flatland_plugins/src/diff_drive.cpp b/flatland_plugins/src/diff_drive.cpp
@@ -231,7 +231,7 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper & timekeeper)
     if (enable_twist_pub_) {
       // Transform global frame velocity into local frame to simulate encoder
       // readings
-      geometry_msgs::msg::TwistStamped twist_pub_msg;
+      geometry_msgs::msg::TwistWithCovarianceStamped twist_pub_msg;
       twist_pub_msg.header.stamp = timekeeper.GetSimTime();
       twist_pub_msg.header.frame_id = odom_msg_.child_frame_id;
 
@@ -245,7 +245,7 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper & timekeeper)
 
       twist_pub_msg.twist.covariance = odom_msg_.twist.covariance;
 
-      twist_pub_.publish(twist_pub_msg);
+      twist_pub_->publish(twist_pub_msg);
     }
 
     // publish odom tf

diff --git a/flatland_plugins/test/imu_test.cpp b/flatland_plugins/test/imu_test.cpp
@@ -66,7 +66,7 @@ TEST(ImuPluginTest, load_test)
 
 // Run all the tests that were declared with TEST()
 int main(int argc, char** argv) {
-  ros::init(argc, argv, "imu_test");
+  rclcpp::init(argc, argv);
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }
diff --git a/flatland_plugins/test/update_timer_test.cpp b/flatland_plugins/test/update_timer_test.cpp
@@ -63,7 +63,7 @@ class TestPlugin : public ModelPlugin
   UpdateTimer update_timer_;
   int update_counter_;
 
-  void OnInitialize(const YAML::Node & config) override
+  void OnInitialize(const YAML::Node &) override
   {
     update_timer_.SetRate(0);
     update_counter_ = 0;