Support for Brushless Crazyflie

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)
 

interfaces/kr_crazyflie_interface/package.xml

@@ -16,6 +16,7 @@
   <depend>geometry_msgs</depend>
   <depend>kr_mav_msgs</depend>
   <depend>nodelet</depend>
+  <depend>crazyflie_driver</depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelet_plugin.xml"/>

interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp

@@ -11,6 +11,8 @@
 #include <nav_msgs/Odometry.h>
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
+#include <crazyflie_driver/sendPacket.h>  // Service definition for sendPacket
+#include <crazyflie_driver/crtpPacket.h>  // Message definition for crtpPacket
 
 #include <Eigen/Geometry>
 
@@ -25,6 +27,8 @@ class SO3CmdToCrazyflie : public nodelet::Nodelet
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  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);
 
@@ -37,6 +41,8 @@ class SO3CmdToCrazyflie : public nodelet::Nodelet
   ros::Subscriber so3_cmd_sub_;
   ros::Subscriber odom_sub_;
 
+  ros::ServiceClient packet_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 +58,76 @@ 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 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;  // 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;
+  srv.request.packet = packet;
+
+  if (packet_client_.call(srv))
+  {
+    ROS_INFO("Arming request sent successfully, arm: %d", arm);
+  }
+  else
+  {
+    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.");
+  }
+
 };
 
 void SO3CmdToCrazyflie::odom_callback(const nav_msgs::Odometry::ConstPtr &odom)
@@ -80,6 +156,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 +192,18 @@ 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;
+      // Reboot Crazyflie to be armed again. 
+      reboot();
+    };
     return;
   }
 
@@ -214,6 +312,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. 
+  packet_client_ = priv_nh.serviceClient<crazyflie_driver::sendPacket>(service_name);
+
+  ROS_INFO("Waiting for send_packet service...");
+  packet_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<geometry_msgs::Twist>("cmd_vel_fast", 10);

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: