diff --git a/.github/workflows/ros_build_test.yml b/.github/workflows/ros_build_test.yml index 28768ce..11cf2b3 100644 --- a/.github/workflows/ros_build_test.yml +++ b/.github/workflows/ros_build_test.yml @@ -3,13 +3,7 @@ name: ros_build_test on: push: - branches: [ devel ] - - paths-ignore: - - '**/README.md' - - pull_request: - branches: [ master ] + pull_request: workflow_dispatch: @@ -17,9 +11,88 @@ concurrency: group: ${{ github.ref }} cancel-in-progress: true + jobs: - build: - uses: ctu-mrs/ci_scripts/.github/workflows/ros_build_test.yml@master - secrets: - PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }} + build_amd64: + runs-on: ubuntu-20.04 + + steps: + + - uses: actions/checkout@v3 + with: + fetch-depth: 0 + submodules: 'recursive' + + - name: Checkout CI scripts + uses: actions/checkout@v3 + with: + repository: ctu-mrs/ci_scripts + ref: master + path: .ci_scripts + + - name: Update submodules + run: | + sudo pip3 install -U gitman + [[ -e .gitman.yml || -e .gitman.yaml ]] && [[ ! -e .gitman_ignore ]] && gitman install || echo "no gitman modules to install" + + - name: Build + run: | + mkdir -p /tmp/artifacts + .ci_scripts/package_build/build_package.sh $GITHUB_WORKSPACE /tmp/artifacts + + build_arm64: + + runs-on: ubuntu-20.04 + + steps: + - uses: actions/checkout@v3 + with: + fetch-depth: 0 + submodules: 'recursive' + + - name: Checkout CI scripts + uses: actions/checkout@v3 + with: + repository: ctu-mrs/ci_scripts + ref: master + path: .ci_scripts + + - name: Update submodules + run: | + sudo pip3 install -U gitman + [[ -e .gitman.yml || -e .gitman.yaml ]] && [[ ! -e .gitman_ignore ]] && gitman install || echo "no gitman modules to install" + + - uses: ctu-mrs/run-on-arch-action@master + name: Build artifact + + id: build + + with: + arch: aarch64 + distro: noetic + + githubToken: ${{ github.token }} + + # Create an artifacts directory + setup: | + mkdir -p "/tmp/artifacts" + mkdir -p "/tmp/repository" + + # Mount the artifacts directory as /artifacts in the container + dockerRunArgs: | + --volume "$PWD:/tmp/repository" + --volume "/tmp/artifacts:/tmp/artifacts" + + # The shell to run commands with in the container + shell: /bin/sh + + install: | + apt-get update -q -y + apt-get upgrade -q -y + + # Produce a binary artifact and place it in the mounted volume + run: | + git config --global --add safe.directory "*" + /tmp/repository/.ci_scripts/package_build/build_package.sh /tmp/repository /tmp/artifacts + diff --git a/.github/workflows/ros_package_build.yml b/.github/workflows/ros_package_build.yml deleted file mode 100644 index d17da2a..0000000 --- a/.github/workflows/ros_package_build.yml +++ /dev/null @@ -1,22 +0,0 @@ -name: ros_package_build - -on: - - push: - branches: [ master ] - - paths-ignore: - - '**/README.md' - - workflow_dispatch: - -concurrency: - group: ${{ github.ref }} - cancel-in-progress: true - -jobs: - - build: - uses: ctu-mrs/ci_scripts/.github/workflows/ros_package_build.yml@master - secrets: - PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }} diff --git a/CMakeLists.txt b/CMakeLists.txt index dbcb11f..2a7c86b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(mrs_uav_px4_api) +project(mrs_usv_ardurover_api) set(CATKIN_DEPENDENCIES cmake_modules @@ -26,7 +26,7 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(LIBRARIES - MrsUavPx4Api_Api + MrsUsvArduroverApi_Api ) catkin_package( @@ -40,16 +40,16 @@ include_directories( # Plugin -add_library(MrsUavPx4Api_Api +add_library(MrsUsvArduroverApi_Api src/api.cpp ) -add_dependencies(MrsUavPx4Api_Api +add_dependencies(MrsUsvArduroverApi_Api ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(MrsUavPx4Api_Api +target_link_libraries(MrsUsvArduroverApi_Api ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ) diff --git a/README.md b/README.md index 876710b..8450cfa 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ -# MRS UAV PX4 API Plugin +# MRS USV ArduPilot Rover API Plugin -[MRS HW Api](https://github.com/ctu-mrs/mrs_uav_hw_api) plugin for interfacing the [UAV Core](https://github.com/ctu-mrs/mrs_uav_core) with the PX4 autopilot. +[MRS HW Api](https://github.com/ctu-mrs/mrs_uav_hw_api) plugin for interfacing the [UAV Core](https://github.com/ctu-mrs/mrs_uav_core) with the ArduPilot Rover for BlueBoat. > :warning: **Attention please: This README is outdated.** > diff --git a/config/px4_api.yaml b/config/ardurover_api.yaml similarity index 67% rename from config/px4_api.yaml rename to config/ardurover_api.yaml index 6c527ad..e147004 100644 --- a/config/px4_api.yaml +++ b/config/ardurover_api.yaml @@ -1,6 +1,8 @@ -hw_interface_plugin: "mrs_uav_px4_api/Api" +hw_interface_plugin: "mrs_usv_ardurover_api/Api" -mavros_timeout: 1.0 # [s] +mavros_timeout: 5.0 # [s] + +position_cmd_map: true simulated_rtk: utm_zone: "32T" @@ -9,19 +11,21 @@ simulated_rtk: amsl: 340.0 inputs: + position: true control_group: false - attitude_rate: true - attitude: true + attitude_rate: false + attitude: false + velocity_hdg_rate: true outputs: - distance_sensor: true + distance_sensor: false gnss: true rtk: true imu: true altitude: true magnetometer_heading: true magnetic_field: true - rc_channels: true + rc_channels: false battery_state: true position: true orientation: true diff --git a/launch/api.launch b/launch/api.launch index 42ad3cc..a51cc53 100644 --- a/launch/api.launch +++ b/launch/api.launch @@ -4,7 +4,6 @@ - @@ -26,7 +25,7 @@ - + @@ -36,23 +35,37 @@ + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 778128f..f283d34 100644 --- a/package.xml +++ b/package.xml @@ -1,12 +1,12 @@ - mrs_uav_px4_api + mrs_usv_ardurover_api 1.0.0 - PX4 API for MRS UAV System + ArduPilot Rover API for MRS UAV System - Tomas Baca - Tomas Baca + Filip Novak + Filip Novak BSD 3-Clause diff --git a/plugins.xml b/plugins.xml index 6ae1203..0fbe6d9 100644 --- a/plugins.xml +++ b/plugins.xml @@ -1,5 +1,5 @@ - - - PX4 API module for the MRS UAV System + + + ArduPilot Rover API module for the MRS UAV System diff --git a/src/api.cpp b/src/api.cpp index 6658049..c74e816 100644 --- a/src/api.cpp +++ b/src/api.cpp @@ -17,10 +17,13 @@ #include #include #include +#include #include +#include #include +#include #include #include @@ -29,6 +32,7 @@ #include #include #include +#include //} @@ -42,15 +46,15 @@ //} -namespace mrs_uav_px4_api +namespace mrs_usv_ardurover_api { -/* class MrsUavPx4Api //{ */ +/* class MrsUsvArduroverApi //{ */ -class MrsUavPx4Api : public mrs_uav_hw_api::MrsUavHwApi { +class MrsUsvArduroverApi : public mrs_uav_hw_api::MrsUavHwApi { public: - ~MrsUavPx4Api(){}; + ~MrsUsvArduroverApi(){}; void initialize(const ros::NodeHandle& parent_nh, std::shared_ptr common_handlers); @@ -150,6 +154,13 @@ class MrsUavPx4Api : public mrs_uav_hw_api::MrsUavHwApi { mrs_lib::PublisherHandler ph_mavros_attitude_target_; mrs_lib::PublisherHandler ph_mavros_actuator_control_; + mrs_lib::PublisherHandler ph_mavros_position_target_; + mrs_lib::PublisherHandler ph_mavros_velocity_target_; + + mrs_lib::PublisherHandler pub_left_usv_thrust_cmd_; + mrs_lib::PublisherHandler pub_right_usv_thrust_cmd_; + mrs_lib::PublisherHandler pub_left_usv_thrust_angle_; + mrs_lib::PublisherHandler pub_right_usv_thrust_angle_; // | ------------------------ variables ----------------------- | @@ -158,6 +169,12 @@ class MrsUavPx4Api : public mrs_uav_hw_api::MrsUavHwApi { std::atomic armed_ = false; std::atomic connected_ = false; std::mutex mutex_status_; + + mrs_lib::Transformer transformer_; + std::mutex transformer_mutex; + + bool position_cmd_map = false; + std::string node_name = "MrsUsvArduroverApi"; }; //} @@ -168,7 +185,7 @@ class MrsUavPx4Api : public mrs_uav_hw_api::MrsUavHwApi { /* initialize() //{ */ -void MrsUavPx4Api::initialize(const ros::NodeHandle& parent_nh, std::shared_ptr common_handlers) { +void MrsUsvArduroverApi::initialize(const ros::NodeHandle& parent_nh, std::shared_ptr common_handlers) { ros::NodeHandle nh_(parent_nh); @@ -178,7 +195,7 @@ void MrsUavPx4Api::initialize(const ros::NodeHandle& parent_nh, std::shared_ptr< _body_frame_name_ = common_handlers->getBodyFrameName(); _world_frame_name_ = common_handlers->getWorldFrameName(); - _capabilities_.api_name = "Px4Api"; + _capabilities_.api_name = "ArduroverApi"; // | ------------------- loading parameters ------------------- | @@ -196,6 +213,9 @@ void MrsUavPx4Api::initialize(const ros::NodeHandle& parent_nh, std::shared_ptr< param_loader.loadParam("inputs/control_group", (bool&)_capabilities_.accepts_control_group_cmd); param_loader.loadParam("inputs/attitude_rate", (bool&)_capabilities_.accepts_attitude_rate_cmd); param_loader.loadParam("inputs/attitude", (bool&)_capabilities_.accepts_attitude_cmd); + param_loader.loadParam("inputs/position", (bool&)_capabilities_.accepts_position_cmd); + param_loader.loadParam("inputs/velocity_hdg_rate", (bool&)_capabilities_.accepts_velocity_hdg_rate_cmd); + param_loader.loadParam("inputs/actuator", (bool&)_capabilities_.accepts_actuator_cmd); param_loader.loadParam("outputs/distance_sensor", (bool&)_capabilities_.produces_distance_sensor); param_loader.loadParam("outputs/gnss", (bool&)_capabilities_.produces_gnss); @@ -212,9 +232,11 @@ void MrsUavPx4Api::initialize(const ros::NodeHandle& parent_nh, std::shared_ptr< param_loader.loadParam("outputs/velocity", (bool&)_capabilities_.produces_velocity); param_loader.loadParam("outputs/angular_velocity", (bool&)_capabilities_.produces_angular_velocity); param_loader.loadParam("outputs/odometry", (bool&)_capabilities_.produces_odometry); + + param_loader.loadParam("position_cmd_map", (bool&)position_cmd_map); if (!param_loader.loadedSuccessfully()) { - ROS_ERROR("[MrsUavPx4Api]: Could not load all parameters!"); + ROS_ERROR("[MrsUsvArduroverApi]: Could not load all parameters!"); ros::shutdown(); } @@ -227,7 +249,7 @@ void MrsUavPx4Api::initialize(const ros::NodeHandle& parent_nh, std::shared_ptr< mrs_lib::SubscribeHandlerOptions shopts; shopts.nh = nh_; - shopts.node_name = "MrsHwPx4Api"; + shopts.node_name = "MrsHwArduroverApi"; shopts.no_message_timeout = mrs_lib::no_timeout; shopts.threadsafe = true; shopts.autostart = true; @@ -235,43 +257,54 @@ void MrsUavPx4Api::initialize(const ros::NodeHandle& parent_nh, std::shared_ptr< shopts.transport_hints = ros::TransportHints().tcpNoDelay(); if (_simulation_) { - sh_ground_truth_ = mrs_lib::SubscribeHandler(shopts, "ground_truth_in", &MrsUavPx4Api::callbackGroundTruth, this); + sh_ground_truth_ = mrs_lib::SubscribeHandler(shopts, "ground_truth_in", &MrsUsvArduroverApi::callbackGroundTruth, this); } if (!_simulation_) { - sh_rtk_ = mrs_lib::SubscribeHandler(shopts, "rtk_in", &MrsUavPx4Api::callbackRTK, this); + sh_rtk_ = mrs_lib::SubscribeHandler(shopts, "rtk_in", &MrsUsvArduroverApi::callbackRTK, this); } - sh_mavros_state_ = mrs_lib::SubscribeHandler(shopts, "mavros_state_in", ros::Duration(0.05), &MrsUavPx4Api::timeoutMavrosState, this, - &MrsUavPx4Api::callbackMavrosState, this); + sh_mavros_state_ = mrs_lib::SubscribeHandler(shopts, "mavros_state_in", ros::Duration(0.05), &MrsUsvArduroverApi::timeoutMavrosState, this, + &MrsUsvArduroverApi::callbackMavrosState, this); - sh_mavros_odometry_local_ = mrs_lib::SubscribeHandler(shopts, "mavros_local_position_in", &MrsUavPx4Api::callbackOdometryLocal, this); + sh_mavros_odometry_local_ = mrs_lib::SubscribeHandler(shopts, "mavros_local_position_in", &MrsUsvArduroverApi::callbackOdometryLocal, this); - sh_mavros_gps_ = mrs_lib::SubscribeHandler(shopts, "mavros_global_position_in", &MrsUavPx4Api::callbackNavsatFix, this); + sh_mavros_gps_ = mrs_lib::SubscribeHandler(shopts, "mavros_global_position_in", &MrsUsvArduroverApi::callbackNavsatFix, this); - sh_mavros_distance_sensor_ = mrs_lib::SubscribeHandler(shopts, "mavros_garmin_in", &MrsUavPx4Api::callbackDistanceSensor, this); + sh_mavros_distance_sensor_ = mrs_lib::SubscribeHandler(shopts, "mavros_garmin_in", &MrsUsvArduroverApi::callbackDistanceSensor, this); - sh_mavros_imu_ = mrs_lib::SubscribeHandler(shopts, "mavros_imu_in", &MrsUavPx4Api::callbackImu, this); + sh_mavros_imu_ = mrs_lib::SubscribeHandler(shopts, "mavros_imu_in", &MrsUsvArduroverApi::callbackImu, this); - sh_mavros_magnetometer_heading_ = mrs_lib::SubscribeHandler(shopts, "mavros_magnetometer_in", &MrsUavPx4Api::callbackMagnetometer, this); + sh_mavros_magnetometer_heading_ = mrs_lib::SubscribeHandler(shopts, "mavros_magnetometer_in", &MrsUsvArduroverApi::callbackMagnetometer, this); sh_mavros_magnetic_field_ = - mrs_lib::SubscribeHandler(shopts, "mavros_magnetic_field_in", &MrsUavPx4Api::callbackMagneticField, this); + mrs_lib::SubscribeHandler(shopts, "mavros_magnetic_field_in", &MrsUsvArduroverApi::callbackMagneticField, this); - sh_mavros_rc_ = mrs_lib::SubscribeHandler(shopts, "mavros_rc_in", &MrsUavPx4Api::callbackRC, this); + sh_mavros_rc_ = mrs_lib::SubscribeHandler(shopts, "mavros_rc_in", &MrsUsvArduroverApi::callbackRC, this); - sh_mavros_altitude_ = mrs_lib::SubscribeHandler(shopts, "mavros_altitude_in", &MrsUavPx4Api::callbackAltitude, this); + sh_mavros_altitude_ = mrs_lib::SubscribeHandler(shopts, "mavros_altitude_in", &MrsUsvArduroverApi::callbackAltitude, this); - sh_mavros_battery_ = mrs_lib::SubscribeHandler(shopts, "mavros_battery_in", &MrsUavPx4Api::callbackBattery, this); + sh_mavros_battery_ = mrs_lib::SubscribeHandler(shopts, "mavros_battery_in", &MrsUsvArduroverApi::callbackBattery, this); // | ----------------------- publishers ----------------------- | ph_mavros_attitude_target_ = mrs_lib::PublisherHandler(nh_, "mavros_attitude_setpoint_out", 1); ph_mavros_actuator_control_ = mrs_lib::PublisherHandler(nh_, "mavros_actuator_control_out", 1); + ph_mavros_position_target_ = mrs_lib::PublisherHandler(nh_, "mavros_position_setpoint_out", 1); + ph_mavros_velocity_target_ = mrs_lib::PublisherHandler(nh_, "mavros_velocity_setpoint_out", 1); + + pub_left_usv_thrust_cmd_ = mrs_lib::PublisherHandler(nh_, "left_usv_thrust_cmd_out", 1); + pub_right_usv_thrust_cmd_ = mrs_lib::PublisherHandler(nh_, "right_usv_thrust_cmd_out", 1); + pub_left_usv_thrust_angle_ = mrs_lib::PublisherHandler(nh_, "left_usv_thrust_angle_out", 1); + pub_right_usv_thrust_angle_ = mrs_lib::PublisherHandler(nh_, "right_usv_thrust_angle_out", 1); + + // | ----------------------- transformer ---------------------- | + transformer_ = mrs_lib::Transformer(nh_, "UavUsvController"); + transformer_.retryLookupNewest(true); // | ----------------------- finish init ---------------------- | - ROS_INFO("[MrsUavPx4Api]: initialized"); + ROS_INFO("[MrsUsvArduroverApi]: initialized"); is_initialized_ = true; } @@ -280,7 +313,7 @@ void MrsUavPx4Api::initialize(const ros::NodeHandle& parent_nh, std::shared_ptr< /* getStatus() //{ */ -mrs_msgs::HwApiStatus MrsUavPx4Api::getStatus() { +mrs_msgs::HwApiStatus MrsUsvArduroverApi::getStatus() { mrs_msgs::HwApiStatus status; @@ -302,7 +335,7 @@ mrs_msgs::HwApiStatus MrsUavPx4Api::getStatus() { /* getCapabilities() //{ */ -mrs_msgs::HwApiCapabilities MrsUavPx4Api::getCapabilities() { +mrs_msgs::HwApiCapabilities MrsUsvArduroverApi::getCapabilities() { _capabilities_.stamp = ros::Time::now(); @@ -313,23 +346,39 @@ mrs_msgs::HwApiCapabilities MrsUavPx4Api::getCapabilities() { /* callbackControlActuatorCmd() //{ */ -bool MrsUavPx4Api::callbackActuatorCmd([[maybe_unused]] const mrs_msgs::HwApiActuatorCmd::ConstPtr msg) { +bool MrsUsvArduroverApi::callbackActuatorCmd([[maybe_unused]] const mrs_msgs::HwApiActuatorCmd::ConstPtr msg) { - ROS_INFO_ONCE("[MrsUavPx4Api]: getting actuator cmd"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting actuator cmd"); + + if (!_capabilities_.accepts_actuator_cmd) { + ROS_ERROR("[%s]: the actuator input is not enabled in the config file", node_name.c_str()); + return false; + } + std_msgs::Float32 right_thurst_cmd, left_thurst_cmd, right_thurst_angle, left_thurst_angle; + + left_thurst_cmd.data = msg->motors[0]; + right_thurst_cmd.data = msg->motors[1]; + left_thurst_angle.data = msg->motors[2]; + right_thurst_angle.data = msg->motors[3]; + + pub_left_usv_thrust_cmd_.publish(left_thurst_cmd); + pub_right_usv_thrust_cmd_.publish(right_thurst_cmd); + pub_left_usv_thrust_angle_.publish(left_thurst_angle); + pub_right_usv_thrust_angle_.publish(right_thurst_angle); - return false; + return true; } //} /* callbackControlGroupCmd() //{ */ -bool MrsUavPx4Api::callbackControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd::ConstPtr msg) { +bool MrsUsvArduroverApi::callbackControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd::ConstPtr msg) { - ROS_INFO_ONCE("[MrsUavPx4Api]: getting control group cmd"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting control group cmd"); if (!_capabilities_.accepts_control_group_cmd) { - ROS_ERROR("[MrsUavPx4Api]: the control group input is not enabled in the config file"); + ROS_ERROR("[MrsUsvArduroverApi]: the control group input is not enabled in the config file"); return false; } @@ -352,12 +401,12 @@ bool MrsUavPx4Api::callbackControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd: /* callbackAttitudeRateCmd() //{ */ -bool MrsUavPx4Api::callbackAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd::ConstPtr msg) { +bool MrsUsvArduroverApi::callbackAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd::ConstPtr msg) { - ROS_INFO_ONCE("[MrsUavPx4Api]: getting attitude rate cmd"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting attitude rate cmd"); if (!_capabilities_.accepts_attitude_rate_cmd) { - ROS_ERROR_THROTTLE(1.0, "[MrsUavPx4Api]: attitude rate input is not enabled in the config file"); + ROS_ERROR_THROTTLE(1.0, "[MrsUsvArduroverApi]: attitude rate input is not enabled in the config file"); return false; } @@ -380,12 +429,12 @@ bool MrsUavPx4Api::callbackAttitudeRateCmd(const mrs_msgs::HwApiAttitudeRateCmd: /* callbackAttitudeCmd() //{ */ -bool MrsUavPx4Api::callbackAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd::ConstPtr msg) { +bool MrsUsvArduroverApi::callbackAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd::ConstPtr msg) { - ROS_INFO_ONCE("[MrsUavPx4Api]: getting attitude cmd"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting attitude cmd"); if (!_capabilities_.accepts_attitude_cmd) { - ROS_ERROR_THROTTLE(1.0, "[MrsUavPx4Api]: attitude input is not enabled in the config file"); + ROS_ERROR_THROTTLE(1.0, "[MrsUsvArduroverApi]: attitude input is not enabled in the config file"); return false; } @@ -412,9 +461,9 @@ bool MrsUavPx4Api::callbackAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd::ConstPt /* callbackAccelerationHdgRateCmd() //{ */ -bool MrsUavPx4Api::callbackAccelerationHdgRateCmd([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd::ConstPtr msg) { +bool MrsUsvArduroverApi::callbackAccelerationHdgRateCmd([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd::ConstPtr msg) { - ROS_INFO_ONCE("[MrsUavPx4Api]: getting acceleration+hdg rate cmd"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting acceleration+hdg rate cmd"); return false; } @@ -423,9 +472,9 @@ bool MrsUavPx4Api::callbackAccelerationHdgRateCmd([[maybe_unused]] const mrs_msg /* callbackAccelerationHdgCmd() //{ */ -bool MrsUavPx4Api::callbackAccelerationHdgCmd([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd::ConstPtr msg) { +bool MrsUsvArduroverApi::callbackAccelerationHdgCmd([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd::ConstPtr msg) { - ROS_INFO_ONCE("[MrsUavPx4Api]: getting acceleration+hdg cmd"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting acceleration+hdg cmd"); return false; } @@ -434,20 +483,37 @@ bool MrsUavPx4Api::callbackAccelerationHdgCmd([[maybe_unused]] const mrs_msgs::H /* callbackVelocityHdgRateCmd() //{ */ -bool MrsUavPx4Api::callbackVelocityHdgRateCmd([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd::ConstPtr msg) { +bool MrsUsvArduroverApi::callbackVelocityHdgRateCmd([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd::ConstPtr msg) { - ROS_INFO_ONCE("[MrsUavPx4Api]: getting velocity+hdg rate cmd"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting velocity+hdg rate cmd"); - return false; + if (!_capabilities_.accepts_velocity_hdg_rate_cmd) { + ROS_ERROR("[%s]: the velocity and heading rate input is not enabled in the config file", node_name.c_str()); + return false; + } + + // fill position target command + geometry_msgs::Twist velocity_cmd; + + velocity_cmd.linear.x = msg->velocity.x; + velocity_cmd.linear.y = 0; + velocity_cmd.linear.z = 0; + velocity_cmd.angular.x = 0; + velocity_cmd.angular.y = 0; + velocity_cmd.angular.z = msg->heading_rate; + + ph_mavros_velocity_target_.publish(velocity_cmd); + + return true; } //} /* callbackVelocityHdgCmd() //{ */ -bool MrsUavPx4Api::callbackVelocityHdgCmd([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd::ConstPtr msg) { +bool MrsUsvArduroverApi::callbackVelocityHdgCmd([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd::ConstPtr msg) { - ROS_INFO_ONCE("[MrsUavPx4Api]: getting velocity+hdg cmd"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting velocity+hdg cmd"); return false; } @@ -456,25 +522,92 @@ bool MrsUavPx4Api::callbackVelocityHdgCmd([[maybe_unused]] const mrs_msgs::HwApi /* callbackPositionCmd() //{ */ -bool MrsUavPx4Api::callbackPositionCmd([[maybe_unused]] const mrs_msgs::HwApiPositionCmd::ConstPtr msg) { +bool MrsUsvArduroverApi::callbackPositionCmd([[maybe_unused]] const mrs_msgs::HwApiPositionCmd::ConstPtr msg) { - ROS_INFO_ONCE("[MrsUavPx4Api]: getting position cmd"); + ROS_INFO_ONCE("[%s]: getting position cmd", node_name.c_str()); + + ROS_INFO("[%s]: getting position cmd", node_name.c_str()); + + if (!_capabilities_.accepts_position_cmd) { + ROS_ERROR("[%s]: the position input is not enabled in the config file", node_name.c_str()); + return false; + } + + // get transformation + mavros_msgs::PositionTarget position_target; + std::string output_frame = "map"; + if(position_cmd_map) + { + output_frame = "map"; + position_target.coordinate_frame = position_target.FRAME_LOCAL_NED; + } else{ + output_frame = _uav_name_ + "/fcu"; + position_target.coordinate_frame = position_target.FRAME_BODY_NED; + } - return false; + std::optional tf_opt; + { + std::scoped_lock lock(transformer_mutex); + tf_opt = transformer_.getTransform(msg->header.frame_id, output_frame); + } + if (!tf_opt) + { + ROS_ERROR("[%s]: Could not obtain transform from %s to %s", node_name.c_str(), msg->header.frame_id.c_str(), output_frame.c_str()); + return false; + } + + // transform + std::optional pose_transformed; + geometry_msgs::PoseWithCovarianceStamped pose_to_transform; + pose_to_transform.header = msg->header; + pose_to_transform.pose.pose.position = msg->position; + pose_to_transform.pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, msg->heading); + { + std::scoped_lock lock(transformer_mutex); + pose_transformed = transformer_.transform(pose_to_transform, tf_opt.value()); + } + if (!pose_transformed) + { + ROS_INFO("[%s]: Failed to transform pose, returning.", node_name.c_str()); + return false; + } + + // get target heading + mrs_lib::AttitudeConverter attitude_converter(pose_transformed.value().pose.pose.orientation); + double position_target_yaw = attitude_converter.getHeading(); + + // fill position target command + position_target.header.stamp = ros::Time::now(); + position_target.coordinate_frame = position_target.FRAME_LOCAL_NED; + + position_target.type_mask = 0; + position_target.type_mask += position_target.IGNORE_VX+position_target.IGNORE_VY+position_target.IGNORE_VZ; + position_target.type_mask += position_target.IGNORE_AFX+position_target.IGNORE_AFY+position_target.IGNORE_AFZ; + position_target.type_mask += position_target.IGNORE_YAW_RATE; + position_target.type_mask += position_target.IGNORE_YAW; + + position_target.position.x = pose_transformed.value().pose.pose.position.x; + position_target.position.y = pose_transformed.value().pose.pose.position.y; + position_target.position.z = pose_transformed.value().pose.pose.position.z; + position_target.yaw = position_target_yaw; + + ph_mavros_position_target_.publish(position_target); + + return true; } //} /* callbackTrackerCmd() //{ */ -void MrsUavPx4Api::callbackTrackerCmd([[maybe_unused]] const mrs_msgs::TrackerCommand::ConstPtr msg) { +void MrsUsvArduroverApi::callbackTrackerCmd([[maybe_unused]] const mrs_msgs::TrackerCommand::ConstPtr msg) { } //} /* callbackArming() //{ */ -std::tuple MrsUavPx4Api::callbackArming([[maybe_unused]] const bool& request) { +std::tuple MrsUsvArduroverApi::callbackArming([[maybe_unused]] const bool& request) { std::stringstream ss; @@ -492,23 +625,25 @@ std::tuple MrsUavPx4Api::callbackArming([[maybe_unused]] cons srv_out.request.param6 = 0; srv_out.request.param7 = 0; - ROS_INFO("[Px4Api]: calling for %s", request ? "arming" : "disarming"); + /* ss << "calling for " << (request ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.result << "'"; */ + /* ROS_INFO_STREAM_THROTTLE(1.0, "[" << node_name.c_str() << "]: " << ss.str()); */ + /* return {true, ss.str()}; */ if (sch_mavros_command_long_.call(srv_out)) { if (srv_out.response.success) { ss << "service call for " << (request ? "arming" : "disarming") << " was successful"; - ROS_INFO_STREAM_THROTTLE(1.0, "[Px4Api]: " << ss.str()); + ROS_INFO_STREAM_THROTTLE(1.0, "[" << "]: " << ss.str()); } else { ss << "service call for " << (request ? "arming" : "disarming") << " failed"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[Px4Api]: " << ss.str()); + ROS_ERROR_STREAM_THROTTLE(1.0, "[" << node_name.c_str() << "]: " << ss.str()); } } else { ss << "calling for " << (request ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.result << "'"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[Px4Api]: " << ss.str()); + ROS_ERROR_STREAM_THROTTLE(1.0, "[" << node_name.c_str() << "]: " << ss.str()); } return {srv_out.response.success, ss.str()}; @@ -518,12 +653,19 @@ std::tuple MrsUavPx4Api::callbackArming([[maybe_unused]] cons /* callbackOffboard() //{ */ -std::tuple MrsUavPx4Api::callbackOffboard(void) { +std::tuple MrsUsvArduroverApi::callbackOffboard(void) { + + /* ROS_INFO("[%s]: callback offboard", node_name.c_str()); */ + + /* std::stringstream ss; */ + /* ss << "switched to offboard mode"; */ + + /* return {true, ss.str()}; */ mavros_msgs::SetMode srv; srv.request.base_mode = 0; - srv.request.custom_mode = "OFFBOARD"; + srv.request.custom_mode = "GUIDED"; bool res = sch_mavros_mode_.call(srv); @@ -533,7 +675,7 @@ std::tuple MrsUavPx4Api::callbackOffboard(void) { ss << "Service call for offboard failed!"; - ROS_ERROR_THROTTLE(1.0, "[Px4Api]: %s", ss.str().c_str()); + ROS_ERROR_THROTTLE(1.0, "[%s]: %s", node_name.c_str(), ss.str().c_str()); return {false, ss.str()}; } else { @@ -542,7 +684,7 @@ std::tuple MrsUavPx4Api::callbackOffboard(void) { ss << "service call for offboard failed, returned " << srv.response.mode_sent; - ROS_WARN_THROTTLE(1.0, "[Px4Api]: %s", ss.str().c_str()); + ROS_WARN_THROTTLE(1.0, "[%s]: %s", node_name.c_str(), ss.str().c_str()); return {false, ss.str()}; @@ -561,7 +703,7 @@ std::tuple MrsUavPx4Api::callbackOffboard(void) { /* timeoutMavrosState() //{ */ -void MrsUavPx4Api::timeoutMavrosState([[maybe_unused]] const std::string& topic, const ros::Time& last_msg) { +void MrsUsvArduroverApi::timeoutMavrosState([[maybe_unused]] const std::string& topic, const ros::Time& last_msg) { if (!is_initialized_) { return; @@ -584,14 +726,14 @@ void MrsUavPx4Api::timeoutMavrosState([[maybe_unused]] const std::string& topic, mode_ = ""; } - ROS_WARN_THROTTLE(1.0, "[MrsUavPx4Api]: Have not received Mavros state for more than '%.3f s'", time.toSec()); + ROS_WARN_THROTTLE(1.0, "[MrsUsvArduroverApi]: Have not received Mavros state for more than '%.3f s'", time.toSec()); } else { - ROS_WARN_THROTTLE(1.0, "[MrsUavPx4Api]: Not recieving Mavros state message for '%.3f s'! Setup the PixHawk SD card!!", time.toSec()); - ROS_WARN_THROTTLE(1.0, "[MrsUavPx4Api]: This could be also caused by the not being PixHawk booted properly due to, e.g., antispark connector jerkyness."); - ROS_WARN_THROTTLE(1.0, "[MrsUavPx4Api]: The Mavros state should be supplied at 100 Hz to provided fast refresh rate on the state of the OFFBOARD mode."); - ROS_WARN_THROTTLE(1.0, "[MrsUavPx4Api]: If missing, the UAV could be disarmed by safety routines while not knowing it has switched to the MANUAL mode."); + ROS_WARN_THROTTLE(1.0, "[MrsUsvArduroverApi]: Not recieving Mavros state message for '%.3f s'! Setup the PixHawk SD card!!", time.toSec()); + ROS_WARN_THROTTLE(1.0, "[MrsUsvArduroverApi]: This could be also caused by the not being PixHawk booted properly due to, e.g., antispark connector jerkyness."); + ROS_WARN_THROTTLE(1.0, "[MrsUsvArduroverApi]: The Mavros state should be supplied at 100 Hz to provided fast refresh rate on the state of the OFFBOARD mode."); + ROS_WARN_THROTTLE(1.0, "[MrsUsvArduroverApi]: If missing, the UAV could be disarmed by safety routines while not knowing it has switched to the MANUAL mode."); } } @@ -599,7 +741,7 @@ void MrsUavPx4Api::timeoutMavrosState([[maybe_unused]] const std::string& topic, /* RCChannelToRange() //{ */ -double MrsUavPx4Api::RCChannelToRange(const double& rc_value) { +double MrsUsvArduroverApi::RCChannelToRange(const double& rc_value) { double tmp_0_to_1 = (rc_value - double(PWM_MIN)) / (double(PWM_RANGE)); @@ -618,18 +760,19 @@ double MrsUavPx4Api::RCChannelToRange(const double& rc_value) { /* //{ callbackMavrosState() */ -void MrsUavPx4Api::callbackMavrosState(const mavros_msgs::State::ConstPtr msg) { +void MrsUsvArduroverApi::callbackMavrosState(const mavros_msgs::State::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting Mavros state"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting Mavros state"); { std::scoped_lock lock(mutex_status_); - offboard_ = msg->mode == "OFFBOARD"; + offboard_ = msg->mode == "GUIDED"; + /* offboard_ = msg->mode == "OFFBOARD"; */ armed_ = msg->armed; connected_ = true; mode_ = msg->mode; @@ -656,13 +799,13 @@ void MrsUavPx4Api::callbackMavrosState(const mavros_msgs::State::ConstPtr msg) { /* callbackOdometryLocal() //{ */ -void MrsUavPx4Api::callbackOdometryLocal(const nav_msgs::Odometry::ConstPtr msg) { +void MrsUsvArduroverApi::callbackOdometryLocal(const nav_msgs::Odometry::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting Mavros's local odometry"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting Mavros's local odometry"); auto odom = msg; @@ -729,13 +872,13 @@ void MrsUavPx4Api::callbackOdometryLocal(const nav_msgs::Odometry::ConstPtr msg) /* callbackNavsatFix() //{ */ -void MrsUavPx4Api::callbackNavsatFix(const sensor_msgs::NavSatFix::ConstPtr msg) { +void MrsUsvArduroverApi::callbackNavsatFix(const sensor_msgs::NavSatFix::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting NavSat fix"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting NavSat fix"); if (_capabilities_.produces_gnss) { @@ -747,13 +890,13 @@ void MrsUavPx4Api::callbackNavsatFix(const sensor_msgs::NavSatFix::ConstPtr msg) /* callbackDistanceSensor() //{ */ -void MrsUavPx4Api::callbackDistanceSensor(const sensor_msgs::Range::ConstPtr msg) { +void MrsUsvArduroverApi::callbackDistanceSensor(const sensor_msgs::Range::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting distnace sensor"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting distnace sensor"); if (_capabilities_.produces_distance_sensor) { @@ -765,13 +908,13 @@ void MrsUavPx4Api::callbackDistanceSensor(const sensor_msgs::Range::ConstPtr msg /* callbackImu() //{ */ -void MrsUavPx4Api::callbackImu(const sensor_msgs::Imu::ConstPtr msg) { +void MrsUsvArduroverApi::callbackImu(const sensor_msgs::Imu::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting IMU"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting IMU"); if (_capabilities_.produces_imu) { @@ -786,13 +929,13 @@ void MrsUavPx4Api::callbackImu(const sensor_msgs::Imu::ConstPtr msg) { /* callbackCompass() //{ */ -void MrsUavPx4Api::callbackMagnetometer(const std_msgs::Float64::ConstPtr msg) { +void MrsUsvArduroverApi::callbackMagnetometer(const std_msgs::Float64::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting magnetometer heading"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting magnetometer heading"); if (_capabilities_.produces_magnetometer_heading) { @@ -809,13 +952,13 @@ void MrsUavPx4Api::callbackMagnetometer(const std_msgs::Float64::ConstPtr msg) { /* callbackMagneticField() //{ */ -void MrsUavPx4Api::callbackMagneticField(const sensor_msgs::MagneticField::ConstPtr msg) { +void MrsUsvArduroverApi::callbackMagneticField(const sensor_msgs::MagneticField::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting magnetic field"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting magnetic field"); if (_capabilities_.produces_magnetic_field) { @@ -827,13 +970,13 @@ void MrsUavPx4Api::callbackMagneticField(const sensor_msgs::MagneticField::Const /* callbackRC() //{ */ -void MrsUavPx4Api::callbackRC(const mavros_msgs::RCIn::ConstPtr msg) { +void MrsUsvArduroverApi::callbackRC(const mavros_msgs::RCIn::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting RC"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting RC"); if (_capabilities_.produces_rc_channels) { @@ -853,13 +996,13 @@ void MrsUavPx4Api::callbackRC(const mavros_msgs::RCIn::ConstPtr msg) { /* callbackAltitude() //{ */ -void MrsUavPx4Api::callbackAltitude(const mavros_msgs::Altitude::ConstPtr msg) { +void MrsUsvArduroverApi::callbackAltitude(const mavros_msgs::Altitude::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting Altitude"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting Altitude"); if (_capabilities_.produces_altitude) { @@ -876,13 +1019,13 @@ void MrsUavPx4Api::callbackAltitude(const mavros_msgs::Altitude::ConstPtr msg) { /* callbackBattery() //{ */ -void MrsUavPx4Api::callbackBattery(const sensor_msgs::BatteryState::ConstPtr msg) { +void MrsUsvArduroverApi::callbackBattery(const sensor_msgs::BatteryState::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting battery"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting battery"); if (_capabilities_.produces_battery_state) { @@ -894,13 +1037,13 @@ void MrsUavPx4Api::callbackBattery(const sensor_msgs::BatteryState::ConstPtr msg /* callbackGroundTruth() //{ */ -void MrsUavPx4Api::callbackGroundTruth(const nav_msgs::Odometry::ConstPtr msg) { +void MrsUsvArduroverApi::callbackGroundTruth(const nav_msgs::Odometry::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting ground truth"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting ground truth"); auto odom = msg; @@ -913,7 +1056,7 @@ void MrsUavPx4Api::callbackGroundTruth(const nav_msgs::Odometry::ConstPtr msg) { // if frame_id is "/world", "world", "/map" or "map" gazebo reports velocitites in global world frame so we need to transform them to body frame if (msg->header.frame_id == "/world" || msg->header.frame_id == "world" || msg->header.frame_id == "/map" || msg->header.frame_id == "map") { - ROS_INFO_ONCE("[MrsUavPx4Api]: transforming Gazebo ground truth velocities from world to body frame"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: transforming Gazebo ground truth velocities from world to body frame"); Eigen::Matrix3d R = mrs_lib::AttitudeConverter(msg->pose.pose.orientation); @@ -974,13 +1117,13 @@ void MrsUavPx4Api::callbackGroundTruth(const nav_msgs::Odometry::ConstPtr msg) { /* callbackRTK() //{ */ -void MrsUavPx4Api::callbackRTK(const mrs_modules_msgs::Bestpos::ConstPtr msg) { +void MrsUsvArduroverApi::callbackRTK(const mrs_modules_msgs::Bestpos::ConstPtr msg) { if (!is_initialized_) { return; } - ROS_INFO_ONCE("[MrsUavPx4Api]: getting rtk"); + ROS_INFO_ONCE("[MrsUsvArduroverApi]: getting rtk"); mrs_msgs::RtkGps rtk_msg_out; @@ -1020,4 +1163,4 @@ void MrsUavPx4Api::callbackRTK(const mrs_modules_msgs::Bestpos::ConstPtr msg) { } // namespace mrs_uav_px4_api #include -PLUGINLIB_EXPORT_CLASS(mrs_uav_px4_api::MrsUavPx4Api, mrs_uav_hw_api::MrsUavHwApi) +PLUGINLIB_EXPORT_CLASS(mrs_usv_ardurover_api::MrsUsvArduroverApi, mrs_uav_hw_api::MrsUavHwApi)