dronepertama
unknown
c_cpp
a year ago
4.0 kB
7
Indexable
#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; }
Editor is loading...
Leave a Comment