add disarm

src/px4_connection/src/px4_controller.cpp

@@ -37,6 +37,7 @@ public:
         this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
 
         RCLCPP_INFO(this->get_logger(), "Arm command sent");
+        armed = true;
 
         // create timer to send vehicle attitude setpoints every second
         timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this));
@@ -52,6 +53,7 @@ private:
     rclcpp::TimerBase::SharedPtr timer_;
     double start_time_;
     bool has_sent_status = false;
+    bool armed = false;
 
     /**
      * @brief Only the attitude is enabled, because that is how the drone will be controlled.
@@ -72,6 +74,14 @@ private:
             msg.thrust_body[2] = 1; // down, 100% thrust up
 
             calculate_quaternion(q, 0, degrees_to_radians(10), 0);
+        } else if (this->get_clock()->now().seconds() - start_time > 20)
+        {
+            if (armed)
+            {
+                publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0);
+                armed = false;
+                RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds");
+            }
         }
         else
         {