Add bezier curve support with cflib (python) backend

crazyflie_examples/crazyflie_examples/data/figure8_bezier.json

@@ -0,0 +1,25 @@
+{
+  "control_points": [
+    [
+      [0, 0, 1, 1],
+      [0, 0, 0.9, 0],
+      [0, 0, 0, 0]
+    ],
+    [
+      [1, 1, 0.5, 0],
+      [0, -0.9, -0.5, 0],
+      [0, 0, 0, 0]
+    ],
+    [
+      [0, -0.5, -1, -1],
+      [0, 0.5, 0.9, 0],
+      [0, 0, 0, 0]
+    ],
+    [
+      [-1, -1, 0, 0],
+      [0, -0.9, 0, 0],
+      [0, 0, 0, 0]
+    ]
+  ],
+  "parameters": [2.0, 2.0, 2.0, 2.0]
+}

crazyflie_examples/crazyflie_examples/figure8_bezier.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+
+import numpy as np
+from pathlib import Path
+
+from crazyflie_py import *
+from crazyflie_py.bezier_trajectory import BezierTrajectory
+
+
+def main():
+
+    print(
+        "WARNING: running a script involving Bezier curves. Currently only cflib backend is supported. Please run the server with 'ros2 launch crazyflie launch.py backend:=cflib'."
+    )
+
+    swarm = Crazyswarm()
+    timeHelper = swarm.timeHelper
+    allcfs = swarm.allcfs
+
+    traj = BezierTrajectory.from_json(
+        Path(__file__).parent / "data/figure8_bezier.json"
+    )
+
+    TRIALS = 1
+    TIMESCALE = 1.0
+    for i in range(TRIALS):
+        for cf in allcfs.crazyflies:
+            cf.uploadBezierTrajectory(0, 0, trajectory=traj)
+
+        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, 4.0)
+        timeHelper.sleep(4.5)
+
+        allcfs.startTrajectory(0, timescale=TIMESCALE)
+        timeHelper.sleep(traj.total_time * TIMESCALE + 2.0)
+
+        timeHelper.sleep(4.5)
+        allcfs.land(targetHeight=0.0, duration=2.0)
+        timeHelper.sleep(3.0)
+
+
+if __name__ == "__main__":
+    main()

crazyflie_examples/setup.cfg

@@ -3,8 +3,9 @@ console_scripts =
     hello_world = crazyflie_examples.hello_world:main
     nice_hover = crazyflie_examples.nice_hover:main
     figure8 = crazyflie_examples.figure8:main
+    figure8_bezier = crazyflie_examples.figure8_bezier:main
     multi_trajectory = crazyflie_examples.multi_trajectory:main
     cmd_full_state = crazyflie_examples.cmd_full_state:main
     set_param = crazyflie_examples.set_param:main
     hello_world4 = crazyflie_examples.hello_world4:main
-    drone_drag = crazyflie_examples.drones_drag.drones_in_drag.Strut.run:main
+    drone_drag = crazyflie_examples.drones_drag.drones_in_drag.Strut.run:main

crazyflie_interfaces/CMakeLists.txt

@@ -24,6 +24,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/LogBlock.msg"
   "msg/Position.msg"
   "msg/TrajectoryPolynomialPiece.msg"
+  "msg/TrajectoryBezierPiece.msg"
   "msg/VelocityWorld.msg"
   "srv/GoTo.srv"
   "srv/Land.srv"
@@ -34,6 +35,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/Takeoff.srv"
   "srv/UpdateParams.srv"
   "srv/UploadTrajectory.srv"
+  "srv/UploadBezierTrajectory.srv"
   "srv/RemoveLogging.srv"
   "srv/AddLogging.srv"
   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs 

crazyflie_interfaces/msg/TrajectoryBezierPiece.msg

@@ -0,0 +1,5 @@
+float32[] bezier_control_pts_x
+float32[] bezier_control_pts_y
+float32[] bezier_control_pts_z
+float32[] bezier_control_pts_yaw
+builtin_interfaces/Duration duration

crazyflie_interfaces/srv/UploadBezierTrajectory.srv

@@ -0,0 +1,4 @@
+uint8 trajectory_id
+uint32 piece_offset
+TrajectoryBezierPiece[] pieces
+---