Feature infinite flight

crazyflie_examples/crazyflie_examples/infinite_flight.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python
+
+from pathlib import Path
+
+from crazyflie_py import Crazyswarm
+from crazyflie_py.uav_trajectory import Trajectory
+import numpy as np
+import time
+
+
+def main():
+    swarm = Crazyswarm()
+    timeHelper = swarm.timeHelper
+    allcfs = swarm.allcfs
+
+    traj1 = Trajectory()
+    traj1.loadcsv(Path(__file__).parent / 'data/figure8.csv')
+
+
+
+    TIMESCALE = 1.0
+    for cf in allcfs.crazyflies:
+        cf.uploadTrajectory(0, 0, traj1)
+    timeHelper.sleep(1)
+
+
+     #pm_state :     0 = on battery ;  1 = charging  ;   2 = charged     ;  3 = low power  ; 4 = shutdown    
+    flight_counter = 1 
+
+    while True : 
+        print("takeoff")
+        allcfs.takeoff(targetHeight=1.0, duration=2.0)
+        timeHelper.sleep(2.5)
+        for cf in allcfs.crazyflies:
+            pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0])
+            cf.goTo(pos, 0, 2.0)
+        timeHelper.sleep(2.5)
+
+        #fly figure8 until battery is low
+        fig8_counter = 0
+        status = allcfs.get_statuses()[0]
+        # while status['battery'] > 3.8:
+        while status['pm_state'] == 0:
+            fig8_counter += 1
+            print(f"starting figure8 number {fig8_counter} of flight number {flight_counter}")
+            allcfs.startTrajectory(0, timescale=TIMESCALE)
+            timeHelper.sleep(traj1.duration * TIMESCALE + 2.0)
+            status = allcfs.get_statuses()[0]
+            print(f"pm state : {status['pm_state']}, battery left : {status['battery']}")
+            timeHelper.sleep(1)
+
+
+        #not sure if useful
+        #check if pm = 3 just to be sure, if not abort test 
+        if status['pm_state'] != 3:
+            print(f"power state is not 3 (low) but {status['pm_state']}. Landing and aborting")
+            allcfs.land(targetHeight=0.06, duration=2.0)
+            timeHelper.sleep(3)
+            return 1                    
+
+
+
+
+        #now that battery is low, we try to land on the pad and see if it's charging
+        allcfs.land(targetHeight=0.06, duration=2.0)
+        timeHelper.sleep(5)
+        status = allcfs.get_statuses()[0]
+
+        #if not charging, take off and land back again until it charges
+        while status['pm_state'] != 1:
+            print("Not charging, retrying")
+            allcfs.takeoff(targetHeight=1.0, duration=2.0)
+            time.sleep(2.5)
+            for cf in allcfs.crazyflies:
+                pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0])
+            cf.goTo(pos, 0, 2.0)
+            timeHelper.sleep(2.5)
+            allcfs.land(targetHeight=0.06, duration=2.0)
+            timeHelper.sleep(5)
+            status = allcfs.get_statuses()[0]       
+
+
+
+        #now we wait until the crazyflie is charged
+        # while status['battery'] < 4.1:
+        while status['pm_state'] != 2:
+            print(f"not charged yet, battery at {status['battery']}V")
+            timeHelper.sleep(60)
+            status = allcfs.get_statuses()[0]
+            ###not sure if useful
+            #check if it's still charging
+            if status['pm_state'] != 1:
+                print(f"charging interrupted, pm state : {status['pm_state']}")
+
+        print("charging finished, time to fly again")
+        flight_counter += 1
+
+
+if __name__ == '__main__':
+    main()

crazyflie_examples/setup.cfg

@@ -8,3 +8,4 @@ console_scripts =
     cmd_full_state = crazyflie_examples.cmd_full_state:main
     set_param = crazyflie_examples.set_param:main
     swap = crazyflie_examples.swap:main
+    infinite_flight = crazyflie_examples.infinite_flight:main

crazyflie_py/crazyflie_py/crazyflie.py

@@ -14,7 +14,7 @@
 # from .visualizer import visNull
 
 
-from crazyflie_interfaces.msg import FullState, Position, TrajectoryPolynomialPiece
+from crazyflie_interfaces.msg import FullState, Position, TrajectoryPolynomialPiece, Status
 from crazyflie_interfaces.srv import GoTo, Land,\
     NotifySetpointsStop, StartTrajectory, Takeoff, UploadTrajectory
 from geometry_msgs.msg import Point
@@ -136,6 +136,8 @@ def __init__(self, node, cfname, paramTypeDict):
         self.setParamsService = node.create_client(
             SetParameters, '/crazyflie_server/set_parameters')
         self.setParamsService.wait_for_service()
+        self.statusSubscriber = node.create_subscription(Status, f'{self.prefix}/status', self.status_topic_callback, 10)
+        self.status = {}
 
         # Query some settings
         getParamsService = node.create_client(GetParameters, '/crazyflie_server/get_parameters')
@@ -698,6 +700,25 @@ def cmdPosition(self, pos, yaw=0.):
     #     self.setParam('ring/solidRed', int(r * 255))
     #     self.setParam('ring/solidGreen', int(g * 255))
     #     self.setParam('ring/solidBlue', int(b * 255))
+
+    def status_topic_callback(self, msg):
+        """Callback method updating the status attribute every time 
+        a crazyflie_interfaces/msg/Status message is published on the topic /cfXXX/status """
+
+        self.status = {'id' : msg.header.frame_id, 'timestamp_sec': msg.header.stamp.sec, 
+                  'timestamp_nsec' : msg.header.stamp.nanosec, 'supervisor' : msg.supervisor_info,
+                  'battery' : msg.battery_voltage, 'pm_state' : msg.pm_state, 'rssi' : msg.rssi, 
+                  'num_rx_broadcast':msg.num_rx_broadcast, 'num_tx_broadcast' : msg.num_tx_broadcast,
+                  'num_rx_unicast':msg.num_rx_unicast, 'num_tx_unicast' : msg.num_tx_unicast}
+
+
+    def get_status(self):
+        """Returns the status dictionary containing info about:   
+        frame id, timestamp, supervisor info, battery voltage, pm state, rssi, number of received or
+        transmitted broadcast or unicast messages. see crazyflie_interfaces/msg/Status for details"""
+
+        # self.node.get_logger().info(f'Crazyflie.get_status() was called {self.status}')
+        return self.status
 
 
 class CrazyflieServer(rclpy.node.Node):
@@ -976,3 +997,16 @@ def cmdFullState(self, pos, vel, acc, yaw, omega):
         self.cmdFullStateMsg.twist.angular.y = omega[1]
         self.cmdFullStateMsg.twist.angular.z = omega[2]
         self.cmdFullStatePublisher.publish(self.cmdFullStateMsg)
+
+    def get_statuses(self):
+        '''
+        Obtain a list containing the status of each crazyflie controlled by the Crazyserver
+        Each status is a dict, see status_topic_callback() for more details
+        '''
+
+        # self.get_logger().info(f'Crazyserver.get_statuses() was called')
+        statuses = []
+        for cf in self.crazyflies:
+            statuses.append(cf.get_status())
+
+        return statuses