From 88f0eb1fa3316a5b364b90d846ee97bd3f1d4aff Mon Sep 17 00:00:00 2001 From: Miguel Fernandez Cortizas Date: Mon, 24 Jun 2024 14:25:28 +0000 Subject: [PATCH] command enabled --- CMakeLists.txt | 2 +- bash_test.bash | 8 + .../as2_platform_mavlink/mavlink_platform.hpp | 76 +- launch/mavlink_launch.py | 23 +- src/mavlink_platform.cpp | 773 +++--------------- tests/pixhawk_platform_test.cpp | 2 +- ...{px4_odom_test.cpp => px4_odom_test__.cpp} | 0 7 files changed, 203 insertions(+), 681 deletions(-) create mode 100755 bash_test.bash rename tests/{px4_odom_test.cpp => px4_odom_test__.cpp} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index b86319d..5afd269 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,7 +91,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - # add_subdirectory(tests) + add_subdirectory(tests) endif() # Create the ament package diff --git a/bash_test.bash b/bash_test.bash new file mode 100755 index 0000000..71dd406 --- /dev/null +++ b/bash_test.bash @@ -0,0 +1,8 @@ +#!/bin/bash + +# This is a comment +#mavlink UDP 14581, remote port: 14541) + +# ros2 launch mavros px4.launch namespace:=drone0 fcu_url:=udp://:14540@127.0.0.1:14557 +# ros2 launch mavros px4.launch namespace:=drone0 fcu_url:=udp://:14ijkj541@127.0.0.1:14581 +ros2 launch mavros px4.launch namespace:=drone0 fcu_url:="udp://:14540@127.0.0.1:14557" diff --git a/include/as2_platform_mavlink/mavlink_platform.hpp b/include/as2_platform_mavlink/mavlink_platform.hpp index b56a16e..6962677 100644 --- a/include/as2_platform_mavlink/mavlink_platform.hpp +++ b/include/as2_platform_mavlink/mavlink_platform.hpp @@ -43,6 +43,7 @@ #include #include +#include #include #include @@ -56,11 +57,16 @@ #include #include +#include +#include +#include +#include #include "as2_core/aerial_platform.hpp" #include "as2_core/sensor.hpp" #include "as2_core/utils/tf_utils.hpp" +#include "as2_core/synchronous_service_client.hpp" namespace as2_platform_mavlink { @@ -105,32 +111,41 @@ class MavlinkPlatform : public as2::AerialPlatform void externalOdomCb(const geometry_msgs::msg::TwistStamped::SharedPtr msg); // mavlink_ subscribers - // rclcpp::Subscription::SharedPtr px4_imu_sub_; - // rclcpp::Subscription::SharedPtr px4_odometry_sub_; - // rclcpp::Subscription::SharedPtr px4_vehicle_control_mode_sub_; - // rclcpp::Subscription::SharedPtr px4_timesync_sub_; - // rclcpp::Subscription::SharedPtr px4_battery_sub_; - // rclcpp::Subscription::SharedPtr px4_gps_sub_; - - // mavlink_ publishers - // rclcpp::Publisher::SharedPtr - // px4_manual_control_switches_pub_; - // rclcpp::Publisher::SharedPtr px4_offboard_control_mode_pub_; - // rclcpp::Publisher::SharedPtr px4_trajectory_setpoint_pub_; - // rclcpp::Publisher::SharedPtr px4_vehicle_command_pub_; - // rclcpp::Publisher::SharedPtr - // px4_vehicle_attitude_setpoint_pub_; - // rclcpp::Publisher::SharedPtr px4_vehicle_rates_setpoint_pub_; + + rclcpp::Subscription::SharedPtr mavlink_state_sub_; + void mavlinkStateCb(const mavros_msgs::msg::State::SharedPtr msg) + { + this->platform_info_msg_.set__connected(msg->connected); + this->platform_info_msg_.set__armed(msg->armed); + if (msg->mode == "OFFBOARD") { + this->platform_info_msg_.set__offboard(true); + } else { + this->platform_info_msg_.set__offboard(false); + } + } + + // mavlink clients + as2::SynchronousServiceClient::SharedPtr mavlink_arm_client_; + as2::SynchronousServiceClient::SharedPtr mavlink_set_mode_client_; + + + // mavlink publishers + + rclcpp::Publisher::SharedPtr mavlink_pose_pub_; + rclcpp::Publisher::SharedPtr mavlink_twist_setpoint_pub_; + rclcpp::Publisher::SharedPtr mavlink_acro_setpoint_pub_; + // mavlink_ Functions - void mavlink_arm(); - void mavlink_disarm(); - void mavlink_publishOffboardControlMode(); - void mavlink_publishTrajectorySetpoint(); - void mavlink_publishAttitudeSetpoint(); - void mavlink_publishRatesSetpoint(); - void mavlink_publishVehicleCommand(uint16_t command, float param1 = 0.0, float param2 = 0.0); - void mavlink_publishVisualOdometry(); + bool mavlink_callOffboardControlMode(); + + // void mavlink_publishTrajectorySetpoint(); + void mavlink_publishAttitudeSetpoint(const geometry_msgs::msg::Quaternion & q, double thrust); + void mavlink_publishRatesSetpoint(double droll, double dpitch, double dyaw, double thrust); + void mavlink_publishTwistSetpoint(const geometry_msgs::msg::TwistStamped & msg); + void mavlink_publishPoseSetpoint(const geometry_msgs::msg::PoseStamped & msg); + // void mavlink_publishVehicleCommand(uint16_t command, float param1 = 0.0, float param2 = 0.0); + // void mavlink_publishVisualOdometry(); private: bool manual_from_operator_ = false; @@ -139,11 +154,7 @@ class MavlinkPlatform : public as2::AerialPlatform std::atomic timestamp_; - // px4_msgs::msg::OffboardControlMode px4_offboard_control_mode_; - // px4_msgs::msg::TrajectorySetpoint px4_trajectory_setpoint_; - // px4_msgs::msg::VehicleAttitudeSetpoint px4_attitude_setpoint_; - // px4_msgs::msg::VehicleRatesSetpoint px4_rates_setpoint_; - // px4_msgs::msg::VehicleOdometry px4_visual_odometry_msg_; + geometry_msgs::msg::PoseStamped mavlink_pose_msg_; float max_thrust_; float min_thrust_; @@ -152,13 +163,8 @@ class MavlinkPlatform : public as2::AerialPlatform std::string base_link_frame_id_; std::string odom_frame_id_; - Eigen::Quaterniond q_ned_to_enu_; - Eigen::Quaterniond q_enu_to_ned_; - Eigen::Quaterniond q_aircraft_to_baselink_; - Eigen::Quaterniond q_baselink_to_aircraft_; - private: - // mavlink_ Callbacks + // mavlink Callbacks // void mavlink_imuCallback(const px4_msgs::msg::SensorCombined::SharedPtr msg); // void mavlink_odometryCallback(const px4_msgs::msg::VehicleOdometry::SharedPtr msg); // void mavlink_VehicleControlModeCallback(const px4_msgs::msg::VehicleControlMode::SharedPtr msg); diff --git a/launch/mavlink_launch.py b/launch/mavlink_launch.py index 56d6102..88b8ddf 100644 --- a/launch/mavlink_launch.py +++ b/launch/mavlink_launch.py @@ -34,8 +34,9 @@ __license__ = 'BSD-3-Clause' from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -52,10 +53,12 @@ def generate_launch_description(): 'config', 'platform_config_file.yaml' ]) + mavros_launch_file = PathJoinSubstitution([ + FindPackageShare('mavros'), 'launch', 'px4.launch']) + return LaunchDescription([ DeclareLaunchArgument('namespace', - default_value=EnvironmentVariable( - 'AEROSTACK2_SIMULATION_DRONE_ID'), + default_value='drone0', description='Drone namespace'), DeclareLaunchArgument('control_modes_file', default_value=control_modes, @@ -63,7 +66,9 @@ def generate_launch_description(): DeclareLaunchArgument('platform_config_file', default_value=platform_config_file, description='Platform configuration file'), - + DeclareLaunchArgument('fcu_url', + default_value='udp://:14540@127.0.0.1:14557', + description='Mavlink connection URL'), Node( package='as2_platform_mavlink', executable='as2_platform_mavlink_node', @@ -77,5 +82,11 @@ def generate_launch_description(): }, LaunchConfiguration('platform_config_file') ] - ) + ), + IncludeLaunchDescription(AnyLaunchDescriptionSource([mavros_launch_file]), + launch_arguments={ + 'namespace': 'drone0/mavros', + 'fcu_url': LaunchConfiguration('fcu_url'), + }.items()) + ]) diff --git a/src/mavlink_platform.cpp b/src/mavlink_platform.cpp index aad46aa..b207d42 100644 --- a/src/mavlink_platform.cpp +++ b/src/mavlink_platform.cpp @@ -36,6 +36,7 @@ */ #include "as2_platform_mavlink/mavlink_platform.hpp" +#include void notImplemented() { @@ -71,9 +72,19 @@ MavlinkPlatform::MavlinkPlatform(const rclcpp::NodeOptions & options) RCLCPP_INFO(this->get_logger(), "External odometry mode: %s", external_odom_ ? "true" : "false"); // declare mavlink_ subscribers - // px4_imu_sub_ = this->create_subscription( - // "/fmu/out/sensor_combined", rclcpp::SensorDataQoS(), - // std::bind(&MavlinkPlatform::px4imuCallback, this, std::placeholders::_1)); + + mavlink_state_sub_ = this->create_subscription( + "mavros/state", rclcpp::SensorDataQoS(), + std::bind(&MavlinkPlatform::mavlinkStateCb, this, std::placeholders::_1)); + + // declare mavlink services + + mavlink_arm_client_ = + std::make_shared>( + "mavros/cmd/arming", this); + mavlink_set_mode_client_ = + std::make_shared>( + "mavros/set_mode", this); // // px4_timesync_sub_ = this->create_subscription( // // "/fmu/out/timesync_status", rclcpp::SensorDataQoS(), @@ -81,10 +92,6 @@ MavlinkPlatform::MavlinkPlatform(const rclcpp::NodeOptions & options) // // timestamp_.store(msg->timestamp); // // }); - // px4_vehicle_control_mode_sub_ = this->create_subscription( - // "/fmu/out/vehicle_control_mode", rclcpp::SensorDataQoS(), - // std::bind(&MavlinkPlatform::px4VehicleControlModeCallback, this, std::placeholders::_1)); - // px4_gps_sub_ = this->create_subscription( // "/fmu/out/vehicle_gps_position", rclcpp::SensorDataQoS(), // std::bind(&MavlinkPlatform::px4GpsCallback, this, std::placeholders::_1)); @@ -110,22 +117,13 @@ MavlinkPlatform::MavlinkPlatform(const rclcpp::NodeOptions & options) // } // declare mavlink_ publishers - // px4_offboard_control_mode_pub_ = this->create_publisher( - // "/fmu/in/offboard_control_mode", rclcpp::SensorDataQoS()); - // px4_trajectory_setpoint_pub_ = this->create_publisher( - // "/fmu/in/trajectory_setpoint", rclcpp::SensorDataQoS()); - // px4_vehicle_attitude_setpoint_pub_ = - // this->create_publisher( - // "/fmu/in/vehicle_attitude_setpoint", rclcpp::SensorDataQoS()); - // px4_vehicle_rates_setpoint_pub_ = this->create_publisher( - // "/fmu/in/vehicle_rates_setpoint", rclcpp::SensorDataQoS()); - // px4_vehicle_command_pub_ = this->create_publisher( - // "/fmu/in/vehicle_command", rclcpp::SensorDataQoS()); - // px4_visual_odometry_pub_ = this->create_publisher( - // "/fmu/in/vehicle_visual_odometry", rclcpp::SensorDataQoS()); - - // px4_manual_control_switches_pub_ = this->create_publisher( - // "fmu/in/manual_control_switches", rclcpp::SensorDataQoS()); + + mavlink_pose_pub_ = this->create_publisher( + "mavros/setpoint_position/local", 10); + mavlink_acro_setpoint_pub_ = this->create_publisher( + "mavros/setpoint_raw/attitude", 10); + mavlink_twist_setpoint_pub_ = this->create_publisher( + "mavros/setpoint_velocity/cmd_vel", 10); } void MavlinkPlatform::configureSensors() @@ -140,255 +138,116 @@ void MavlinkPlatform::configureSensors() bool MavlinkPlatform::ownSetArmingState(bool state) { - return false; -// { -// if (state) { -// this->mavlink_arm(); -// } else { -// set_disarm_ = true; -// // TODO(miferco97): wait for takeoff_status to be able to mavlink_ disarm -// this->mavlink_disarm(); -// } -// return true; + auto request = std::make_shared(); + auto response = std::make_shared(); + request->value = state; + + auto result = mavlink_arm_client_->sendRequest(request, response); + if (!result) { + RCLCPP_ERROR(this->get_logger(), "Failed to arm/disarm the vehicle"); + return false; + } + return response->success; } bool MavlinkPlatform::ownSetOffboardControl(bool offboard) { - return false; -} -// // TODO(miferco97): CREATE A DEFAULT CONTROL MODE FOR BEING ABLE TO SWITCH TO OFFBOARD -// // MODE BEFORE RUNNING THE CONTROLLER - -// if (offboard == false) { -// RCLCPP_ERROR( -// this->get_logger(), -// "Turning into MANUAL Mode is not allowed from the onboard computer"); -// return false; -// } - -// px4_offboard_control_mode_ = px4_msgs::msg::OffboardControlMode(); // RESET CONTROL MODE -// px4_offboard_control_mode_.body_rate = true; -// resetRatesSetpoint(); + // TODO(miferco97): CREATE A DEFAULT CONTROL MODE FOR BEING ABLE TO SWITCH TO OFFBOARD + // MODE BEFORE RUNNING THE CONTROLLER + + if (offboard == false) { + RCLCPP_ERROR( + this->get_logger(), + "Turning into MANUAL Mode is not allowed from the onboard computer"); + return false; + } -// RCLCPP_DEBUG(this->get_logger(), "Switching to OFFBOARD mode"); -// // Following mavlink_ offboard guidelines -// rclcpp::Rate r(100); -// for (int i = 0; i < 10; i++) { -// mavlink_publishRatesSetpoint(); -// r.sleep(); -// } -// mavlink_publishOffboardControlMode(); -// return true; -// } + RCLCPP_DEBUG(this->get_logger(), "Switching to OFFBOARD mode"); + auto out = mavlink_callOffboardControlMode(); + if (!out) { + RCLCPP_ERROR(this->get_logger(), "Failed to set OFFBOARD mode"); + return false; + } + if (!getArmingState()) { + this->ownSetArmingState(true); + } + return true; +} bool MavlinkPlatform::ownSetPlatformControlMode(const as2_msgs::msg::ControlMode & msg) { - return false; + switch (msg.control_mode) { + case as2_msgs::msg::ControlMode::POSITION: { + RCLCPP_INFO(this->get_logger(), "POSITION_MODE ENABLED"); + } break; + case as2_msgs::msg::ControlMode::SPEED: { + RCLCPP_INFO(this->get_logger(), "SPEED_MODE ENABLED"); + } break; + case as2_msgs::msg::ControlMode::ATTITUDE: { + RCLCPP_INFO(this->get_logger(), "ATTITUDE_MODE ENABLED"); + } break; + case as2_msgs::msg::ControlMode::ACRO: { + RCLCPP_INFO(this->get_logger(), "ACRO_MODE ENABLED"); + } break; + default: { + RCLCPP_WARN(this->get_logger(), "CONTROL MODE %d NOT SUPPORTED", msg.control_mode); + has_mode_settled_ = false; + return false; + } + } + has_mode_settled_ = true; + return true; } -// px4_offboard_control_mode_ = px4_msgs::msg::OffboardControlMode(); // RESET CONTROL MODE - -// /* PIXHAWK CONTROL MODES: -// px4_offboard_control_mode_.position -> x,y,z -// px4_offboard_control_mode_.velocity -> vx,vy,vz -// px4_offboard_control_mode_.acceleration -> ax,ay,az -// px4_offboard_control_mode_.attitude -> q(r,p,y) + T(tx,ty,tz) in -// multicopters tz = -Collective_Thrust px4_offboard_control_mode_.body_rate -> -// p ,q ,r + T(tx,ty,tz) in multicopters tz = -Collective_Thrust */ - -// switch (msg.control_mode) { -// case as2_msgs::msg::ControlMode::POSITION: { -// px4_offboard_control_mode_.position = true; -// RCLCPP_INFO(this->get_logger(), "POSITION_MODE ENABLED"); -// } break; -// case as2_msgs::msg::ControlMode::SPEED: { -// px4_offboard_control_mode_.velocity = true; -// RCLCPP_INFO(this->get_logger(), "SPEED_MODE ENABLED"); -// } break; -// case as2_msgs::msg::ControlMode::ATTITUDE: { -// px4_offboard_control_mode_.attitude = true; -// RCLCPP_INFO(this->get_logger(), "ATTITUDE_MODE ENABLED"); -// } break; -// // TODO(miferco97): ACCEL MODE NOT IMPLEMENTED -// // case as2_msgs::msg::ControlMode::ACCEL_MODE: { -// // px4_offboard_control_mode_.acceleration = true; -// // RCLCPP_INFO(this->get_logger(), "ACCEL_MODE ENABLED"); -// // } break; -// case as2_msgs::msg::ControlMode::ACRO: { -// px4_offboard_control_mode_.body_rate = true; -// RCLCPP_INFO(this->get_logger(), "ACRO_MODE ENABLED"); -// } break; -// default: -// RCLCPP_WARN(this->get_logger(), "CONTROL MODE %d NOT SUPPORTED", msg.control_mode); -// has_mode_settled_ = false; -// return false; -// } - -// has_mode_settled_ = true; -// return true; -// } void MavlinkPlatform::sendCommand() { + if (!getArmingState() || !getOffboardMode() || !has_mode_settled_) { + mavlink_publishRatesSetpoint(0.0, 0.0, 0.0, 1.0 * min_thrust_); + } + ownSendCommand(); return; -// // Actuator commands are published continously - -// if (!getArmingState()) { -// return; -// } - -// if ((!getOffboardMode() || !has_mode_settled_) && !manual_from_operator_) { -// px4_offboard_control_mode_ = px4_msgs::msg::OffboardControlMode(); // RESET CONTROL MODE -// px4_offboard_control_mode_.body_rate = true; - -// resetRatesSetpoint(); -// px4_rates_setpoint_.thrust_body[2] = -1.0 * min_thrust_; - -// // RCLCPP_INFO(this->get_logger(), "SendCommand - Offboard Mode - Publish Rates Setpoint"); -// mavlink_publishRatesSetpoint(); -// return; -// } - -// ownSendCommand(); -// return; } bool MavlinkPlatform::ownSendCommand() { - return false; -} -/* as2_msgs::msg::ControlMode platform_control_mode = this->getControlMode(); // Switch case to set setpoint switch (platform_control_mode.control_mode) { case as2_msgs::msg::ControlMode::POSITION: { - this->resetTrajectorySetpoint(); if (platform_control_mode.yaw_mode == as2_msgs::msg::ControlMode::YAW_ANGLE) { - px4_trajectory_setpoint_.yaw = yawEnuToAircraft(this->command_pose_msg_); + mavlink_publishPoseSetpoint(this->command_pose_msg_); } else { // ENU --> NED - px4_trajectory_setpoint_.yawspeed = -command_twist_msg_.twist.angular.z; + notImplemented(); } - - Eigen::Vector3d position_enu; - position_enu.x() = this->command_pose_msg_.pose.position.x; - position_enu.y() = this->command_pose_msg_.pose.position.y; - position_enu.z() = this->command_pose_msg_.pose.position.z; - - // TODO(miferco97): px4_ros_com done - const Eigen::PermutationMatrix<3> ned_enu_reflection_xy(Eigen::Vector3i(1, 0, 2)); - const Eigen::DiagonalMatrix ned_enu_reflection_z(1, 1, -1); - Eigen::Vector3d position_ned = ned_enu_reflection_xy * - (ned_enu_reflection_z * position_enu); - // Eigen::Vector3d position_ned = px4_ros_com::frame_transforms::transform_static_frame( - // position_enu, px4_ros_com::frame_transforms::StaticTF::ENU_TO_NED); - - px4_trajectory_setpoint_.position[0] = position_ned.x(); - px4_trajectory_setpoint_.position[1] = position_ned.y(); - px4_trajectory_setpoint_.position[2] = position_ned.z(); } break; case as2_msgs::msg::ControlMode::SPEED: { - this->resetTrajectorySetpoint(); if (platform_control_mode.yaw_mode == as2_msgs::msg::ControlMode::YAW_ANGLE) { - px4_trajectory_setpoint_.yaw = yawEnuToAircraft(this->command_pose_msg_); + notImplemented(); } else { - // ENU --> NED - px4_trajectory_setpoint_.yawspeed = -command_twist_msg_.twist.angular.z; + mavlink_publishTwistSetpoint(this->command_twist_msg_); } - - Eigen::Vector3d speed_enu; - speed_enu.x() = this->command_twist_msg_.twist.linear.x; - speed_enu.y() = this->command_twist_msg_.twist.linear.y; - speed_enu.z() = this->command_twist_msg_.twist.linear.z; - - // TODO(miferco97): px4_ros_com done - const Eigen::PermutationMatrix<3> ned_enu_reflection_xy(Eigen::Vector3i(1, 0, 2)); - const Eigen::DiagonalMatrix ned_enu_reflection_z(1, 1, -1); - Eigen::Vector3d speed_ned = ned_enu_reflection_xy * (ned_enu_reflection_z * speed_enu); - // Eigen::Vector3d speed_ned = px4_ros_com::frame_transforms::transform_static_frame( - // speed_enu, px4_ros_com::frame_transforms::StaticTF::ENU_TO_NED); - - px4_trajectory_setpoint_.velocity[0] = speed_ned.x(); - px4_trajectory_setpoint_.velocity[1] = speed_ned.y(); - px4_trajectory_setpoint_.velocity[2] = speed_ned.z(); } break; case as2_msgs::msg::ControlMode::ATTITUDE: { - this->resetAttitudeSetpoint(); - if (platform_control_mode.yaw_mode == as2_msgs::msg::ControlMode::YAW_SPEED) { - RCLCPP_WARN_ONCE(this->get_logger(), "Yaw Speed control not supported on ATTITUDE mode"); - } - - Eigen::Quaterniond q_enu; - q_enu.w() = this->command_pose_msg_.pose.orientation.w; - q_enu.x() = this->command_pose_msg_.pose.orientation.x; - q_enu.y() = this->command_pose_msg_.pose.orientation.y; - q_enu.z() = this->command_pose_msg_.pose.orientation.z; - - // TODO(miferco97): px4_ros_com done - Eigen::Quaterniond q_ned = q_enu_to_ned_ * q_enu; - Eigen::Quaterniond q_aircraft = q_ned * q_baselink_to_aircraft_; - // Eigen::Quaterniond q_ned = px4_ros_com::frame_transforms::transform_orientation( - // q_enu, px4_ros_com::frame_transforms::StaticTF::ENU_TO_NED); - // Eigen::Quaterniond q_aircraft = px4_ros_com::frame_transforms::transform_orientation( - // q_ned, px4_ros_com::frame_transforms::StaticTF::BASELINK_TO_AIRCRAFT); - - px4_attitude_setpoint_.q_d[0] = q_aircraft.w(); - px4_attitude_setpoint_.q_d[1] = q_aircraft.x(); - px4_attitude_setpoint_.q_d[2] = q_aircraft.y(); - px4_attitude_setpoint_.q_d[3] = q_aircraft.z(); - - // minus because px4 uses NED (Z is downwards) - if (command_thrust_msg_.thrust < min_thrust_) { - px4_attitude_setpoint_.thrust_body[2] = -min_thrust_; - if (this->set_disarm_) { - px4_rates_setpoint_.thrust_body[2] = 0.0f; - } - } else { - px4_attitude_setpoint_.thrust_body[2] = -command_thrust_msg_.thrust / max_thrust_; - } + mavlink_publishAttitudeSetpoint( + this->command_pose_msg_.pose.orientation, + this->command_thrust_msg_.thrust); } break; case as2_msgs::msg::ControlMode::ACRO: { - this->resetRatesSetpoint(); - if (platform_control_mode.yaw_mode == as2_msgs::msg::ControlMode::YAW_ANGLE) { - RCLCPP_WARN_ONCE(this->get_logger(), "Yaw Angle control not supported on ACRO mode"); - } - - // FLU --> FRD - px4_rates_setpoint_.roll = command_twist_msg_.twist.angular.x; - px4_rates_setpoint_.pitch = -command_twist_msg_.twist.angular.y; - px4_rates_setpoint_.yaw = -command_twist_msg_.twist.angular.z; - - // minus because px4 uses NED (Z is downwards) - if (command_thrust_msg_.thrust < min_thrust_) { - px4_rates_setpoint_.thrust_body[2] = -min_thrust_; - if (this->set_disarm_) { - px4_rates_setpoint_.thrust_body[2] = 0.0f; - } - } else { - px4_rates_setpoint_.thrust_body[2] = -command_thrust_msg_.thrust / max_thrust_; - } + mavlink_publishRatesSetpoint( + this->command_twist_msg_.twist.angular.x, + this->command_twist_msg_.twist.angular.y, + this->command_twist_msg_.twist.angular.z, + this->command_thrust_msg_.thrust); } break; default: return false; } - - if (px4_offboard_control_mode_.attitude) { - this->mavlink_publishAttitudeSetpoint(); - } else if (px4_offboard_control_mode_.body_rate) { - this->mavlink_publishRatesSetpoint(); - } else if ( // NOLINT - px4_offboard_control_mode_.position || - px4_offboard_control_mode_.velocity || - px4_offboard_control_mode_.acceleration) - { - this->mavlink_publishTrajectorySetpoint(); - } return true; } -*/ void MavlinkPlatform::ownKillSwitch() { - notImplemented(); // RCLCPP_ERROR(this->get_logger(), "KILL SWITCH TRIGGERED"); // px4_msgs::msg::ManualControlSwitches kill_switch_msg; @@ -398,41 +257,6 @@ void MavlinkPlatform::ownKillSwitch() void MavlinkPlatform::ownStopPlatform() {RCLCPP_WARN(this->get_logger(), "NOT IMPLEMENTED");} -// void MavlinkPlatform::resetTrajectorySetpoint() -// { -// px4_trajectory_setpoint_.position[0] = NAN; -// px4_trajectory_setpoint_.position[1] = NAN; -// px4_trajectory_setpoint_.position[2] = NAN; - -// px4_trajectory_setpoint_.velocity[0] = NAN; -// px4_trajectory_setpoint_.velocity[1] = NAN; -// px4_trajectory_setpoint_.velocity[2] = NAN; - -// px4_trajectory_setpoint_.yaw = NAN; -// px4_trajectory_setpoint_.yawspeed = NAN; - -// px4_trajectory_setpoint_.acceleration = std::array{NAN, NAN, NAN}; -// px4_trajectory_setpoint_.jerk = std::array{NAN, NAN, NAN}; -// } - -// void MavlinkPlatform::resetAttitudeSetpoint() -// { -// px4_attitude_setpoint_.pitch_body = NAN; -// px4_attitude_setpoint_.roll_body = NAN; -// px4_attitude_setpoint_.yaw_body = NAN; - -// px4_attitude_setpoint_.q_d = std::array{0, 0, 0, 1}; -// px4_attitude_setpoint_.thrust_body = std::array{0, 0, -min_thrust_}; -// } - -// void MavlinkPlatform::resetRatesSetpoint() -// { -// px4_rates_setpoint_.roll = 0.0f; -// px4_rates_setpoint_.pitch = 0.0f; -// px4_rates_setpoint_.yaw = 0.0f; -// px4_attitude_setpoint_.thrust_body = std::array{0, 0, -min_thrust_}; -// } - // void MavlinkPlatform::externalOdomCb(const geometry_msgs::msg::TwistStamped::SharedPtr _twist_msg) // { // try { @@ -455,387 +279,60 @@ void MavlinkPlatform::ownStopPlatform() {RCLCPP_WARN(this->get_logger(), "NOT IM /** ------------------------- mavlink_ FUNCTIONS -------------------------*/ /** -----------------------------------------------------------------*/ -/** - * @brief Send a command to Arm the vehicle - */ -// void MavlinkPlatform::mavlink_arm() -// { -// mavlink_publishVehicleCommand( -// px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, -// 1.0); -// RCLCPP_DEBUG(this->get_logger(), "Arm command send"); -// } - -// /** -// * @brief Send a command to Disarm the vehicle -// */ -// void MavlinkPlatform::mavlink_disarm() -// { -// mavlink_publishVehicleCommand( -// px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, -// 0.0); -// RCLCPP_DEBUG(this->get_logger(), "Disarm command send"); -// } // /** // * @brief Publish the offboard control mode. // * // */ -// void MavlinkPlatform::mavlink_publishOffboardControlMode() -// { -// mavlink_publishVehicleCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); -// RCLCPP_DEBUG(this->get_logger(), "OFFBOARD mode enabled"); -// } - -// /** -// * @brief Publish a trajectory setpoint -// */ -// void MavlinkPlatform::mavlink_publishTrajectorySetpoint() -// { -// // px4_trajectory_setpoint_.timestamp = timestamp_.load(); -// // px4_offboard_control_mode_.timestamp = timestamp_.load(); -// px4_trajectory_setpoint_.timestamp = this->get_clock()->now().nanoseconds() / 1000; -// px4_offboard_control_mode_.timestamp = this->get_clock()->now().nanoseconds() / 1000; - -// px4_trajectory_setpoint_pub_->publish(px4_trajectory_setpoint_); -// px4_offboard_control_mode_pub_->publish(px4_offboard_control_mode_); -// } - -// /** -// * @brief Publish a attitude setpoint -// */ -// void MavlinkPlatform::mavlink_publishAttitudeSetpoint() -// { -// // px4_attitude_setpoint_.timestamp = timestamp_.load(); -// // px4_offboard_control_mode_.timestamp = timestamp_.load(); -// px4_attitude_setpoint_.timestamp = this->get_clock()->now().nanoseconds() / 1000; -// px4_offboard_control_mode_.timestamp = this->get_clock()->now().nanoseconds() / 1000; - -// px4_vehicle_attitude_setpoint_pub_->publish(px4_attitude_setpoint_); -// px4_offboard_control_mode_pub_->publish(px4_offboard_control_mode_); -// } - -// /** -// * @brief Publish a vehicle rates setpoint -// */ -// void MavlinkPlatform::mavlink_publishRatesSetpoint() -// { -// // px4_rates_setpoint_.timestamp = timestamp_.load(); -// // px4_offboard_control_mode_.timestamp = timestamp_.load(); -// px4_rates_setpoint_.timestamp = this->get_clock()->now().nanoseconds() / 1000; -// px4_offboard_control_mode_.timestamp = this->get_clock()->now().nanoseconds() / 1000; - -// px4_vehicle_rates_setpoint_pub_->publish(px4_rates_setpoint_); -// px4_offboard_control_mode_pub_->publish(px4_offboard_control_mode_); -// } - -// /** -// * @brief Publish vehicle commands -// * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD -// * codes) -// * @param param1 Command parameter 1 -// * @param param2 Command parameter 2 -// */ -// void MavlinkPlatform::mavlink_publishVehicleCommand(uint16_t command, float param1, float param2) -// { -// px4_msgs::msg::VehicleCommand msg{}; -// // msg.timestamp = timestamp_.load(); -// msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; -// msg.param1 = param1; -// msg.param2 = param2; -// msg.command = command; -// msg.target_system = 1; -// msg.target_component = 1; -// msg.source_system = 1; -// msg.source_component = 1; -// msg.from_external = true; - -// px4_vehicle_command_pub_->publish(msg); -// } - -// /// @brief See documentation: https://docs.px4.io/v1.13/en/msg_docs/vehicle_odometry.html -// void MavlinkPlatform::mavlink_publishVisualOdometry() -// { -// // Position in meters. Frame of reference defined by local_frame -// px4_visual_odometry_msg_.pose_frame = px4_msgs::msg::VehicleOdometry::POSE_FRAME_FRD; -// // FLU --> FRD -// px4_visual_odometry_msg_.position[0] = odometry_msg_.pose.pose.position.x; -// px4_visual_odometry_msg_.position[1] = -odometry_msg_.pose.pose.position.y; -// px4_visual_odometry_msg_.position[2] = -odometry_msg_.pose.pose.position.z; - -// // Quaternion rotation from FRD body frame to refernce frame -// Eigen::Quaterniond q_baselink( -// odometry_msg_.pose.pose.orientation.w, odometry_msg_.pose.pose.orientation.x, -// odometry_msg_.pose.pose.orientation.y, odometry_msg_.pose.pose.orientation.z); -// // BASELINK --> AIRCRAFT (FLU --> FRD) - -// // TODO(miferco97): px4_ros_com done - -// Eigen::Quaterniond q_aircraft = q_baselink * q_baselink_to_aircraft_.inverse(); -// // Eigen::Quaterniond q_aircraft = -// // px4_ros_com::frame_transforms::transform_orientation(q_baselink, -// // px4_ros_com::frame_transforms::StaticTF::BASELINK_TO_AIRCRAFT); - -// px4_visual_odometry_msg_.q[0] = q_aircraft.w(); -// px4_visual_odometry_msg_.q[1] = q_aircraft.x(); -// px4_visual_odometry_msg_.q[2] = q_aircraft.y(); -// px4_visual_odometry_msg_.q[3] = q_aircraft.z(); - -// // Velocity in meters/sec. Frame of reference defined by velocity_frame variable. -// px4_visual_odometry_msg_.velocity_frame = px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_BODY_FRD; -// // FLU --> FRD -// px4_visual_odometry_msg_.velocity[0] = odometry_msg_.twist.twist.linear.x; -// px4_visual_odometry_msg_.velocity[1] = -odometry_msg_.twist.twist.linear.y; -// px4_visual_odometry_msg_.velocity[2] = -odometry_msg_.twist.twist.linear.z; - -// // Angular rate in body-fixed frame (rad/s). -// // FLU --> FRD -// px4_visual_odometry_msg_.angular_velocity[0] = odometry_msg_.twist.twist.angular.x; -// px4_visual_odometry_msg_.angular_velocity[1] = -odometry_msg_.twist.twist.angular.y; -// px4_visual_odometry_msg_.angular_velocity[2] = -odometry_msg_.twist.twist.angular.z; - -// // px4_visual_odometry_msg_.timestamp = timestamp_.load(); -// px4_visual_odometry_msg_.timestamp = this->get_clock()->now().nanoseconds() / 1000; -// px4_visual_odometry_pub_->publish(px4_visual_odometry_msg_); -// } - -/** -----------------------------------------------------------------*/ -/** ---------------------- SUBSCRIBER CALLBACKS ---------------------*/ -/** -----------------------------------------------------------------*/ - -// void MavlinkPlatform::px4imuCallback(const px4_msgs::msg::SensorCombined::SharedPtr msg) -// { -// auto timestamp = this->get_clock()->now(); -// sensor_msgs::msg::Imu imu_msg; -// imu_msg.header.stamp = timestamp; -// imu_msg.header.frame_id = base_link_frame_id_; -// imu_msg.linear_acceleration.x = msg->accelerometer_m_s2[0]; -// imu_msg.linear_acceleration.y = msg->accelerometer_m_s2[1]; -// imu_msg.linear_acceleration.z = msg->accelerometer_m_s2[2]; -// imu_msg.angular_velocity.x = msg->gyro_rad[0]; -// imu_msg.angular_velocity.y = msg->gyro_rad[1]; -// imu_msg.angular_velocity.z = msg->gyro_rad[2]; - -// imu_sensor_ptr_->updateData(imu_msg); -// } - -// /// @brief See documentation: https://docs.px4.io/v1.13/en/msg_docs/vehicle_odometry.html -// /// @param msg vehicle odometry -// void MavlinkPlatform::px4odometryCallback(const px4_msgs::msg::VehicleOdometry::SharedPtr msg) -// { -// nav_msgs::msg::Odometry odom_msg; -// odom_msg.header.stamp = this->get_clock()->now(); -// odom_msg.header.frame_id = odom_frame_id_; -// odom_msg.child_frame_id = base_link_frame_id_; - -// switch (msg->pose_frame) { -// case px4_msgs::msg::VehicleOdometry::POSE_FRAME_NED: -// case px4_msgs::msg::VehicleOdometry::POSE_FRAME_FRD: { -// // as2 understand initial Forward as North, so LOCAL_FRD equals LOCAL_NED - -// // Position in meters. Frame of reference defined by local_frame -// Eigen::Vector3d pos_ned(msg->position[0], msg->position[1], msg->position[2]); - -// // TODO(miferco97): px4_ros_com done -// const Eigen::PermutationMatrix<3> ned_enu_reflection_xy(Eigen::Vector3i(1, 0, 2)); -// const Eigen::DiagonalMatrix ned_enu_reflection_z(1, 1, -1); -// Eigen::Vector3d pos_enu = ned_enu_reflection_xy * (ned_enu_reflection_z * pos_ned); -// // Eigen::Vector3d pos_enu = px4_ros_com::frame_transforms::ned_to_enu_local_frame(pos_ned); - -// odom_msg.pose.pose.position.x = pos_enu[0]; -// odom_msg.pose.pose.position.y = pos_enu[1]; -// odom_msg.pose.pose.position.z = pos_enu[2]; - -// // Quaternion rotation from FRD body frame to refernce frame (LOCAL_FRAME_NED) -// // q_offset Quaternion rotation from odometry reference frame to navigation frame -// Eigen::Quaterniond q_local_ned(msg->q[0], msg->q[1], msg->q[2], msg->q[3]); - -// // TODO(miferco97): px4_ros_com -// Eigen::Quaterniond q_local_enu = q_ned_to_enu_ * q_local_ned; -// Eigen::Quaterniond q_flu = q_local_enu * q_aircraft_to_baselink_; - -// // Eigen::Quaterniond q_local_enu = -// // px4_ros_com::frame_transforms::ned_to_enu_orientation(q_local_ned); -// // // AIRCRAFT (FRD): BODY FRD --> BODY FLU -// // Eigen::Quaterniond q_flu = -// // px4_ros_com::frame_transforms::aircraft_to_baselink_orientation(q_local_enu); - -// odom_msg.pose.pose.orientation.w = q_flu.w(); -// odom_msg.pose.pose.orientation.x = q_flu.x(); -// odom_msg.pose.pose.orientation.y = q_flu.y(); -// odom_msg.pose.pose.orientation.z = q_flu.z(); -// break; -// } -// default: -// RCLCPP_ERROR(this->get_logger(), "Vehicle Odometry local frame not supported."); -// break; -// } - -// // Velocity in meters/sec. Frame of reference defined by velocity_frame variable -// switch (msg->velocity_frame) { -// case px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_NED: -// case px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_FRD: { -// // Convert from NED to FLU -// Eigen::Vector3d vel_ned = -// Eigen::Vector3d(msg->velocity[0], msg->velocity[1], msg->velocity[2]); - -// // TODO(miferco97): px4_ros_com done -// const Eigen::PermutationMatrix<3> ned_enu_reflection_xy(Eigen::Vector3i(1, 0, 2)); -// const Eigen::DiagonalMatrix ned_enu_reflection_z(1, 1, -1); -// Eigen::Vector3d vel_enu = ned_enu_reflection_xy * (ned_enu_reflection_z * vel_ned); -// // Eigen::Vector3d vel_enu = px4_ros_com::frame_transforms::ned_to_enu_local_frame(vel_ned); - -// Eigen::Quaterniond q_baselink( -// odom_msg.pose.pose.orientation.w, odom_msg.pose.pose.orientation.x, -// odom_msg.pose.pose.orientation.y, odom_msg.pose.pose.orientation.z); - -// // TODO(miferco97): px4_ros_com done -// Eigen::Affine3d transformation(q_baselink.inverse()); -// Eigen::Vector3d vel_flu = transformation * vel_enu; -// // Eigen::Vector3d vel_flu = -// // px4_ros_com::frame_transforms::transform_frame(vel_enu, q_baselink.inverse()); - -// odom_msg.twist.twist.linear.x = vel_flu[0]; -// odom_msg.twist.twist.linear.y = vel_flu[1]; -// odom_msg.twist.twist.linear.z = vel_flu[2]; -// break; -// } -// case px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_BODY_FRD: { -// // FRD --> FLU -// odom_msg.twist.twist.linear.x = msg->velocity[0]; -// odom_msg.twist.twist.linear.y = -msg->velocity[1]; -// odom_msg.twist.twist.linear.z = -msg->velocity[2]; -// break; -// } -// default: -// RCLCPP_ERROR(this->get_logger(), "Vehicle Odometry velocity frame not supported."); -// break; -// } - -// // Angular rate in body-fixed frame (rad/s): FRD --> FLU -// odom_msg.twist.twist.angular.x = msg->angular_velocity[0]; -// odom_msg.twist.twist.angular.y = -msg->angular_velocity[1]; -// odom_msg.twist.twist.angular.z = -msg->angular_velocity[2]; - -// odometry_raw_estimation_ptr_->updateData(odom_msg); -// } - -// void MavlinkPlatform::px4VehicleControlModeCallback( -// const px4_msgs::msg::VehicleControlMode::SharedPtr msg) -// { -// static bool last_arm_state = false; -// static bool last_offboard_state = false; - -// this->platform_info_msg_.armed = msg->flag_armed; -// this->platform_info_msg_.offboard = msg->flag_control_offboard_enabled; - -// if (this->platform_info_msg_.offboard != last_offboard_state) { -// if (this->platform_info_msg_.offboard) { -// RCLCPP_INFO(this->get_logger(), "OFFBOARD_ENABLED"); -// } else { -// RCLCPP_INFO(this->get_logger(), "OFFBOARD_DISABLED"); -// manual_from_operator_ = true; -// } -// last_offboard_state = this->platform_info_msg_.offboard; -// } - -// if (this->platform_info_msg_.armed != last_arm_state) { -// if (this->platform_info_msg_.armed) { -// RCLCPP_INFO(this->get_logger(), "ARMING"); -// this->handleStateMachineEvent(as2_msgs::msg::PlatformStateMachineEvent::ARM); -// } else { -// RCLCPP_INFO(this->get_logger(), "DISARMING"); -// this->handleStateMachineEvent(as2_msgs::msg::PlatformStateMachineEvent::DISARM); -// } -// last_arm_state = this->platform_info_msg_.armed; -// } -// } - -// void MavlinkPlatform::px4GpsCallback(const px4_msgs::msg::SensorGps::SharedPtr msg) -// { -// // Reference: -// // https://github.com/mavlink_/mavlink_-Autopilot/blob/052adfbfd977abfad5b6f58d5404bba7dd209736/src/modules/mavlink/streams/GPS_RAW_INT.hpp#L58 -// // https://github.com/mavlink/mavros/blob/b392c23add8781a67ac90915278fe41086fecaeb/mavros/src/plugins/global_position.cpp#L161 - -// auto timestamp = this->get_clock()->now(); - -// sensor_msgs::msg::NavSatFix nav_sat_fix_msg; -// nav_sat_fix_msg.header.stamp = timestamp; - -// nav_sat_fix_msg.header.frame_id = "wgs84"; -// if (msg->fix_type > 2) { // At least 3D position -// nav_sat_fix_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; -// } else { -// nav_sat_fix_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; -// } -// nav_sat_fix_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; // DEFAULT - -// nav_sat_fix_msg.latitude = msg->lat; -// nav_sat_fix_msg.longitude = msg->lon; -// nav_sat_fix_msg.altitude = msg->alt_ellipsoid; - -// if (!std::isnan(msg->eph) && !std::isnan(msg->epv)) { -// // Position uncertainty --> Diagonal known -// nav_sat_fix_msg.position_covariance.fill(0.0); -// nav_sat_fix_msg.position_covariance[0] = std::pow(msg->eph, 2); -// nav_sat_fix_msg.position_covariance[4] = std::pow(msg->eph, 2); -// nav_sat_fix_msg.position_covariance[8] = std::pow(msg->epv, 2); -// nav_sat_fix_msg.position_covariance_type = -// sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; -// } else { -// // UNKOWN -// nav_sat_fix_msg.position_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; -// nav_sat_fix_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; -// } -// nav_sat_fix_msg.latitude = nav_sat_fix_msg.latitude / 1e7; -// nav_sat_fix_msg.longitude = nav_sat_fix_msg.longitude / 1e7; -// nav_sat_fix_msg.altitude = nav_sat_fix_msg.altitude / 1e3; - -// gps_sensor_ptr_->updateData(nav_sat_fix_msg); -// } +bool MavlinkPlatform::mavlink_callOffboardControlMode() +{ + auto request = std::make_shared(); + auto response = std::make_shared(); + request->custom_mode = "OFFBOARD"; + auto result = mavlink_set_mode_client_->sendRequest(request, response); + if (!result) { + RCLCPP_ERROR(this->get_logger(), "Failed to set OFFBOARD mode"); + return false; + } + return response->mode_sent; +} -// void MavlinkPlatform::px4BatteryCallback(const px4_msgs::msg::BatteryStatus::SharedPtr msg) -// { -// auto timestamp = this->get_clock()->now(); - -// sensor_msgs::msg::BatteryState battery_msg; -// battery_msg.header.stamp = timestamp; - -// battery_msg.voltage = msg->voltage_v; -// battery_msg.temperature = msg->temperature; -// battery_msg.current = msg->current_a; -// battery_msg.charge = NAN; -// battery_msg.capacity = msg->capacity; -// battery_msg.design_capacity = msg->design_capacity; -// battery_msg.percentage = 100.0 * msg->remaining; -// // TODO(miferco97): config file with battery settings -// battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; -// battery_msg.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; -// battery_msg.power_supply_technology = -// sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN; -// battery_msg.present = msg->connected; -// // battery_msg.cell_voltage = msg->voltage_cell_v; -// battery_msg.cell_voltage = {}; -// battery_msg.cell_temperature = {}; -// battery_msg.location = '0'; -// battery_msg.serial_number = std::to_string(msg->serial_number); - -// if (msg->warning >= 0) { -// RCLCPP_WARN_ONCE(this->get_logger(), "Battery warning #%d", msg->warning); -// } +void MavlinkPlatform::mavlink_publishRatesSetpoint( + double droll, double dpitch, double dyaw, + double dthrust) +{ + auto msg = mavros_msgs::msg::AttitudeTarget(); + msg.header.stamp = this->get_clock()->now(); + msg.type_mask = mavros_msgs::msg::AttitudeTarget::IGNORE_ATTITUDE; + msg.body_rate.x = droll; + msg.body_rate.y = dpitch; + msg.body_rate.z = dyaw; + msg.thrust = dthrust; + mavlink_acro_setpoint_pub_->publish(msg); +} -// // if (msg->faults >= 0) { -// // RCLCPP_ERROR_ONCE(this->get_logger(), "Battery error bitmask: %d", -// // msg->faults); -// // } +void MavlinkPlatform::mavlink_publishAttitudeSetpoint( + const geometry_msgs::msg::Quaternion & q, + double thrust) +{ + auto msg = mavros_msgs::msg::AttitudeTarget(); + msg.header.stamp = this->get_clock()->now(); + msg.type_mask = mavros_msgs::msg::AttitudeTarget::IGNORE_PITCH_RATE | + mavros_msgs::msg::AttitudeTarget::IGNORE_ROLL_RATE | + mavros_msgs::msg::AttitudeTarget::IGNORE_YAW_RATE; + msg.orientation = q; + msg.thrust = thrust; + mavlink_acro_setpoint_pub_->publish(msg); +} -// battery_sensor_ptr_->updateData(battery_msg); -// } -// bool MavlinkPlatform::getFlagSimulationMode() -// { -// // TODO(miferco97): check if this is better than creating a variable to store the value -// return this->get_parameter("use_sim_time").as_bool(); -// } +void MavlinkPlatform::mavlink_publishTwistSetpoint(const geometry_msgs::msg::TwistStamped & msg) +{ + mavlink_twist_setpoint_pub_->publish(msg); +} +void MavlinkPlatform::mavlink_publishPoseSetpoint(const geometry_msgs::msg::PoseStamped & msg) +{ + mavlink_pose_pub_->publish(msg); +} -} // namespace as2_platform_pixhawk +} // namespace as2_platform_mavlink diff --git a/tests/pixhawk_platform_test.cpp b/tests/pixhawk_platform_test.cpp index bd7bccb..5632fb5 100644 --- a/tests/pixhawk_platform_test.cpp +++ b/tests/pixhawk_platform_test.cpp @@ -110,7 +110,7 @@ void spinForTime( int platform_test() { - std::string ros_namespace = "test_mavlink_platform"; + std::string ros_namespace = "drone0"; auto test_node = std::make_shared(ros_namespace); auto tello_node = get_node(ros_namespace); diff --git a/tests/px4_odom_test.cpp b/tests/px4_odom_test__.cpp similarity index 100% rename from tests/px4_odom_test.cpp rename to tests/px4_odom_test__.cpp