Program Listing for File set_offboard_mode_behavior.hpp

↰ Return to documentation for file (/tmp/ws/src/aerostack2/as2_behaviors/as2_behaviors_platform/include/as2_behaviors_platform/set_offboard_mode_behavior.hpp)

#ifndef __SET_OFFBOARD_STATE_BEHAVIOR_HPP__
#define __SET_OFFBOARD_STATE_BEHAVIOR_HPP__

#include "as2_behavior/behavior_server.hpp"
#include "as2_core/names/services.hpp"
#include "as2_msgs/action/set_offboard_mode.hpp"
#include "std_srvs/srv/set_bool.hpp"

class SetOffboardModeBehavior
    : public as2_behavior::BehaviorServer<as2_msgs::action::SetOffboardMode>

{
public:
  SetOffboardModeBehavior()
      : as2_behavior::BehaviorServer<as2_msgs::action::SetOffboardMode>(
            as2_names::services::platform::set_offboard_mode) {
    client_ = this->create_client<std_srvs::srv::SetBool>(
        as2_names::services::platform::set_offboard_mode);
  }

  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_;
  rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture future_;

public:
  bool on_activate(std::shared_ptr<const as2_msgs::action::SetOffboardMode::Goal> goal) override {
    auto req = std::make_shared<std_srvs::srv::SetBool::Request>();

    req->data = goal->request;
    using namespace std::chrono_literals;
    if (!client_->wait_for_service(5s)) {
      RCLCPP_INFO(get_logger(), "service not available");
      return false;
    }
    future_ = client_->async_send_request(req).share();
    if (!future_.valid()) {
      RCLCPP_INFO(get_logger(), "request not sent");
      return false;
    }
    return true;
  };

  bool on_modify(std::shared_ptr<const as2_msgs::action::SetOffboardMode::Goal> goal) override {
    RCLCPP_WARN(get_logger(), "Cannot modify a service request");
    return false;
  };

  bool on_deactivate(const std::shared_ptr<std::string>& message) override {
    *message = "Unable to deactivate InstantBehavior";
    return false;
  };
  bool on_pause(const std::shared_ptr<std::string>& message) override {
    *message = "Unable to pause InstantBehavior";
    return false;
  };

  bool on_resume(const std::shared_ptr<std::string>& message) override {
    *message = "Unable to resume InstantBehavior";
    return false;
  };

  void on_execution_end(const as2_behavior::ExecutionStatus& state) override{};

  as2_behavior::ExecutionStatus on_run(
      const typename std::shared_ptr<const as2_msgs::action::SetOffboardMode::Goal>& goal,
      typename std::shared_ptr<as2_msgs::action::SetOffboardMode::Feedback>& feedback_msg,
      typename std::shared_ptr<as2_msgs::action::SetOffboardMode::Result>& result_msg) override {
    if (future_.valid() && future_.wait_for(std::chrono::seconds(0)) == std::future_status::ready) {
      auto result         = future_.get();
      result_msg->success = result->success;
      if (result->success) {
        result_msg->success = true;
        return as2_behavior::ExecutionStatus::SUCCESS;
      } else {
        result_msg->success = false;
        return as2_behavior::ExecutionStatus::FAILURE;
      }
    }
    return as2_behavior::ExecutionStatus::RUNNING;
  };
};
#endif