From e2a1655f088e1e8c733f14c37fdc27f1adad2e25 Mon Sep 17 00:00:00 2001 From: spencerfolk Date: Thu, 12 Dec 2024 17:29:59 -0500 Subject: [PATCH 1/4] Added arm sequence to support brushless crazyflie. --- .../kr_crazyflie_interface/CMakeLists.txt | 4 +- interfaces/kr_crazyflie_interface/package.xml | 1 + .../src/so3cmd_to_crazyflie_nodelet.cpp | 84 +++++++++++++++++++ 3 files changed, 88 insertions(+), 1 deletion(-) diff --git a/interfaces/kr_crazyflie_interface/CMakeLists.txt b/interfaces/kr_crazyflie_interface/CMakeLists.txt index 9029cf6f..0bc62d42 100644 --- a/interfaces/kr_crazyflie_interface/CMakeLists.txt +++ b/interfaces/kr_crazyflie_interface/CMakeLists.txt @@ -17,7 +17,8 @@ find_package( nav_msgs geometry_msgs kr_mav_msgs - nodelet) + nodelet + crazyflie_driver) find_package(Eigen3 REQUIRED) include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) @@ -31,6 +32,7 @@ catkin_package( geometry_msgs kr_mav_msgs nodelet + crazyflie_driver DEPENDS EIGEN3) diff --git a/interfaces/kr_crazyflie_interface/package.xml b/interfaces/kr_crazyflie_interface/package.xml index 413943ce..8357e720 100644 --- a/interfaces/kr_crazyflie_interface/package.xml +++ b/interfaces/kr_crazyflie_interface/package.xml @@ -16,6 +16,7 @@ geometry_msgs kr_mav_msgs nodelet + crazyflie_driver diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp index ddc8bb67..b2eb9ebd 100644 --- a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp +++ b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp @@ -11,6 +11,8 @@ #include #include #include +#include // Service definition for sendPacket +#include // Message definition for crtpPacket #include @@ -25,6 +27,7 @@ class SO3CmdToCrazyflie : public nodelet::Nodelet EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: + void send_arming_request(bool arm); void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); @@ -37,6 +40,8 @@ class SO3CmdToCrazyflie : public nodelet::Nodelet ros::Subscriber so3_cmd_sub_; ros::Subscriber odom_sub_; + ros::ServiceClient arm_client_; // Service client to send packets + double so3_cmd_timeout_; ros::Time last_so3_cmd_time_; kr_mav_msgs::SO3Command last_so3_cmd_; @@ -52,6 +57,45 @@ class SO3CmdToCrazyflie : public nodelet::Nodelet int thrust_pwm_max_; // Mapped to PWM int motor_status_; + bool armed_; + int arm_status_; +}; + +void SO3CmdToCrazyflie::send_arming_request(bool arm) +{ + ROS_INFO("Setting arm to: %d", arm); + + // Create a packet to arm or disarm + // crazyflie_driver::crtpPacket packet; + // packet.header = 0x0D; + // packet.size = 1; // Payload length is 1 byte for arm/disarm + // // Clear the data array before populating it + // std::fill(packet.data.begin(), packet.data.end(), 0); + // // Set the payload (1 byte for arm or disarm) + // packet.data[0] = arm ? 1 : 0; // 1 for arm, 0 for disarm + + // Create a custom packet for arming/disarming + crazyflie_driver::crtpPacket packet; + packet.header = 220; // Same as 13 in decimal + packet.size = 2; // Payload length of 2 bytes + // Set the first byte of the payload to 1 (indicating the arming/disarming operation) + packet.data[0] = 1; + // Set the second byte to arm/disarm (1 for arm, 0 for disarm) + packet.data[1] = arm ? 1 : 0; + + // Call the sendPacket service to send the packet + crazyflie_driver::sendPacket srv; + srv.request.packet = packet; + + if (arm_client_.call(srv)) + { + ROS_INFO("Arming request sent successfully, arm: %d", arm); + } + else + { + ROS_ERROR("Failed to send arming request"); + } + }; void SO3CmdToCrazyflie::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) @@ -80,6 +124,16 @@ void SO3CmdToCrazyflie::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr // switch on motors if(msg->aux.enable_motors) { + // First try to arm the drone. + if(!armed_) + { + for(int i=0; i<1; i++) + { + send_arming_request(true); + } + armed_ = true; + }; + // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start if(motor_status_ < 3) { @@ -106,6 +160,16 @@ void SO3CmdToCrazyflie::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr crazy_cmd_vel_pub_.publish(motors_vel_cmd); last_so3_cmd_ = *msg; last_so3_cmd_time_ = msg->header.stamp; + + // Disarm motors + if(armed_) + { + for(int i=0; i<1; i++) + { + send_arming_request(false); + } + armed_ = false; + }; return; } @@ -214,6 +278,26 @@ void SO3CmdToCrazyflie::onInit(void) odom_set_ = false; so3_cmd_set_ = false; motor_status_ = 0; + armed_ = false; + arm_status_ = 0; + + // Retrieve mav_name parameter + std::string mav_name; + if (!priv_nh.getParam("mav_name", mav_name)) + { + ROS_FATAL("mav_name parameter not found."); + return; + } + + // Construct the full service name + std::string service_name = "/" + mav_name + "/send_packet"; + + // Arming packet client. + arm_client_ = priv_nh.serviceClient(service_name); + + ROS_INFO("Waiting for send_packet service..."); + arm_client_.waitForExistence(); + ROS_INFO("send_packet service is available."); // TODO make sure this is publishing to the right place crazy_fast_cmd_vel_pub_ = priv_nh.advertise("cmd_vel_fast", 10); From b64acb1316b564a4180d37e8cd37dfe109e32215 Mon Sep 17 00:00:00 2001 From: spencerfolk Date: Tue, 17 Dec 2024 11:15:22 -0500 Subject: [PATCH 2/4] Better documentation. --- .../src/so3cmd_to_crazyflie_nodelet.cpp | 20 +++++-------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp index b2eb9ebd..fc3f7db3 100644 --- a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp +++ b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp @@ -65,23 +65,13 @@ void SO3CmdToCrazyflie::send_arming_request(bool arm) { ROS_INFO("Setting arm to: %d", arm); - // Create a packet to arm or disarm - // crazyflie_driver::crtpPacket packet; - // packet.header = 0x0D; - // packet.size = 1; // Payload length is 1 byte for arm/disarm - // // Clear the data array before populating it - // std::fill(packet.data.begin(), packet.data.end(), 0); - // // Set the payload (1 byte for arm or disarm) - // packet.data[0] = arm ? 1 : 0; // 1 for arm, 0 for disarm - // Create a custom packet for arming/disarming + // https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_platform/ crazyflie_driver::crtpPacket packet; - packet.header = 220; // Same as 13 in decimal - packet.size = 2; // Payload length of 2 bytes - // Set the first byte of the payload to 1 (indicating the arming/disarming operation) - packet.data[0] = 1; - // Set the second byte to arm/disarm (1 for arm, 0 for disarm) - packet.data[1] = arm ? 1 : 0; + packet.header = 220; // Port 13 but byteshifted following https://github.com/bitcraze/crazyflie-lib-python/blob/master/cflib/crtp/crtpstack.py#L120-L132 + packet.size = 2; // Payload length + packet.data[0] = 1; // Channel 0 -- Platform commands + packet.data[1] = arm ? 1 : 0; // Arm message (bool). // Call the sendPacket service to send the packet crazyflie_driver::sendPacket srv; From 485fa0013e483f97be5e72c0f4af62fa62e208bf Mon Sep 17 00:00:00 2001 From: spencerfolk Date: Tue, 17 Dec 2024 11:38:27 -0500 Subject: [PATCH 3/4] Implemented rebooting to reset CF supervisor after disarm. --- .../src/so3cmd_to_crazyflie_nodelet.cpp | 52 +++++++++++++++++-- 1 file changed, 48 insertions(+), 4 deletions(-) diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp index fc3f7db3..b0bf4465 100644 --- a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp +++ b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp @@ -28,6 +28,7 @@ class SO3CmdToCrazyflie : public nodelet::Nodelet private: void send_arming_request(bool arm); + void reboot(); void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); @@ -40,7 +41,7 @@ class SO3CmdToCrazyflie : public nodelet::Nodelet ros::Subscriber so3_cmd_sub_; ros::Subscriber odom_sub_; - ros::ServiceClient arm_client_; // Service client to send packets + ros::ServiceClient packet_client_; // Service client to send packets double so3_cmd_timeout_; ros::Time last_so3_cmd_time_; @@ -77,7 +78,7 @@ void SO3CmdToCrazyflie::send_arming_request(bool arm) crazyflie_driver::sendPacket srv; srv.request.packet = packet; - if (arm_client_.call(srv)) + if (packet_client_.call(srv)) { ROS_INFO("Arming request sent successfully, arm: %d", arm); } @@ -85,6 +86,47 @@ void SO3CmdToCrazyflie::send_arming_request(bool arm) { ROS_ERROR("Failed to send arming request"); } +}; + +void SO3CmdToCrazyflie::reboot() +{ + ROS_INFO("Attempting to reboot Crazyflie..."); + + // Create custom packet for rebooting. + crazyflie_driver::crtpPacket packet; + packet.header = 255; // + packet.size = 2; // Payload length + packet.data[0] = 0xFE; + packet.data[1] = 0x02; // SYSOFF command. + + // Call the sendPacket service to send the packet + crazyflie_driver::sendPacket srv; + srv.request.packet = packet; + + if (packet_client_.call(srv)) + { + ROS_INFO("Powering down."); + } + else + { + ROS_ERROR("Failed to power down"); + } + + // Wait a little bit. + ros::Duration(0.5).sleep(); // Sleep for a short time. + + // Now send boot up command. + packet.data[1] = 0x03; // SYSON command. + srv.request.packet = packet; + + if (packet_client_.call(srv)) + { + ROS_INFO("Powering up."); + } + else + { + ROS_ERROR("Failed to power up."); + } }; @@ -159,6 +201,8 @@ void SO3CmdToCrazyflie::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr send_arming_request(false); } armed_ = false; + // Reboot Crazyflie to be armed again. + reboot(); }; return; } @@ -283,10 +327,10 @@ void SO3CmdToCrazyflie::onInit(void) std::string service_name = "/" + mav_name + "/send_packet"; // Arming packet client. - arm_client_ = priv_nh.serviceClient(service_name); + packet_client_ = priv_nh.serviceClient(service_name); ROS_INFO("Waiting for send_packet service..."); - arm_client_.waitForExistence(); + packet_client_.waitForExistence(); ROS_INFO("send_packet service is available."); // TODO make sure this is publishing to the right place From ef7c92fd271a3d41a44a64d2458616a07e79cf74 Mon Sep 17 00:00:00 2001 From: spencerfolk Date: Tue, 17 Dec 2024 14:33:32 -0500 Subject: [PATCH 4/4] Disable motors on until Crazyflie has rebooted. --- rqt_mav_manager/src/rqt_mav_manager/__init__.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rqt_mav_manager/src/rqt_mav_manager/__init__.py b/rqt_mav_manager/src/rqt_mav_manager/__init__.py index 85665c2a..12c0fdd7 100644 --- a/rqt_mav_manager/src/rqt_mav_manager/__init__.py +++ b/rqt_mav_manager/src/rqt_mav_manager/__init__.py @@ -6,6 +6,7 @@ import rospy from python_qt_binding import loadUi from python_qt_binding.QtWidgets import QWidget +from PyQt5.QtCore import QTimer from rqt_gui_py.plugin import Plugin import kr_mav_manager.srv @@ -66,11 +67,15 @@ def _on_motors_on_pressed(self): def _on_motors_off_pressed(self): try: + self._widget.motors_on_push_button.setEnabled(False) motors_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/motors' rospy.wait_for_service(motors_topic, timeout=1.0) motors_off = rospy.ServiceProxy(motors_topic, std_srvs.srv.SetBool) resp = motors_off(False) print(resp.success) + + QTimer.singleShot(3000, lambda: self._widget.motors_on_push_button.setEnabled(True)) + except rospy.ServiceException as e: print("Service call failed: %s"%e) except(rospy.ROSException) as e: