Merge pull request #501 from IMRCLab/feature_arming

[server_cpp] add arming support

crazyflie/src/crazyflie_server.cpp

@@ -12,6 +12,7 @@
 #include "crazyflie_interfaces/srv/land.hpp"
 #include "crazyflie_interfaces/srv/go_to.hpp"
 #include "crazyflie_interfaces/srv/notify_setpoints_stop.hpp"
+#include "crazyflie_interfaces/srv/arm.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -34,6 +35,7 @@ using crazyflie_interfaces::srv::Land;
 using crazyflie_interfaces::srv::GoTo;
 using crazyflie_interfaces::srv::UploadTrajectory;
 using crazyflie_interfaces::srv::NotifySetpointsStop;
+using crazyflie_interfaces::srv::Arm;
 using std_srvs::srv::Empty;
 
 using motion_capture_tracking_interfaces::msg::NamedPoseArray;
@@ -161,6 +163,7 @@ class CrazyflieROS
     service_go_to_ = node->create_service<GoTo>(name + "/go_to", std::bind(&CrazyflieROS::go_to, this, _1, _2), service_qos, callback_group_cf_srv);
     service_upload_trajectory_ = node->create_service<UploadTrajectory>(name + "/upload_trajectory", std::bind(&CrazyflieROS::upload_trajectory, this, _1, _2), service_qos, callback_group_cf_srv);
     service_notify_setpoints_stop_ = node->create_service<NotifySetpointsStop>(name + "/notify_setpoints_stop", std::bind(&CrazyflieROS::notify_setpoints_stop, this, _1, _2), service_qos, callback_group_cf_srv);
+    service_arm_ = node->create_service<Arm>(name + "/arm", std::bind(&CrazyflieROS::arm, this, _1, _2), service_qos, callback_group_cf_srv);
 
     // Topics
 
@@ -711,6 +714,16 @@ class CrazyflieROS
     cf_.notifySetpointsStop(request->remain_valid_millisecs);
   }
 
+  void arm(const std::shared_ptr<Arm::Request> request,
+                         std::shared_ptr<Arm::Response> response)
+  {
+    RCLCPP_INFO(logger_, "[%s] arm(%d)",
+                name_.c_str(),
+                request->arm);
+
+    cf_.sendArmingRequest(request->arm);
+  }
+
   void on_logging_pose(uint32_t time_in_ms, const logPose* data) {
     if (publisher_pose_) {
       geometry_msgs::msg::PoseStamped msg;
@@ -919,6 +932,7 @@ class CrazyflieROS
   rclcpp::Service<GoTo>::SharedPtr service_go_to_;
   rclcpp::Service<UploadTrajectory>::SharedPtr service_upload_trajectory_;
   rclcpp::Service<NotifySetpointsStop>::SharedPtr service_notify_setpoints_stop_;
+  rclcpp::Service<Arm>::SharedPtr service_arm_;
 
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_cmd_vel_legacy_;
   rclcpp::Subscription<crazyflie_interfaces::msg::FullState>::SharedPtr subscription_cmd_full_state_;
@@ -1102,6 +1116,7 @@ class CrazyflieServer : public rclcpp::Node
     service_land_ = this->create_service<Land>("all/land", std::bind(&CrazyflieServer::land, this, _1, _2), service_qos, callback_group_all_srv_);
     service_go_to_ = this->create_service<GoTo>("all/go_to", std::bind(&CrazyflieServer::go_to, this, _1, _2), service_qos, callback_group_all_srv_);
     service_notify_setpoints_stop_ = this->create_service<NotifySetpointsStop>("all/notify_setpoints_stop", std::bind(&CrazyflieServer::notify_setpoints_stop, this, _1, _2), service_qos, callback_group_all_srv_);
+    service_arm_ = this->create_service<Arm>("all/arm", std::bind(&CrazyflieServer::arm, this, _1, _2), service_qos, callback_group_all_srv_);
 
     // This is the last service to announce and can be used to check if the server is fully available
     service_emergency_ = this->create_service<Empty>("all/emergency", std::bind(&CrazyflieServer::emergency, this, _1, _2), service_qos, callback_group_all_srv_);
@@ -1209,6 +1224,21 @@ class CrazyflieServer : public rclcpp::Node
     }
   }
 
