From 993c674830bc01a006d64b81ae637f9a98f0e063 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 May 2024 16:49:03 +0200 Subject: [PATCH 1/2] [server_cpp] add arming support --- crazyflie/deps/crazyflie_tools | 2 +- crazyflie/src/crazyflie_server.cpp | 31 +++++++++++++++++ .../crazyflie_examples/arming.py | 24 ++++++++++++++ crazyflie_examples/setup.cfg | 1 + crazyflie_interfaces/CMakeLists.txt | 1 + crazyflie_interfaces/srv/Arm.srv | 2 ++ crazyflie_py/crazyflie_py/crazyflie.py | 33 ++++++++++++++++++- docs2/overview.rst | 4 +++ 8 files changed, 96 insertions(+), 2 deletions(-) create mode 100644 crazyflie_examples/crazyflie_examples/arming.py create mode 100644 crazyflie_interfaces/srv/Arm.srv diff --git a/crazyflie/deps/crazyflie_tools b/crazyflie/deps/crazyflie_tools index 32218e742..a64ff9363 160000 --- a/crazyflie/deps/crazyflie_tools +++ b/crazyflie/deps/crazyflie_tools @@ -1 +1 @@ -Subproject commit 32218e7429651a681edf573d2ec24006fb07591e +Subproject commit a64ff93637f9c8ef7a909cada68c5dcb95b60e63 diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index a31a5e5e1..689cc542f 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/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(name + "/go_to", std::bind(&CrazyflieROS::go_to, this, _1, _2), service_qos, callback_group_cf_srv); service_upload_trajectory_ = node->create_service(name + "/upload_trajectory", std::bind(&CrazyflieROS::upload_trajectory, this, _1, _2), service_qos, callback_group_cf_srv); service_notify_setpoints_stop_ = node->create_service(name + "/notify_setpoints_stop", std::bind(&CrazyflieROS::notify_setpoints_stop, this, _1, _2), service_qos, callback_group_cf_srv); + service_arm_ = node->create_service(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 request, + std::shared_ptr 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::SharedPtr service_go_to_; rclcpp::Service::SharedPtr service_upload_trajectory_; rclcpp::Service::SharedPtr service_notify_setpoints_stop_; + rclcpp::Service::SharedPtr service_arm_; rclcpp::Subscription::SharedPtr subscription_cmd_vel_legacy_; rclcpp::Subscription::SharedPtr subscription_cmd_full_state_; @@ -1102,6 +1116,7 @@ class CrazyflieServer : public rclcpp::Node service_land_ = this->create_service("all/land", std::bind(&CrazyflieServer::land, this, _1, _2), service_qos, callback_group_all_srv_); service_go_to_ = this->create_service("all/go_to", std::bind(&CrazyflieServer::go_to, this, _1, _2), service_qos, callback_group_all_srv_); service_notify_setpoints_stop_ = this->create_service("all/notify_setpoints_stop", std::bind(&CrazyflieServer::notify_setpoints_stop, this, _1, _2), service_qos, callback_group_all_srv_); + service_arm_ = this->create_service("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("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 request, + std::shared_ptr 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::SharedPtr service_land_; rclcpp::Service::SharedPtr service_go_to_; rclcpp::Service::SharedPtr service_notify_setpoints_stop_; + rclcpp::Service::SharedPtr service_arm_; std::map> crazyflies_; diff --git a/crazyflie_examples/crazyflie_examples/arming.py b/crazyflie_examples/crazyflie_examples/arming.py new file mode 100644 index 000000000..5eb12a985 --- /dev/null +++ b/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() diff --git a/crazyflie_examples/setup.cfg b/crazyflie_examples/setup.cfg index f387d09cf..5a796d668 100644 --- a/crazyflie_examples/setup.cfg +++ b/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 diff --git a/crazyflie_interfaces/CMakeLists.txt b/crazyflie_interfaces/CMakeLists.txt index 0e92f4d09..1fb150d8a 100644 --- a/crazyflie_interfaces/CMakeLists.txt +++ b/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 ) diff --git a/crazyflie_interfaces/srv/Arm.srv b/crazyflie_interfaces/srv/Arm.srv new file mode 100644 index 000000000..e659ce1ff --- /dev/null +++ b/crazyflie_interfaces/srv/Arm.srv @@ -0,0 +1,2 @@ +bool arm +--- diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 006dcb1bb..4c9a8fec3 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/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,19 @@ def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0): req.group_mask = groupMask self.notifySetpointsStopService.call_async(req) + def arm(self, arm=True): + """ + Arms the quadrotor, i.e. 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 +779,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 +971,19 @@ def startTrajectory(self, trajectoryId, req.relative = relative self.startTrajectoryService.call_async(req) + def arm(self, arm=True): + """ + Broadcasted arming of the quadrotors, i.e. 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: diff --git a/docs2/overview.rst b/docs2/overview.rst index cc413fe1f..b504d7bfd 100644 --- a/docs2/overview.rst +++ b/docs2/overview.rst @@ -101,3 +101,7 @@ Support functionality with backends +---------------------+---------+-----------+---------+ | - LPS | Yes | Yes | No | +---------------------+---------+-----------+---------+ +| Misc | ++---------------------+---------+-----------+---------+ +| - Arming | Yes | No | n/a | ++---------------------+---------+-----------+---------+ \ No newline at end of file From 67491378446113b4316de926d60eb8fab21378c9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 May 2024 17:01:03 +0200 Subject: [PATCH 2/2] fix flake8 --- crazyflie_py/crazyflie_py/crazyflie.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 4c9a8fec3..02f391f1b 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -471,8 +471,10 @@ def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0): def arm(self, arm=True): """ - Arms the quadrotor, i.e. for a brushless or Bolt-based drone the - motors start spinning and flight is enabled. + 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. @@ -973,8 +975,10 @@ def startTrajectory(self, trajectoryId, def arm(self, arm=True): """ - Broadcasted arming of the quadrotors, i.e. for a brushless or Bolt-based drone the - motors start spinning and flight is enabled. + 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.