From 3a7dea1c37334966f7ea4566abab2c9e29ba0e40 Mon Sep 17 00:00:00 2001 From: Vladislavert Date: Tue, 27 Jun 2023 22:56:47 +0300 Subject: [PATCH 1/3] Added message optical flow --- mavros_extras/CMakeLists.txt | 1 + mavros_extras/mavros_plugins.xml | 6 + mavros_extras/src/plugins/optical_flow.cpp | 210 +++++++++++++++++++++ mavros_msgs/CMakeLists.txt | 1 + mavros_msgs/msg/OpticalFlow.msg | 12 ++ 5 files changed, 230 insertions(+) create mode 100644 mavros_extras/src/plugins/optical_flow.cpp create mode 100644 mavros_msgs/msg/OpticalFlow.msg diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index d8d1e568d..ac992ef49 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -111,6 +111,7 @@ add_library(mavros_extras_plugins SHARED src/plugins/obstacle_distance.cpp src/plugins/odom.cpp src/plugins/onboard_computer_status.cpp + src/plugins/optical_flow.cpp src/plugins/play_tune.cpp src/plugins/px4flow.cpp src/plugins/rangefinder.cpp diff --git a/mavros_extras/mavros_plugins.xml b/mavros_extras/mavros_plugins.xml index 30e522c40..6436128ee 100644 --- a/mavros_extras/mavros_plugins.xml +++ b/mavros_extras/mavros_plugins.xml @@ -167,6 +167,12 @@ Publishes the status of the onboard computer @plugin px4flow This plugin can publish data from PX4Flow camera to ROS + + + @brief Optical Flow custom plugin +@plugin optical_flow + +This plugin can publish data from optical flow camera to ROS @brief Ardupilot Rangefinder plugin. diff --git a/mavros_extras/src/plugins/optical_flow.cpp b/mavros_extras/src/plugins/optical_flow.cpp new file mode 100644 index 000000000..5e07c829d --- /dev/null +++ b/mavros_extras/src/plugins/optical_flow.cpp @@ -0,0 +1,210 @@ +/* + * Copyright 2023 Vladislav Chuvarjov + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief OpticalFlow plugin + * @file optical_flow.cpp + * @author Vladislav Chuvarjov + * + * @addtogroup plugin + * @{ + */ + +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/optical_flow.hpp" +#include "sensor_msgs/msg/range.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT + +/** + * @brief Optical Flow custom plugin + * @plugin optical_flow + * + * This plugin can publish data from OpticalFlow camera to ROS + */ +class OpticalFlowPlugin : public plugin::Plugin +{ +public: + explicit OpticalFlowPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "optical_flow"), + ranger_fov(0.0), + ranger_min_range(0.3), + ranger_max_range(5.0) + { + enable_node_watch_parameters(); + + node_declare_and_watch_parameter( + "frame_id", "optical_flow", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + /** + * @note Default rangefinder is Maxbotix HRLV-EZ4 + * This is a narrow beam (60cm wide at 5 meters, + * but also at 1 meter). 6.8 degrees at 5 meters, 31 degrees + * at 1 meter + */ + node_declare_and_watch_parameter( + "ranger_fov", 0.119428926, [&](const rclcpp::Parameter & p) { + ranger_fov = p.as_double(); + }); + + node_declare_and_watch_parameter( + "ranger_min_range", 0.3, [&](const rclcpp::Parameter & p) { + ranger_min_range = p.as_double(); + }); + + node_declare_and_watch_parameter( + "ranger_max_range", 5.0, [&](const rclcpp::Parameter & p) { + ranger_max_range = p.as_double(); + }); + + flow_pub = node->create_publisher( + "~/raw/optical_flow", 10); + range_pub = node->create_publisher("~/ground_distance", 10); + + flow_sub = node->create_subscription( + "~/raw/send", 1, std::bind( + &OpticalFlowPlugin::send_cb, this, + _1)); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&OpticalFlowPlugin::handle_optical_flow) + }; + } + +private: + std::string frame_id; + + double ranger_fov; + double ranger_min_range; + double ranger_max_range; + + rclcpp::Publisher::SharedPtr flow_pub; + rclcpp::Publisher::SharedPtr range_pub; + rclcpp::Subscription::SharedPtr flow_sub; + + void handle_optical_flow( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::OPTICAL_FLOW & flow, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + auto header = uas->synchronized_header(frame_id, flow.time_usec); + + /** + * Raw message with axes mapped to ROS conventions and temp in degrees celsius. + * + * The optical flow camera is essentially an angular sensor, so conversion is like + * gyroscope. (aircraft -> baselink) + */ + auto int_flow = ftf::transform_frame_aircraft_baselink( + Eigen::Vector3d( + 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, + flow.flow_rate_y, + 0.0)); + + auto flow_msg = mavros_msgs::msg::OpticalFlow(); + + flow_msg.header = header; + + 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; + + flow_pub->publish(flow_msg); + + // Rangefinder + /** + * @todo: use distance_sensor plugin only to publish this data + * (which receives DISTANCE_SENSOR msg with multiple rangefinder + * sensors data) + * + */ + auto range_msg = sensor_msgs::msg::Range(); + + range_msg.header = header; + + range_msg.radiation_type = sensor_msgs::msg::Range::INFRARED; + range_msg.field_of_view = ranger_fov; + range_msg.min_range = ranger_min_range; + range_msg.max_range = ranger_max_range; + range_msg.range = flow.ground_distance; + + range_pub->publish(range_msg); + } + + void send_cb(const mavros_msgs::msg::OpticalFlow::SharedPtr msg) + { + mavlink::common::msg::OPTICAL_FLOW flow_msg = {}; + + auto int_flow = ftf::transform_frame_baselink_aircraft( + Eigen::Vector3d( + msg->flow_x, + msg->flow_y, + 0.0)); + auto int_flow_comp_m_xy = ftf::transform_frame_baselink_aircraft( + Eigen::Vector3d( + msg->flow_comp_m_x, + msg->flow_comp_m_y, + 0.0)); + auto int_flow_rate_xy = ftf::transform_frame_baselink_aircraft( + Eigen::Vector3d( + msg->flow_rate_x, + msg->flow_rate_y, + 0.0)); + + flow_msg.time_usec = get_time_usec(msg->header.stamp); + flow_msg.sensor_id = 0; + 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.quality = msg->quality; + flow_msg.ground_distance = msg->ground_distance; // temperature in centi-degrees Celsius + flow_msg.flow_rate_x = int_flow_rate_xy.x(); + flow_msg.flow_rate_y = int_flow_rate_xy.y(); + + uas->send_message(flow_msg); + } +}; +} // namespace extra_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::OpticalFlowPlugin) diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index dfbd4e3ac..cfba07955 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -70,6 +70,7 @@ set(msg_files msg/NavControllerOutput.msg msg/OnboardComputerStatus.msg msg/OpticalFlowRad.msg + msg/OpticalFlow.msg msg/OverrideRCIn.msg msg/Param.msg msg/ParamEvent.msg diff --git a/mavros_msgs/msg/OpticalFlow.msg b/mavros_msgs/msg/OpticalFlow.msg new file mode 100644 index 000000000..2aed8ad09 --- /dev/null +++ b/mavros_msgs/msg/OpticalFlow.msg @@ -0,0 +1,12 @@ +# OPTICAL_FLOW message data + +std_msgs/Header header + +int16 flow_x +int16 flow_y +float32 flow_comp_m_x +float32 flow_comp_m_y +uint8 quality +float32 ground_distance +float32 flow_rate_x +float32 flow_rate_y From d1b44e8dc55fedd0a7e42f3cae5603d41e2014b2 Mon Sep 17 00:00:00 2001 From: Vladislavert Date: Thu, 29 Jun 2023 16:50:56 +0300 Subject: [PATCH 2/3] Added vectors to the message OpticalFlow.msg --- mavros_extras/src/plugins/optical_flow.cpp | 25 +++++++++++----------- mavros_msgs/msg/OpticalFlow.msg | 9 +++----- 2 files changed, 15 insertions(+), 19 deletions(-) diff --git a/mavros_extras/src/plugins/optical_flow.cpp b/mavros_extras/src/plugins/optical_flow.cpp index 5e07c829d..4aa481401 100644 --- a/mavros_extras/src/plugins/optical_flow.cpp +++ b/mavros_extras/src/plugins/optical_flow.cpp @@ -134,15 +134,14 @@ class OpticalFlowPlugin : public plugin::Plugin flow_msg.header = header; - flow_msg.flow_x = int_flow.x(); - flow_msg.flow_y = int_flow.y(); + flow_msg.flow[0] = int_flow.x(); + flow_msg.flow[1] = 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_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_x = int_flow_rate_xy.x(); - flow_msg.flow_rate_y = int_flow_rate_xy.y(); + flow_msg.flow_rate[0] = int_flow_rate_xy.x(); + flow_msg.flow_rate[1] = int_flow_rate_xy.y(); flow_msg.ground_distance = flow.ground_distance; flow_msg.quality = flow.quality; @@ -175,18 +174,18 @@ class OpticalFlowPlugin : public plugin::Plugin auto int_flow = ftf::transform_frame_baselink_aircraft( Eigen::Vector3d( - msg->flow_x, - msg->flow_y, + 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_x, - msg->flow_comp_m_y, + 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_x, - msg->flow_rate_y, + msg->flow_rate[0], + msg->flow_rate[1], 0.0)); flow_msg.time_usec = get_time_usec(msg->header.stamp); diff --git a/mavros_msgs/msg/OpticalFlow.msg b/mavros_msgs/msg/OpticalFlow.msg index 2aed8ad09..92b5a5f31 100644 --- a/mavros_msgs/msg/OpticalFlow.msg +++ b/mavros_msgs/msg/OpticalFlow.msg @@ -2,11 +2,8 @@ std_msgs/Header header -int16 flow_x -int16 flow_y -float32 flow_comp_m_x -float32 flow_comp_m_y +int16[2] flow +float32[2] flow_comp_m uint8 quality float32 ground_distance -float32 flow_rate_x -float32 flow_rate_y +float32[2] flow_rate From 6339691fb871c53851bca41a1afce565dde4000f Mon Sep 17 00:00:00 2001 From: Vladislavert Date: Fri, 30 Jun 2023 10:12:28 +0300 Subject: [PATCH 3/3] 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