dronepertama

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <mavros_msgs/srv/command_bool.hpp>
#include <mavros_msgs/srv/set_mode.hpp>
#include <mavros_msgs/msg/state.hpp>
#include <stdint.h>

#include <chrono>
#include <iostream>
#include <memory>

using namespace std::chrono;
using namespace std::chrono_literals;
using std::placeholders::_1;

class ControlDrone : public rclcpp::Node
{
  public:
    ControlDrone() : Node("control_drone"), count_(0)
    {
      if(current_state.armed){RCLCPP_INFO(rclcpp::get_logger("status_check"), "Drone is armed");}
      if(current_state.mode == "OFFBOARD"){RCLCPP_INFO(rclcpp::get_logger("status_check"), "Drone is on offboard mode");}
      
      // state_subscriber = this->create_subscription<mavros_msgs::msg::State>("mavros/state", 10, std::bind(&ControlDrone::state_cb, this, _1));
      move_publisher = this->create_publisher<geometry_msgs::msg::PoseStamped>("mavros/setpoint_position/local", 10);
      timer_ = this->create_wall_timer(100ms, std::bind(&ControlDrone::move_drone, this));

      move_drone();
    }

  private:
    // void state_cb(const mavros_msgs::msg::State::ConstPtr& msg)
    // {
    //   current_state = *msg;
    // }


    void move_drone()
    {
      auto pose = geometry_msgs::msg::PoseStamped();
      pose.pose.position.x = 0;
      pose.pose.position.y = 0;
      pose.pose.position.z = 3;

      move_publisher->publish(pose);
      RCLCPP_INFO(this->get_logger(), "taking off...");
    }


    mavros_msgs::msg::State current_state;
    // rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_subscriber;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr move_publisher;
    rclcpp::TimerBase::SharedPtr timer_;
    size_t count_;

};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  std::shared_ptr<rclcpp::Node> initNode = rclcpp::Node::make_shared("init_node");
  rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client =
  initNode->create_client<mavros_msgs::srv::CommandBool>("mavros/cmd/arming");
  rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr setmode_client =
  initNode->create_client<mavros_msgs::srv::SetMode>("mavros/set_mode");
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr takeoff_publisher =
  initNode->create_publisher<geometry_msgs::msg::PoseStamped>("mavros/setpoint_position/local", 10);

  auto takeoff_pose = geometry_msgs::msg::PoseStamped();
  takeoff_pose.pose.position.x = 0;
  takeoff_pose.pose.position.y = 0;
  takeoff_pose.pose.position.z = 3;

  for(int i = 100; rclcpp::ok() && i > 0; --i)
  {
    takeoff_publisher->publish(takeoff_pose);
    rclcpp::spin_some(initNode);
  }

  auto request_arm = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
  request_arm->set__value(true);

  while (!arming_client->wait_for_service(1s))
  {
    if (!rclcpp::ok())
    {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
      throw("Interrupted while waiting for the service");
    }

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
  }

  auto future = arming_client->async_send_request(request_arm);
  RCLCPP_INFO(initNode->get_logger(), "Arming command sent");

  auto request_offbmode = std::make_shared<mavros_msgs::srv::SetMode::Request>();
  request_offbmode->custom_mode = "OFFBOARD";

  while (!setmode_client->wait_for_service(1s))
  {
    if (!rclcpp::ok())
    {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
      throw("Interrupted while waiting for the service");
    }

    RCLCPP_INFO(initNode->get_logger(), "service not available, waiting again...");
  }

  auto future_offboard = setmode_client->async_send_request(request_offbmode);
  RCLCPP_INFO(initNode->get_logger(), "Offboard command sent");

  rclcpp::spin(std::make_shared<ControlDrone>());
  rclcpp::shutdown();
  return 0;
}