From 6339691fb871c53851bca41a1afce565dde4000f Mon Sep 17 00:00:00 2001
From: Vladislavert <chuvarevanv@gmail.com>
Date: Fri, 30 Jun 2023 10:12:28 +0300
Subject: [PATCH] Added geometry_msgs/Vector3 to OpticalFlow.msg

---
 mavros_extras/src/plugins/optical_flow.cpp | 38 ++++++++--------------
 mavros_msgs/msg/OpticalFlow.msg            |  6 ++--
 2 files changed, 17 insertions(+), 27 deletions(-)

diff --git a/mavros_extras/src/plugins/optical_flow.cpp b/mavros_extras/src/plugins/optical_flow.cpp
index 4aa481401..a89909247 100644
--- a/mavros_extras/src/plugins/optical_flow.cpp
+++ b/mavros_extras/src/plugins/optical_flow.cpp
@@ -119,11 +119,13 @@ class OpticalFlowPlugin : public plugin::Plugin
         flow.flow_x,
         flow.flow_y,
         0.0));
+
     auto int_flow_comp_m_xy = ftf::transform_frame_aircraft_baselink(
       Eigen::Vector3d(
         flow.flow_comp_m_x,
         flow.flow_comp_m_y,
         0.0));
+
     auto int_flow_rate_xy = ftf::transform_frame_aircraft_baselink(
       Eigen::Vector3d(
         flow.flow_rate_x,
@@ -134,15 +136,12 @@ class OpticalFlowPlugin : public plugin::Plugin
 
     flow_msg.header = header;
 
-    flow_msg.flow[0] = int_flow.x();
-    flow_msg.flow[1] = int_flow.y();
-
-    flow_msg.flow_comp_m[0] = int_flow_comp_m_xy.x();
-    flow_msg.flow_comp_m[1] = int_flow_comp_m_xy.y();
-
-    flow_msg.flow_rate[0] = int_flow_rate_xy.x();
-    flow_msg.flow_rate[1] = int_flow_rate_xy.y();
-
+    flow_msg.flow.x = int_flow.x();
+    flow_msg.flow.y = int_flow.y();
+    flow_msg.flow_comp_m.x = int_flow_comp_m_xy.x();
+    flow_msg.flow_comp_m.y = int_flow_comp_m_xy.y();
+    flow_msg.flow_rate.x = int_flow_rate_xy.x();
+    flow_msg.flow_rate.y = int_flow_rate_xy.y();
     flow_msg.ground_distance = flow.ground_distance;
     flow_msg.quality = flow.quality;
 
@@ -172,21 +171,12 @@ class OpticalFlowPlugin : public plugin::Plugin
   {
     mavlink::common::msg::OPTICAL_FLOW flow_msg = {};
 
-    auto int_flow = ftf::transform_frame_baselink_aircraft(
-      Eigen::Vector3d(
-        msg->flow[0],
-        msg->flow[1],
-        0.0));
-    auto int_flow_comp_m_xy = ftf::transform_frame_baselink_aircraft(
-      Eigen::Vector3d(
-        msg->flow_comp_m[0],
-        msg->flow_comp_m[1],
-        0.0));
-    auto int_flow_rate_xy = ftf::transform_frame_baselink_aircraft(
-      Eigen::Vector3d(
-        msg->flow_rate[0],
-        msg->flow_rate[1],
-        0.0));
+    auto int_flow =
+      ftf::transform_frame_baselink_aircraft(mavros::ftf::to_eigen(msg->flow));
+    auto int_flow_comp_m_xy =
+      ftf::transform_frame_baselink_aircraft(mavros::ftf::to_eigen(msg->flow_comp_m));
+    auto int_flow_rate_xy =
+      ftf::transform_frame_baselink_aircraft(mavros::ftf::to_eigen(msg->flow_rate));
 
     flow_msg.time_usec = get_time_usec(msg->header.stamp);
     flow_msg.sensor_id = 0;
diff --git a/mavros_msgs/msg/OpticalFlow.msg b/mavros_msgs/msg/OpticalFlow.msg
index 92b5a5f31..db857b8b4 100644
--- a/mavros_msgs/msg/OpticalFlow.msg
+++ b/mavros_msgs/msg/OpticalFlow.msg
@@ -2,8 +2,8 @@
 
 std_msgs/Header header
 
-int16[2] flow
-float32[2] flow_comp_m
+geometry_msgs/Vector3 flow
+geometry_msgs/Vector3 flow_comp_m
 uint8 quality
 float32 ground_distance
-float32[2] flow_rate
+geometry_msgs/Vector3 flow_rate