From 1e8ef226cf47d5e72a7ee61cf3afbebd141a3a00 Mon Sep 17 00:00:00 2001
From: julienthevenoz <125280576+julienthevenoz@users.noreply.github.com>
Date: Mon, 18 Mar 2024 12:17:59 +0100
Subject: [PATCH] Feature infinite flight (#444)

* infinite flight example
---
 .../crazyflie_examples/infinite_flight.py     | 89 +++++++++++++++++++
 crazyflie_examples/setup.cfg                  |  1 +
 crazyflie_py/crazyflie_py/crazyflie.py        | 35 +++++++-
 3 files changed, 124 insertions(+), 1 deletion(-)
 create mode 100644 crazyflie_examples/crazyflie_examples/infinite_flight.py

diff --git a/crazyflie_examples/crazyflie_examples/infinite_flight.py b/crazyflie_examples/crazyflie_examples/infinite_flight.py
new file mode 100644
index 000000000..6aa94c6fb
--- /dev/null
+++ b/crazyflie_examples/crazyflie_examples/infinite_flight.py
@@ -0,0 +1,89 @@
+#!/usr/bin/env python
+
+from pathlib import Path
+
+from crazyflie_py import Crazyswarm
+from crazyflie_py.uav_trajectory import Trajectory
+import numpy as np
+
+
+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.crazyflies[0].get_status()
+        # 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.crazyflies[0].get_status()
+            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.crazyflies[0].get_status()
+
+        # 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)
+            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)
+            allcfs.land(targetHeight=0.06, duration=2.0)
+            timeHelper.sleep(5)
+            status = allcfs.crazyflies[0].get_status()
+
+        # now we wait until the crazyflie is charged
+        # while status['battery'] < 4.1:
+        while status['pm_state'] != 2:
+            print(f'Charging in progress, battery at {status["battery"]}V')
+            timeHelper.sleep(60)
+            status = allcfs.crazyflies[0].get_status()
+            # check if it's still charging ###not sure if this check is useful
+            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()
diff --git a/crazyflie_examples/setup.cfg b/crazyflie_examples/setup.cfg
index 0c0238812..f387d09cf 100644
--- a/crazyflie_examples/setup.cfg
+++ b/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
diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py
index 30c50dd27..a1e39466d 100644
--- a/crazyflie_py/crazyflie_py/crazyflie.py
+++ b/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, Status, TrajectoryPolynomialPiece
 from crazyflie_interfaces.srv import GoTo, Land,\
     NotifySetpointsStop, StartTrajectory, Takeoff, UploadTrajectory
 from geometry_msgs.msg import Point
@@ -136,6 +136,9 @@ 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')
@@ -699,6 +702,36 @@ def cmdPosition(self, pos, yaw=0.):
     #     self.setParam('ring/solidGreen', int(g * 255))
     #     self.setParam('ring/solidBlue', int(b * 255))
 
+    def status_topic_callback(self, msg):
+        """
+        Call back for topic /cfXXX/status.
+
+        Update 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):
+        """
+        Return the status attribute.
+
+        Status is a dictionary containing:
+        frame id, timestamp, supervisor info, battery voltage, pm state, rssi, nb 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):
     """