Add bezier curve support with cflib (python) backend

crazyflie/config/motion_capture.yaml

@@ -1,7 +1,7 @@
 /motion_capture_tracking:
   ros__parameters:
     type: "vicon"
-    hostname: "169.254.217.218"
+    hostname: "169.254.122.233"
     # mode: "libRigidBodyTracker" # one of motionCapture,libRigidBodyTracker
     # mode: "libRigidBodyTracker"
     mode: "motionCapture"

crazyflie/scripts/crazyflie_server.py

@@ -19,12 +19,20 @@
 import cflib.crtp
 from cflib.crazyflie.swarm import CachedCfFactory
 from cflib.crazyflie.swarm import Swarm
-from cflib.crazyflie.log import LogConfig
 from cflib.crazyflie.mem import MemoryElement
+from cflib.crazyflie.mem import CompressedSegment
+from cflib.crazyflie.mem import CompressedStart
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.high_level_commander import HighLevelCommander
 from cflib.crazyflie.mem import Poly4D
 
 from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging
-from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop
+from crazyflie_interfaces.srv import (
+    UploadTrajectory,
+    StartTrajectory,
+    NotifySetpointsStop,
+    UploadBezierTrajectory,
+)
 from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType
 from crazyflie_interfaces.msg import Status, Hover, LogDataGeneric, FullState
 from motion_capture_tracking_interfaces.msg import NamedPoseArray
