added callback and getter for status topic to python layer ; added in…

…finite_flight to crazyflie_examples

crazyflie_examples/crazyflie_examples/infinite_flight.py

@@ -12,60 +12,88 @@ def main():
  swarm = Crazyswarm()
  timeHelper = swarm.timeHelper
  allcfs = swarm.allcfs
- # time.sleep(2)
- # for cf in allcfs.crazyflies:
- # status = cf.get_status()
- # print(status)
-
-
-
- # from time import sleep
- # while True:
- # status, counter = cf.get_status()
- # print(status)
- # sleep(0.1)
- # id, sec, nsec, s, b, p, r, rxb, txb, rxu, txu = status.values()
- # print(f"{cf.prefix} : {id}, {sec}, {nsec}, {s}, {b}, {p}, {r}, {rxb}, {rxu}, {txb}, {txu} ")
 
  traj1 = Trajectory()
  traj1.loadcsv(Path(__file__).parent / 'data/figure8.csv')
 
+
+
  TIMESCALE = 1.0
  for cf in allcfs.crazyflies:
  cf.uploadTrajectory(0, 0, traj1)
-
- 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.startTrajectory(0, timescale=TIMESCALE)
- timeHelper.sleep(traj1.duration * TIMESCALE + 2.0)
-
-
- timeHelper.sleep(2.5)
- status = allcfs.status() #NB what happens if we have multiple CFs ? 
- print(f"battery {status.battery}")
-
- ###second figure8
- allcfs.startTrajectory(0, TIMESCALE)
- timeHelper.sleep(traj1.duration * TIMESCALE + 2.0)
-
-
- allcfs.land(targetHeight=0.06, duration=2.0)
- timeHelper.sleep(3)
- status = allcfs.status()
- while status.pm_state != 1:
- print("Not charging, retrying")
- allcfs.takeoff(targetHeight=0.5, duration=1.0)
- timeHelper.sleep(1.5)
- allcfs.land(targetHeight=0.06, duration=1.0)
- timeHelper.sleep(2)
-
- print("charging !")
- timeHelper.sleep(3.0)
+ 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__':

crazyflie_py/crazyflie_py/crazyflie.py

@@ -705,20 +705,19 @@ def listener_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}
- self.node.get_logger().info(f' Callbacl 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'get_status() was called {self.status}')
+ # self.node.get_logger().info(f'Crazyflie.get_status() was called {self.status}')
  return self.status
 
 
@@ -998,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 listener_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