enable odom

aerostack2 · Jun 24, 2024 · 31e1b57 · 31e1b57
1 parent b47c973
commit 31e1b57
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 1 deletion.
diff --git a/include/as2_platform_mavlink/mavlink_platform.hpp b/include/as2_platform_mavlink/mavlink_platform.hpp
@@ -44,6 +44,8 @@
 
 #include <cmath>
 #include <mavros_msgs/msg/detail/attitude_target__struct.hpp>
+#include <nav_msgs/msg/detail/odometry__struct.hpp>
+#include <rclcpp/subscription.hpp>
 #include <string>
 #include <memory>
 
@@ -113,6 +115,8 @@ class MavlinkPlatform : public as2::AerialPlatform
   // mavlink_ subscribers
 
   rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr mavlink_state_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr mavlink_odom_sub_;
+
   void mavlinkStateCb(const mavros_msgs::msg::State::SharedPtr msg)
   {
     this->platform_info_msg_.set__connected(msg->connected);

diff --git a/launch/mavros_launch.py b/launch/mavros_launch.py
@@ -76,7 +76,6 @@ def get_node(context, *args, **kwargs) -> list:
                         pluginlists_yaml, config_yaml],
             remappings=[('imu/data', f'/{namespace}/sensor_measurements/imu'),
                         ('gps/fix', f'/{namespace}/sensor_measurements/gps_raw'),
-                        ('odometry/in', f'/{namespace}/sensor_measurements/odometry'),
                         ],
         )]
 

diff --git a/src/mavlink_platform.cpp b/src/mavlink_platform.cpp
@@ -77,6 +77,15 @@ MavlinkPlatform::MavlinkPlatform(const rclcpp::NodeOptions & options)
     "mavros/state", rclcpp::SensorDataQoS(),
     std::bind(&MavlinkPlatform::mavlinkStateCb, this, std::placeholders::_1));
 
+  mavlink_odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+    "mavros/local_position/odom", rclcpp::SensorDataQoS(),
+
+    [&](const nav_msgs::msg::Odometry::SharedPtr msg) {
+      msg->child_frame_id = base_link_frame_id_;
+      msg->header.frame_id = odom_frame_id_;
+      this->odometry_raw_estimation_ptr_->updateData(*msg);
+    });
+
   // declare mavlink services
 
   mavlink_arm_client_ =
@@ -86,6 +95,7 @@ MavlinkPlatform::MavlinkPlatform(const rclcpp::NodeOptions & options)
     std::make_shared<as2::SynchronousServiceClient<mavros_msgs::srv::SetMode>>(
     "mavros/set_mode", this);
 
+
   // // px4_timesync_sub_ = this->create_subscription<px4_msgs::msg::TimesyncStatus>(
   // //     "/fmu/out/timesync_status", rclcpp::SensorDataQoS(),
   // //     [this](const px4_msgs::msg::TimesyncStatus::UniquePtr msg) {