@@ -258,9 +266,14 @@ def __init__(self):
                     self._upload_trajectory_callback, uri=uri)
             )
             self.create_service(
-                NotifySetpointsStop, name +
-                "/notify_setpoints_stop", partial(
-                    self._notify_setpoints_stop_callback, uri=uri)
+                UploadBezierTrajectory,
+                name + "/upload_bezier_trajectory",
+                partial(self._upload_bezier_trajectory_callback, uri=uri),
+            )
+            self.create_service(
+                NotifySetpointsStop,
+                name + "/notify_setpoints_stop",
+                partial(self._notify_setpoints_stop_callback, uri=uri),
             )
             self.create_subscription(
                 Twist, name +
@@ -761,8 +774,6 @@ def _takeoff_callback(self, request, response, uri="all"):
             a certain height in high level commander
         """
 
-        print("call1 ", uri)
-
         duration = float(request.duration.sec) + \
             float(request.duration.nanosec / 1e9)
         self.get_logger().info(
@@ -866,17 +877,17 @@ def _upload_trajectory_callback(self, request, response, uri="all"):
 
         id = request.trajectory_id
         offset = request.piece_offset
-        lenght = len(request.pieces)
+        length = len(request.pieces)
         total_duration = 0
-        self.get_logger().info("[%s] upload_trajectory(id=%d,offset=%d, lenght=%d)" % (
+        self.get_logger().info("[%s] upload_trajectory(id=%d,offset=%d, length=%d)" % (
             self.cf_dict[uri],
             id,
             offset,
-            lenght,
+            length,
         ))
 
         trajectory = []
-        for i in range(lenght):
+        for i in range(length):
             piece = request.pieces[i]
             px = Poly4D.Poly(piece.poly_x)
             py = Poly4D.Poly(piece.poly_y)
@@ -891,7 +902,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"):
             upload_success_all = True
             for link_uri in self.uris:
                 trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems(
-                    MemoryElement.TYPE_TRAJ)[id]
+                    MemoryElement.TYPE_TRAJ)[0]
                 trajectory_mem.trajectory = trajectory
                 upload_result = trajectory_mem.write_data_sync()
                 if not upload_result:
@@ -905,15 +916,90 @@ def _upload_trajectory_callback(self, request, response, uri="all"):
                 return response
         else:
             trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems(
-                MemoryElement.TYPE_TRAJ)[id]
+                MemoryElement.TYPE_TRAJ)[0]
             trajectory_mem.trajectory = trajectory
             upload_result = trajectory_mem.write_data_sync()
             if not upload_result:
                 self.get_logger().info(f"[{self.cf_dict[uri]}] Upload failed")
                 response.success = False
                 return response
             self.swarm._cfs[uri].cf.high_level_commander.define_trajectory(
-                id, offset, len(trajectory))
+                id, offset, len(trajectory)
+            )
+
+        return response
+
+    def _upload_bezier_trajectory_callback(self, request, response, uri="all"):
+        id = request.trajectory_id
+        offset = request.piece_offset
+        num_pieces = len(request.pieces)
+        total_duration = 0
+
+        self.get_logger().info(
+            "upload_trajectory(id=%d, offset=%d, num_pieces=%d)"
+            % (id, offset, num_pieces)
+        )
+
+        trajectory = []
+        start_x = request.pieces[0].bezier_control_pts_x[0]
+        start_y = request.pieces[0].bezier_control_pts_y[0]
+        start_z = request.pieces[0].bezier_control_pts_z[0]
+        start_yaw = 0.0
+        trajectory.append(CompressedStart(start_x, start_y, start_z, start_yaw))
+        for i in range(num_pieces):
+            piece = request.pieces[i]
+            duration = float(piece.duration.sec) + float(piece.duration.nanosec) / 1e9
+            p = CompressedSegment(
+                duration,
+                piece.bezier_control_pts_x[1:],
+                piece.bezier_control_pts_y[1:],
+                piece.bezier_control_pts_z[1:],
+                piece.bezier_control_pts_yaw[1:],
+            )
+            trajectory.append(p)
+            total_duration += duration
+
+        if uri == "all":
+            upload_success_all = True
+            for link_uri in self.uris:
+                trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems(
+                    MemoryElement.TYPE_TRAJ
+                )[0]
+                trajectory_mem.trajectory = trajectory
+                upload_result = trajectory_mem.write_data_sync()
+                if not upload_result:
+                    self.get_logger().info(
+                        f"[{self.cf_dict[uri]}] Upload bezier trajectory failed"
+                    )
+                    upload_success_all = False
+                else:
+                    self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory(
+                        id,
+                        offset,
+                        len(trajectory),
+                        type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED,
+                    )
+            if upload_success_all is False:
+                response.success = False
+                return response
+        else:
+            trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems(
+                MemoryElement.TYPE_TRAJ
+            )[0]
+            trajectory_mem.trajectory = trajectory
+            upload_result = trajectory_mem.write_data_sync()
+            if not upload_result:
+                self.get_logger().info(
+                    f"[{self.cf_dict[uri]}] Upload bezier trajectory failed"
+                )
+                response.success = False
+                return response
+            self.swarm._cfs[uri].cf.high_level_commander.define_trajectory(
+                id,
+                offset,
+                len(trajectory),
+                type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED,
+            )
 
         return response
 

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,49 @@
+#!/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)
+            cf.uploadBezierTrajectory(1, 0, trajectory=traj) # upload multiple trajectory is possible
+
+        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)
+        # allcfs.startTrajectory(1, 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,6 +3,7 @@ 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
     group_mask = crazyflie_examples.group_mask:main
     multi_trajectory = crazyflie_examples.multi_trajectory:main
     cmd_full_state = crazyflie_examples.cmd_full_state:main

crazyflie_interfaces/CMakeLists.txt

@@ -27,6 +27,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Position.msg"
   "msg/Status.msg"
   "msg/TrajectoryPolynomialPiece.msg"
+  "msg/TrajectoryBezierPiece.msg"
   "msg/VelocityWorld.msg"
   "srv/GoTo.srv"
   "srv/Land.srv"
@@ -37,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/Takeoff.srv"
   "srv/UpdateParams.srv"
   "srv/UploadTrajectory.srv"
+  "srv/UploadBezierTrajectory.srv"
   "srv/RemoveLogging.srv"
   "srv/AddLogging.srv"
   "srv/Arm.srv"

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
+---