+  void arm(const std::shared_ptr<Arm::Request> request,
+                         std::shared_ptr<Arm::Response> response)
+  {
+    RCLCPP_INFO(logger_, "[all] arm(%d)",
+                request->arm);
+
+    for (int i = 0; i < broadcasts_num_repeats_; ++i) {
+      for (auto &bc : broadcaster_) {
+        auto &cfbc = bc.second;
+        cfbc->sendArmingRequest(request->arm);
+      }
+      std::this_thread::sleep_for(std::chrono::milliseconds(broadcasts_delay_between_repeats_ms_));
+    }
+  }
+
   void cmd_full_state_changed(const crazyflie_interfaces::msg::FullState::SharedPtr msg)
   { 
     float x = msg->pose.position.x;
@@ -1450,6 +1480,7 @@ class CrazyflieServer : public rclcpp::Node
     rclcpp::Service<Land>::SharedPtr service_land_;
     rclcpp::Service<GoTo>::SharedPtr service_go_to_;
     rclcpp::Service<NotifySetpointsStop>::SharedPtr service_notify_setpoints_stop_;
+    rclcpp::Service<Arm>::SharedPtr service_arm_;
 
     std::map<std::string, std::unique_ptr<CrazyflieROS>> crazyflies_;
 

crazyflie_examples/crazyflie_examples/arming.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+
+from crazyflie_py import Crazyswarm
+
+
+def main():
+    swarm = Crazyswarm()
+    timeHelper = swarm.timeHelper
+    allcfs = swarm.allcfs
+
+    # arm (one by one)
+    for cf in allcfs.crazyflies:
+        cf.arm(True)
+        timeHelper.sleep(1.0)
+
+    timeHelper.sleep(2.0)
+
+    # disarm (broadcast)
+    allcfs.arm(False)
+    timeHelper.sleep(5.0)
+
+
+if __name__ == '__main__':
+    main()

crazyflie_examples/setup.cfg

@@ -9,3 +9,4 @@ console_scripts =
     set_param = crazyflie_examples.set_param:main
     swap = crazyflie_examples.swap:main
     infinite_flight = crazyflie_examples.infinite_flight:main
+    arming = crazyflie_examples.arming:main

crazyflie_interfaces/CMakeLists.txt

@@ -39,6 +39,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/UploadTrajectory.srv"
   "srv/RemoveLogging.srv"
   "srv/AddLogging.srv"
+  "srv/Arm.srv"
   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs 
   ADD_LINTER_TESTS
 )

crazyflie_interfaces/srv/Arm.srv

@@ -0,0 +1,2 @@
+bool arm
+---

crazyflie_py/crazyflie_py/crazyflie.py

@@ -15,7 +15,7 @@
 
 
 from crazyflie_interfaces.msg import FullState, Position, Status, TrajectoryPolynomialPiece
-from crazyflie_interfaces.srv import GoTo, Land,\
+from crazyflie_interfaces.srv import Arm, GoTo, Land,\
     NotifySetpointsStop, StartTrajectory, Takeoff, UploadTrajectory
 from geometry_msgs.msg import Point
 import numpy as np
@@ -133,6 +133,9 @@ def __init__(self, node, cfname, paramTypeDict):
         self.notifySetpointsStopService = node.create_client(
             NotifySetpointsStop, prefix + '/notify_setpoints_stop')
         self.notifySetpointsStopService.wait_for_service()
+        self.armService = node.create_client(
+            Arm, prefix + '/arm')
+        # self.armService.wait_for_service()
         self.setParamsService = node.create_client(
             SetParameters, '/crazyflie_server/set_parameters')
         self.setParamsService.wait_for_service()
@@ -466,6 +469,21 @@ def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0):
         req.group_mask = groupMask
         self.notifySetpointsStopService.call_async(req)
 
+    def arm(self, arm=True):
+        """
+        Arms the quadrotor.
+
+        For a brushless or Bolt-based drone the motors start spinning and flight
+        is enabled.
+
+        Args:
+            arm (boolean): True if the motors should be armed, False otherwise.
+
+        """
+        req = Arm.Request()
+        req.arm = arm
+        self.armService.call_async(req)
+
     # def position(self):
     #     """Returns the last true position measurement from motion capture.
 
@@ -763,6 +781,8 @@ def __init__(self):
         self.goToService.wait_for_service()
         self.startTrajectoryService = self.create_client(StartTrajectory, 'all/start_trajectory')
         self.startTrajectoryService.wait_for_service()
+        self.armService = self.create_client(Arm, 'all/arm')
+        # self.armService.wait_for_service()
         self.setParamsService = self.create_client(
             SetParameters, '/crazyflie_server/set_parameters')
         self.setParamsService.wait_for_service()
@@ -953,6 +973,21 @@ def startTrajectory(self, trajectoryId,
         req.relative = relative
         self.startTrajectoryService.call_async(req)
 
+    def arm(self, arm=True):
+        """
+        Broadcasted arming.
+
+        For a brushless or Bolt-based drone the motors start spinning and flight
+        is enabled.
+
+        Args:
+            arm (boolean): True if the motors should be armed, False otherwise.
+
+        """
+        req = Arm.Request()
+        req.arm = arm
+        self.armService.call_async(req)
+
     def setParam(self, name, value):
         """Set parameter via broadcasts. See Crazyflie.setParam for details."""
         try:

docs2/overview.rst

@@ -101,3 +101,7 @@ Support functionality with backends
 +---------------------+---------+-----------+---------+
 | - LPS               | Yes     | Yes       | No      |
 +---------------------+---------+-----------+---------+
+| Misc                                                |
++---------------------+---------+-----------+---------+
+| - Arming            | Yes     | No        | n/a     |
++---------------------+---------+-----------+---------+