<commit_before>//----------------------------------------------------------------------------------------------------------------------
// GRVC UAL
//----------------------------------------------------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2016 GRVC University of Seville
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
// Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//----------------------------------------------------------------------------------------------------------------------

#include <string>
#include <chrono>
#include <uav_abstraction_layer/backend_mavros.h>
#include <uav_abstraction_layer/tinyxml2.h>  // TODO: move?
#include <argument_parser/argument_parser.h>
#include <Eigen/Eigen>
#include <ros/ros.h>
#include <ros/package.h>

using namespace tinyxml2;

namespace grvc { namespace ual {

BackendMavros::BackendMavros(int _argc, char** _argv)
    : args_(_argc, _argv)
{
    ROS_INFO("BackendMavros constructor");

    // Parse arguments
    robot_id_ = args_.getArgument("uav_id", 1);

    // Init ros communications
    std::string node_name = "backend_" + std::to_string(robot_id_);
    ros::init(_argc, _argv, node_name);
    nh_ = new ros::NodeHandle();

    std::string mavros_ns = "mavros_" + std::to_string(robot_id_);
    std::string set_mode_srv = mavros_ns + "/set_mode";
    std::string arming_srv = mavros_ns + "/cmd/arming";
    std::string set_pose_topic = mavros_ns + "/setpoint_position/local";
    std::string set_vel_topic = mavros_ns + "/setpoint_velocity/cmd_vel";
    std::string pose_topic = mavros_ns + "/local_position/pose";
    std::string state_topic = mavros_ns + "/state";

    flight_mode_client_ = nh_->serviceClient<mavros_msgs::SetMode>(set_mode_srv.c_str());
    arming_client_ = nh_->serviceClient<mavros_msgs::CommandBool>(arming_srv.c_str());

    mavros_ref_pose_pub_ = nh_->advertise<geometry_msgs::PoseStamped>(set_pose_topic.c_str(), 10);
    mavros_ref_vel_pub_ = nh_->advertise<geometry_msgs::TwistStamped>(set_vel_topic.c_str(), 10);

    mavros_cur_pose_sub_  = nh_->subscribe<geometry_msgs::PoseStamped>(pose_topic.c_str(), 10, \
        [this](const geometry_msgs::PoseStamped::ConstPtr& _msg) {
            this->cur_pose_ = *_msg;
            this->mavros_has_pose_ = true;
    });
    mavros_cur_state_sub_ = nh_->subscribe<mavros_msgs::State>(state_topic.c_str(), 10, \
        [this](const mavros_msgs::State::ConstPtr& _msg) {
            this->mavros_state_ = *_msg;
    });

    // Make communications spin!
    spin_thread_ = std::thread([this](){
        ros::spin();
    });

    // TODO: Check this and solve frames issue
    // Wait until we have pose
    while (!mavros_has_pose_ && ros::ok()) {
        //ROS_INFO("Waiting for pose");
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }
    initLocalCoordMatrix();

    // Thread publishing target pose at 10Hz for offboard mode
    offboard_thread_ = std::thread([this]() {
        while (ros::ok()) {
            if (control_in_vel_) {
                mavros_ref_vel_pub_.publish(ref_vel_);
                ref_pose_ = cur_pose_;
            } else {
                mavros_ref_pose_pub_.publish(ref_pose_);
                ref_vel_.twist.linear.x = 0;
                ref_vel_.twist.linear.y = 0;
                ref_vel_.twist.linear.z = 0;
                ref_vel_.twist.angular.z = 0;
            }
            // TODO: Check this frequency and use ros::Rate
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    });
}

void BackendMavros::arm() {
    mavros_msgs::CommandBool arming_service;
    arming_service.request.value = true;
    // Arm: abortable
    while (!mavros_state_.armed && !abort_ && ros::ok()) {
        if (!arming_client_.call(arming_service)) {
            ROS_ERROR("Error in arming service calling!");
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(300));
        ROS_INFO("Arming service response.success = %s", arming_service.response.success ? "true" : "false");
        ROS_INFO("Trying to arm... mavros_state_.armed = %s", mavros_state_.armed ? "true" : "false");
    }
}

void BackendMavros::setFlightMode(const std::string& _flight_mode) {
    mavros_msgs::SetMode flight_mode_service;
    flight_mode_service.request.base_mode = 0;
    flight_mode_service.request.custom_mode = _flight_mode;
    // Set mode: abortable
    while (mavros_state_.mode != _flight_mode && !abort_ && ros::ok()) {
        if (!flight_mode_client_.call(flight_mode_service)) {
            ROS_ERROR("Error in set flight mode [%s] service calling!", _flight_mode.c_str());
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(300));
        ROS_INFO("Set flight mode [%s] response.success = %s", _flight_mode.c_str(), \
            flight_mode_service.response.success ? "true" : "false");
        ROS_INFO("Trying to set offboard mode; mavros_state_.mode = %s", mavros_state_.mode.c_str());
    }
}

void BackendMavros::takeOff(double _height) {
    control_in_vel_ = false;  // Take off control is performed in position (not velocity)

    arm();
    // Set offboard mode after saving home pose
    home_pose_ = cur_pose_;
    // TODO: solve frames issue!
    local_start_pos_ -= Eigen::Vector3d(home_pose_.pose.position.x, \
        home_pose_.pose.position.y, home_pose_.pose.position.z);
    ref_pose_ = home_pose_;
    ref_pose_.pose.position.z += _height;
    setFlightMode("OFFBOARD");

    // Wait until take off: unabortable!
    while (!referencePoseReached() && ros::ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    ROS_INFO("Flying!");
}

void BackendMavros::land() {
    control_in_vel_ = false;  // Back to control in position (just in case)
    // Set land mode
    setFlightMode("AUTO.LAND");
    ROS_INFO("Landing...");
    ref_pose_ = cur_pose_;
    ref_pose_.pose.position.z = 0;
    // Landing is unabortable!
    while (!referencePoseReached() && ros::ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    // TODO: better land condition checking
    ROS_INFO("Landed!");
}

void BackendMavros::setVelocity(const Velocity& _vel) {
    control_in_vel_ = true;  // Velocity control!
    // TODO: _vel world <-> body tf...
    ref_vel_ = _vel;
}<commit_msg>Improve landing condition checking<commit_after>//----------------------------------------------------------------------------------------------------------------------
// GRVC UAL
//----------------------------------------------------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2016 GRVC University of Seville
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
// Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//----------------------------------------------------------------------------------------------------------------------

#include <string>
#include <chrono>
#include <uav_abstraction_layer/backend_mavros.h>
#include <uav_abstraction_layer/tinyxml2.h>  // TODO: move?
#include <argument_parser/argument_parser.h>
#include <Eigen/Eigen>
#include <ros/ros.h>
#include <ros/package.h>

using namespace tinyxml2;

namespace grvc { namespace ual {

BackendMavros::BackendMavros(int _argc, char** _argv)
    : args_(_argc, _argv)
{
    ROS_INFO("BackendMavros constructor");

    // Parse arguments
    robot_id_ = args_.getArgument("uav_id", 1);

    // Init ros communications
    std::string node_name = "backend_" + std::to_string(robot_id_);
    ros::init(_argc, _argv, node_name);
    nh_ = new ros::NodeHandle();

    std::string mavros_ns = "mavros_" + std::to_string(robot_id_);
    std::string set_mode_srv = mavros_ns + "/set_mode";
    std::string arming_srv = mavros_ns + "/cmd/arming";
    std::string set_pose_topic = mavros_ns + "/setpoint_position/local";
    std::string set_vel_topic = mavros_ns + "/setpoint_velocity/cmd_vel";
    std::string pose_topic = mavros_ns + "/local_position/pose";
    std::string state_topic = mavros_ns + "/state";

    flight_mode_client_ = nh_->serviceClient<mavros_msgs::SetMode>(set_mode_srv.c_str());
    arming_client_ = nh_->serviceClient<mavros_msgs::CommandBool>(arming_srv.c_str());

    mavros_ref_pose_pub_ = nh_->advertise<geometry_msgs::PoseStamped>(set_pose_topic.c_str(), 10);
    mavros_ref_vel_pub_ = nh_->advertise<geometry_msgs::TwistStamped>(set_vel_topic.c_str(), 10);

    mavros_cur_pose_sub_  = nh_->subscribe<geometry_msgs::PoseStamped>(pose_topic.c_str(), 10, \
        [this](const geometry_msgs::PoseStamped::ConstPtr& _msg) {
            this->cur_pose_ = *_msg;
            this->mavros_has_pose_ = true;
    });
    mavros_cur_state_sub_ = nh_->subscribe<mavros_msgs::State>(state_topic.c_str(), 10, \
        [this](const mavros_msgs::State::ConstPtr& _msg) {
            this->mavros_state_ = *_msg;
    });

    // Make communications spin!
    spin_thread_ = std::thread([this](){
        ros::spin();
    });

    // TODO: Check this and solve frames issue
    // Wait until we have pose
    while (!mavros_has_pose_ && ros::ok()) {
        //ROS_INFO("Waiting for pose");
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }
    initLocalCoordMatrix();

    // Thread publishing target pose at 10Hz for offboard mode
    offboard_thread_ = std::thread([this]() {
        while (ros::ok()) {
            if (control_in_vel_) {
                mavros_ref_vel_pub_.publish(ref_vel_);
                ref_pose_ = cur_pose_;
            } else {
                mavros_ref_pose_pub_.publish(ref_pose_);
                ref_vel_.twist.linear.x = 0;
                ref_vel_.twist.linear.y = 0;
                ref_vel_.twist.linear.z = 0;
                ref_vel_.twist.angular.z = 0;
            }
            // TODO: Check this frequency and use ros::Rate
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    });
}

void BackendMavros::arm() {
    mavros_msgs::CommandBool arming_service;
    arming_service.request.value = true;
    // Arm: abortable
    while (!mavros_state_.armed && !abort_ && ros::ok()) {
        if (!arming_client_.call(arming_service)) {
            ROS_ERROR("Error in arming service calling!");
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(300));
        ROS_INFO("Arming service response.success = %s", arming_service.response.success ? "true" : "false");
        ROS_INFO("Trying to arm... mavros_state_.armed = %s", mavros_state_.armed ? "true" : "false");
    }
}

void BackendMavros::setFlightMode(const std::string& _flight_mode) {
    mavros_msgs::SetMode flight_mode_service;
    flight_mode_service.request.base_mode = 0;
    flight_mode_service.request.custom_mode = _flight_mode;
    // Set mode: abortable
    while (mavros_state_.mode != _flight_mode && !abort_ && ros::ok()) {
        if (!flight_mode_client_.call(flight_mode_service)) {
            ROS_ERROR("Error in set flight mode [%s] service calling!", _flight_mode.c_str());
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(300));
        ROS_INFO("Set flight mode [%s] response.success = %s", _flight_mode.c_str(), \
            flight_mode_service.response.success ? "true" : "false");
        ROS_INFO("Trying to set offboard mode; mavros_state_.mode = %s", mavros_state_.mode.c_str());
    }
}

void BackendMavros::takeOff(double _height) {
    control_in_vel_ = false;  // Take off control is performed in position (not velocity)

    arm();
    // Set offboard mode after saving home pose
    home_pose_ = cur_pose_;
    // TODO: solve frames issue!
    local_start_pos_ -= Eigen::Vector3d(home_pose_.pose.position.x, \
        home_pose_.pose.position.y, home_pose_.pose.position.z);
    ref_pose_ = home_pose_;
    ref_pose_.pose.position.z += _height;
    setFlightMode("OFFBOARD");

    // Wait until take off: unabortable!
    while (!referencePoseReached() && ros::ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    ROS_INFO("Flying!");
}

void BackendMavros::land() {
    control_in_vel_ = false;  // Back to control in position (just in case)
    // Set land mode
    setFlightMode("AUTO.LAND");
    ROS_INFO("Landing...");
    ref_pose_ = cur_pose_;
    ref_pose_.pose.position.z = 0;
    // Landing is unabortable!
    while (mavros_state_.armed && ros::ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    ROS_INFO("Landed!");
}

void BackendMavros::setVelocity(const Velocity& _vel) {
    control_in_vel_ = true;  // Velocity control!
    // TODO: _vel world <-> body tf...
    ref_vel_ = _vel;
}
{'fraction_non_alphanumeric': 0.16945563827607404, 'fraction_numerical': 0.005737313025066594, 'mean_word_length': 2.7883570504527815, 'pattern_counts': {'":': 0, '<': 33, '<?xml version=': 0, '>': 51, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 4}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2880622', 'n_tokens_mistral': 4698, 'n_tokens_neox': 4505, 'n_words': 1046}