### Launch Mapping Example

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

Command to start the simple mapper launch file for the Crazyflie.

```bash
ros2 launch crazyflie_examples multiranger_simple_mapper_launch.py
```

--------------------------------

### Resource File Installation using CMake

Source: https://github.com/imrclab/crazyswarm2/blob/main/crazyflie_examples/CMakeLists.txt

Installs configuration, data, and launch files for the CrazySwarm2 project. The install command copies the 'config', 'data', and 'launch' directories to the share directory of the installed package.

```cmake
# Install launch and config files.
install(
  DIRECTORY
  config
  data
  launch
  DESTINATION share/${PROJECT_NAME}/
)
```

--------------------------------

### Utilize TimeHelper for Timing in Python

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

This Python script demonstrates the use of the `TimeHelper` class from `crazyflie_py` for consistent timing across real hardware and simulation. It shows how to get the current time, sleep for specific durations, and implement rate-controlled loops for consistent command streaming. The script includes an example of oscillating movement.

```python
from crazyflie_py import Crazyswarm

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs

    # Get current time in seconds
    start_time = timeHelper.time()
    print(f"Start time: {start_time}")

    # Sleep for fixed duration
    timeHelper.sleep(2.0)  # Sleep for 2 seconds

    allcfs.takeoff(targetHeight=1.0, duration=2.0)
    timeHelper.sleep(2.5)

    # Rate-controlled loop (useful for streaming commands)
    RATE_HZ = 50
    duration = 3.0
    loop_start = timeHelper.time()

    while not timeHelper.isShutdown():
        elapsed = timeHelper.time() - loop_start
        if elapsed > duration:
            break

        # Oscillating position
        import math
        x_offset = 0.3 * math.sin(2 * math.pi * elapsed / 2.0)

        for cf in allcfs.crazyflies:
            cf.cmdPosition([cf.initialPosition[0] + x_offset,
                           cf.initialPosition[1],
                           1.0], yaw=0)

        timeHelper.sleepForRate(RATE_HZ)  # Maintain consistent rate

    # Notify and land
    for cf in allcfs.crazyflies:
        cf.notifySetpointsStop()

    allcfs.land(targetHeight=0.04, duration=2.0)
    timeHelper.sleep(3.0)

    # Check for shutdown (Ctrl+C)
    if timeHelper.isShutdown():
        print("Shutdown requested")

if __name__ == '__main__':
    main()

```

--------------------------------

### Install Navigation2 Bringup Package

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

Command to install the Navigation2 bringup package via apt. This is a prerequisite for using the generated maps for autonomous navigation.

```bash
sudo apt-get install ros-DISTRO-nav2-bringup
```

--------------------------------

### Configure setup.py for Data Files

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/installation.md

Snippet to include in the setup.py file to ensure launch and config files are correctly installed into the package share directory.

```python
(os.path.join('share', package_name, 'launch'), glob('launch/*')),
(os.path.join('share', package_name, 'config'), glob('config/*'))
```

--------------------------------

### Install Teleoperation Package

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

Installs the ROS 2 teleop_twist_keyboard package required for controlling the Crazyflie via keyboard input.

```bash
sudo apt-get install ros-DISTRO-teleop-twist-keyboard
```

--------------------------------

### Launch Crazyswarm2 ROS 2 Server with Bash

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

This section provides bash commands for launching the Crazyswarm2 ROS 2 server. It covers setting up the environment by sourcing the ROS 2 workspace and configuring ROS domain IDs. Examples include launching with different backends (simulation or hardware), disabling motion capture, running example scripts, and using `sim_time` for deterministic simulation.

```bash
# Source ROS 2 workspace
source install/local_setup.bash

# Set ROS domain for isolated testing
export ROS_LOCALHOST_ONLY=1
export ROS_DOMAIN_ID=42

# Launch server for physical robots with motion capture
ros2 launch crazyflie launch.py

# Launch server with simulation backend
ros2 launch crazyflie launch.py backend:=sim

# Launch without motion capture (for onboard positioning)
ros2 launch crazyflie launch.py mocap:=False

# Run example scripts against server
ros2 run crazyflie_examples hello_world

# Or use combined launch for simulation
ros2 launch crazyflie_examples launch.py script:=hello_world backend:=sim

# Run with sim_time for deterministic simulation
ros2 run crazyflie_examples hello_world --ros-args -p use_sim_time:=True

```

--------------------------------

### ROS 2 Workspace Setup

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/usage.md

Sources the ROS 2 workspace setup script. This is a prerequisite for using ROS 2 commands and tools in the current terminal session. It needs to be run in every new terminal.

```bash
. install/local_setup.bash
```

--------------------------------

### Execute CrazySwarm2 Launch Script

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

This command sequence demonstrates how to source the ROS 2 setup file and then launch the CrazySwarm2 mapping process using the previously defined Python launch script. Ensure that the 'setup.bash' script is sourced before executing the launch command.

```bash
source install/setup.bash
ros2 launch crazyflie_examples multiranger_mapping_launch.py
```

--------------------------------

### Launch Motion Capture Helper (Bash)

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/configuration.md

Launches the mocap_helper node within the CrazySwarm ROS package, which assists in determining marker coordinates for motion capture systems. Requires sourcing the ROS workspace setup.

```bash
source ros_ws/devel/setup.bash
roslaunch crazyswarm mocap_helper.launch
```

--------------------------------

### Start Trajectory Service

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Starts a predefined trajectory for a specified robot.

```APIDOC
## POST /cf231/start_trajectory

### Description
Starts a predefined trajectory for the specified robot.

### Method
ROS 2 Service Call

### Endpoint
`/cf231/start_trajectory`

### Parameters
#### Request Body
- **group_mask** (integer) - Required - Mask for the group of robots to control.
- **trajectory_id** (integer) - Required - The ID of the trajectory to start.
- **timescale** (float) - Optional - A factor to scale the playback speed of the trajectory.
- **reversed** (boolean) - Optional - If true, the trajectory will be played in reverse.
- **relative** (boolean) - Optional - If true, the trajectory is relative to the robot's current position.

### Request Example
```bash
ros2 service call /cf231/start_trajectory crazyflie_interfaces/srv/StartTrajectory \ 
  "{group_mask: 0, trajectory_id: 0, timescale: 1.0, reversed: false, relative: true}"
```

### Response
#### Success Response (200)
This service does not return a response body upon success.
```

--------------------------------

### Launch RViz2 for Visualization

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

This command launches the RViz2 visualization tool, which is essential for observing the mapping process and drone status in real-time. It requires the ROS 2 distribution's setup script to be sourced.

```bash
source /opt/ros/DISTRO/setup.bash
rviz2
```

--------------------------------

### Launch Teleoperation and Control

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

Commands to launch the Crazyflie server with velocity multiplexing and start the keyboard teleoperation node.

```bash
ros2 launch crazyflie_examples keyboard_velmux_launch.py
ros2 run teleop_twist_keyboard teleop_twist_keyboard
```

--------------------------------

### Configure Crazyflie Robots with YAML

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

This is an example of a `crazyflies.yaml` configuration file used to define Crazyflie robot parameters. It specifies robot definitions, types, firmware parameters, and logging configurations. This file is essential for setting up the swarm and customizing individual robot behaviors.

```yaml

```

--------------------------------

### Debug C++ Backend with GDB

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/howto.md

Instructions for compiling the Crazyswarm2 server in debug mode and launching it with an xterm window for GDB debugging. Requires the xterm package installed on the host system.

```bash
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug
ros2 launch crazyflie launch.py debug:=True
```

--------------------------------

### CMake Project Setup and Dependency Declaration

Source: https://github.com/imrclab/crazyswarm2/blob/main/crazyflie_examples/CMakeLists.txt

Configures the CMake build system for the CrazySwarm2 project. It specifies the minimum required CMake version, sets the project name, and finds necessary dependencies like ament_cmake and ament_cmake_python.

```cmake
cmake_minimum_required(VERSION 3.8)
project(crazyflie_examples)

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
```

--------------------------------

### Set Object Tracking Mode

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/configuration.md

Configures the object tracking strategy. 'motionCapture' uses vendor-specific unique markers, while 'libobjecttracker' handles duplicated marker arrangements or single-marker setups using raw point clouds.

```yaml
object_tracking_type: "motionCapture"
```

```yaml
object_tracking_type: "libobjecttracker"
```

--------------------------------

### Manage Firmware Parameters

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Demonstrates reading and writing firmware parameters to configure controllers, estimators, and peripherals like the LED ring at runtime.

```python
from crazyflie_py import Crazyswarm

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs
    cf = allcfs.crazyflies[0]

    estimator = cf.getParam('stabilizer.estimator')
    controller = cf.getParam('stabilizer.controller')

    cf.setParam('commander.enHighLevel', 1)
    cf.setParam('stabilizer.controller', 2)
    cf.setParam('usd.logging', 1)

    allcfs.takeoff(targetHeight=1.0, duration=2.0)
    timeHelper.sleep(3.0)

    allcfs.setParam('ring.effect', 7)
    allcfs.setParam('ring.solidRed', 255)

    timeHelper.sleep(2.0)
    cf.setParam('usd.logging', 0)
    allcfs.land(targetHeight=0.04, duration=2.0)
    timeHelper.sleep(3.0)

if __name__ == '__main__':
    main()
```

--------------------------------

### Control Crazyflie via Command Line

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/howto.md

Demonstrates basic flight operations including rebooting, launching the server, setting parameters, and executing takeoff and landing services.

```bash
ros2 run crazyflie reboot --uri radio://0/80/2M/E7E7E7E706 --mode sysoff
ros2 launch crazyflie launch.py
ros2 param set crazyflie_server cf1.params.commander.enHighLevel 1
ros2 param set crazyflie_server cf1.params.stabilizer.estimator 2
ros2 service call cf1/takeoff crazyflie_interfaces/srv/Takeoff "{height: 0.5, duration: {sec: 2}}"
ros2 service call cf1/land crazyflie_interfaces/srv/Land "{height: 0.0, duration: {sec: 2}}"
```

--------------------------------

### Install Python Package with CMake

Source: https://github.com/imrclab/crazyswarm2/blob/main/crazyflie_sim/CMakeLists.txt

Installs the Python package for the project using ament_python_install_package. This command is essential for making the Python modules of the project available in the Python environment.

```cmake
cmake_minimum_required(VERSION 3.8)
project(crazyflie_sim)

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)

# Install Python modules
ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME})
```

--------------------------------

### Python Package Installation with CMake

Source: https://github.com/imrclab/crazyswarm2/blob/main/crazyflie_examples/CMakeLists.txt

Installs Python modules for the CrazySwarm2 project using CMake. It utilizes ament_python_install_package to deploy Python scripts to the appropriate directory within the package.

```cmake
# Install Python modules
ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME})
```

--------------------------------

### Initialize Crazyswarm and Access Robots

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Demonstrates how to initialize the Crazyswarm class to connect to hardware, access individual robots by index, name, or ID, and utilize the time helper for rate control.

```python
from crazyflie_py import Crazyswarm

def main():
    swarm = Crazyswarm()
    allcfs = swarm.allcfs
    cf = allcfs.crazyflies[0]
    cf_by_name = allcfs.crazyfliesByName['cf231']
    cf_by_id = allcfs.crazyfliesById[231]
    timeHelper = swarm.timeHelper
    joystick = swarm.input
    print(f"Connected to {len(allcfs.crazyflies)} Crazyflie(s)")
    print(f"Robot URIs: {[cf.uri for cf in allcfs.crazyflies]}")
    print(f"Initial positions: {[cf.initialPosition for cf in allcfs.crazyflies]}")

if __name__ == '__main__':
    main()
```

--------------------------------

### Execute Takeoff and Landing Maneuvers

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Shows how to perform asynchronous takeoff and landing commands for the entire swarm or individual robots. Uses timeHelper.sleep to manage command execution timing.

```python
from crazyflie_py import Crazyswarm

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs
    TAKEOFF_HEIGHT = 1.0
    TAKEOFF_DURATION = 2.5
    HOVER_DURATION = 5.0
    LAND_HEIGHT = 0.04
    LAND_DURATION = 2.5
    allcfs.takeoff(targetHeight=TAKEOFF_HEIGHT, duration=TAKEOFF_DURATION)
    timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION)
    allcfs.land(targetHeight=LAND_HEIGHT, duration=LAND_DURATION)
    timeHelper.sleep(LAND_DURATION + 1.0)
    cf = allcfs.crazyflies[0]
    cf.takeoff(targetHeight=0.5, duration=2.0)
    timeHelper.sleep(2.5)
    cf.land(targetHeight=0.04, duration=2.0)
    timeHelper.sleep(2.5)

if __name__ == '__main__':
    main()
```

--------------------------------

### Stream Full-State Setpoints in Python

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Shows how to stream position, velocity, acceleration, yaw, and angular velocity setpoints using cmdFullState. Includes the critical step of calling notifySetpointsStop to resume high-level command usage.

```python
from pathlib import Path
from crazyflie_py import Crazyswarm
from crazyflie_py.uav_trajectory import Trajectory
import numpy as np

def executeTrajectory(timeHelper, cf, trajpath, rate=100, offset=np.zeros(3)):
    """Execute trajectory using full-state streaming commands."""
    traj = Trajectory()
    traj.loadcsv(trajpath)

    start_time = timeHelper.time()
    while not timeHelper.isShutdown():
        t = timeHelper.time() - start_time
        if t > traj.duration:
            break

        # Evaluate trajectory at current time
        e = traj.eval(t)

        # Send full state command with position offset
        cf.cmdFullState(
            pos=e.pos + np.array(cf.initialPosition) + offset,
            vel=e.vel,
            acc=e.acc,
            yaw=e.yaw,
            omega=e.omega
        )

        timeHelper.sleepForRate(rate)

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    cf = swarm.allcfs.crazyflies[0]

    Z = 0.5
    RATE = 30.0  # Hz

    cf.takeoff(targetHeight=Z, duration=Z + 1.0)
    timeHelper.sleep(Z + 2.0)

    # Execute trajectory with streaming full-state commands
    executeTrajectory(
        timeHelper, cf,
        Path(__file__).parent / 'data/figure8.csv',
        rate=RATE,
        offset=np.array([0, 0, 0.5])
    )

    # CRITICAL: Notify that streaming commands are stopping
    cf.notifySetpointsStop(remainValidMillisecs=100)

    # Now safe to use high-level commands again
    cf.land(targetHeight=0.03, duration=Z + 1.0)
    timeHelper.sleep(Z + 2.0)

if __name__ == '__main__':
    main()
```

--------------------------------

### Configure Motion Capture System (YAML)

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/usage.md

This snippet shows how to configure the motion capture system type and hostname in the motion_capture.yaml file. It supports various systems like OptiTrack and Vicon.

```yaml
/motion_capture_tracking:
    ros__parameters:
        type: "optitrack"
        hostname: "optitrackPC"
```

--------------------------------

### Python Launch File for Crazyflie Navigation

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

This Python script defines a ROS 2 launch configuration for a Crazyflie. It includes nodes for the crazyflie server, velocity multiplexing, SLAM toolbox (for localization), and Nav2 for navigation. It also configures RVIZ for visualization. Key parameters for each component are set to enable autonomous navigation with a pre-defined map.

```python
map_name = 'map'

return LaunchDescription([

    IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('crazyflie'), 'launch'),
            '/launch.py']),
        launch_arguments={
            'backend': 'cflib',
            'gui': 'false',
            'teleop': 'false',
            'mocap': 'false',
            }.items()),
    Node(
        package='crazyflie',
        executable='vel_mux.py',
        name='vel_mux',
        output='screen',
        parameters=[{'hover_height': 0.3},
                    {'incoming_twist_topic': '/cmd_vel'},
                    {'robot_prefix': '/cf231'}]
    ),
    IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('slam_toolbox'), 'launch/online_async_launch.py')),
        launch_arguments={
            'slam_params_file': os.path.join(get_package_share_directory('crazyflie_examples'), 'config/slam_params.yaml'),
            'use_sim_time': 'False',
        }.items()
    ),
    IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('nav2_bringup'), 'launch/bringup_launch.py')),
        launch_arguments={
            'slam': 'False',
            'use_sim_time': 'False',
            'map': get_package_share_directory('crazyflie_examples') + '/data/' + map_name + '.yaml',
            'params_file': os.path.join(get_package_share_directory('crazyflie_examples'), 'config/nav2_params.yaml'),
            'autostart': 'True',
            'use_composition': 'True',
            'transform_publish_period': '0.02'
        }.items()
    ),
    IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_launch_dir, 'rviz_launch.py')),
        launch_arguments={
            'rviz_config': os.path.join(
                get_package_share_directory('nav2_bringup'), 'rviz', 'nav2_default_view.rviz')}.items())
])
```

--------------------------------

### Crazyflie Robot Configuration (YAML)

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/usage.md

Defines the configuration for individual Crazyflie robots, including their enablement status, unique radio URIs, initial positions, and types. This YAML structure allows for detailed setup of each drone in the swarm.

```yaml
robots:
    cf231:
        enabled: true
        uri: radio://0/80/2M/E7E7E7E7E7
        initial_position: [0, 0, 0]
        type: cf21  # see robot_types

    cf5:
        enabled: false
        uri: radio://0/80/2M/E7E7E7E705
        initial_position: [0, -0.5, 0]
        type: cf21  # see robot_types
```

--------------------------------

### Configure Mapping Parameters

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

YAML configuration for enabling odometry, scan logging, and Kalman filter settings required for mapping.

```yaml
firmware_logging:
  enabled: true
  default_topics:
    odom:
      frequency: 10 # Hz
    scan:
      frequency: 10 # Hz

firmware_params:
  stabilizer:
    estimator: 2 # 1: complementary, 2: kalman
    controller: 1 # 1: PID, 2: mellinger

reference_frame: cf231/odom
```

--------------------------------

### POST /trajectory/start

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Initiates the execution of a previously uploaded trajectory on the swarm.

```APIDOC
## POST /trajectory/start

### Description
Starts the execution of a trajectory stored on the drone with configurable speed and direction.

### Method
POST

### Endpoint
/trajectory/start

### Parameters
#### Request Body
- **trajectoryId** (int) - Required - ID of the trajectory to execute.
- **timescale** (float) - Required - Speed multiplier (1.0 = normal).
- **reverse** (boolean) - Required - Whether to play the trajectory in reverse.
- **relative** (boolean) - Required - Whether the trajectory is relative to current position.

### Request Example
{
  "trajectoryId": 0,
  "timescale": 1.0,
  "reverse": false,
  "relative": true
}

### Response
#### Success Response (200)
- **status** (string) - Execution started.
```

--------------------------------

### Implement FullState Publisher in Python

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/howto/howto.md

This snippet demonstrates how to initialize a ROS publisher for the FullState message type and implement a method to update and publish state data. It uses a pre-allocated message object to minimize heap allocation overhead during high-frequency updates.

```python
from crazyswarm.msg import FullState
import rospy

class Crazyflie:
    def __init__(self, prefix):
        self.cmdFullStatePublisher = rospy.Publisher(prefix + "/cmd_full_state", FullState, queue_size=1)
        self.cmdFullStateMsg = FullState()
        self.cmdFullStateMsg.header.seq = 0
        self.cmdFullStateMsg.header.frame_id = "/world"

    def cmdFullState(self, pos, vel, acc, yaw, omega):
        self.cmdFullStateMsg.header.stamp = rospy.Time.now()
        self.cmdFullStateMsg.header.seq += 1
        self.cmdFullStateMsg.pose.position.x = pos[0]
        self.cmdFullStateMsg.twist.angular.z = omega[2]
        self.cmdFullStatePublisher.publish(self.cmdFullStateMsg)
```

--------------------------------

### Launch Teleop Twist Keyboard for Mapping

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

Commands to source the ROS2 environment and launch the teleop_twist_keyboard node. This allows manual control of the Crazyflie to perform room mapping.

```bash
source /opt/ros/DISTRO/setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard
```

--------------------------------

### Launch Crazyflie Mapping with ROS 2

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

This Python script defines a ROS 2 launch sequence for the CrazySwarm2 project. It includes nodes for initializing the crazyflie server using cflib, setting up a velocity multiplexer for drone control, and launching the SLAM toolbox for mapping. Dependencies include the 'crazyflie' and 'slam_toolbox' ROS 2 packages.

```python
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_source import PythonLaunchDescriptionSource
import os
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    return LaunchDescription([
        IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('crazyflie'), 'launch'),
            '/launch.py'])),
        launch_arguments={
            'backend': 'cflib',
            'gui': 'false',
            'teleop': 'false',
            'mocap': 'false',
            }.items()),
        Node(
            package='crazyflie',
            executable='vel_mux.py',
            name='vel_mux',
            output='screen',
            parameters=[{'hover_height': 0.3},
                        {'incoming_twist_topic': '/cmd_vel'},
                        {'robot_prefix': '/cf231'}]
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(get_package_share_directory('slam_toolbox'), 'launch/online_async_launch.py')),
            launch_arguments={
                'slam_params_file': os.path.join(get_package_share_directory('crazyflie_examples'), 'config/slam_params.yaml'),
                'use_sim_time': 'False',
            }.items()
        ),
    ])
```

--------------------------------

### Initialize ROS 2 Package for Crazyswarm

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/installation.md

Commands to create a new Python-based ROS 2 package within a workspace. This sets up the directory structure and the basic package configuration.

```bash
mkdir -p ros2_ws/src
cd ros2_ws/src
ros2 pkg create --build-type ament_python --license MIT --node-name hello_world crazyflie_test
```

--------------------------------

### Navigate Robots with GoTo Trajectories

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Demonstrates polynomial trajectory planning using the goTo method. Supports both absolute coordinates and relative offsets for individual or broadcast swarm movement.

```python
from crazyflie_py import Crazyswarm
import numpy as np

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs
    Z = 1.0
    allcfs.takeoff(targetHeight=Z, duration=2.0)
    timeHelper.sleep(2.5)
    for cf in allcfs.crazyflies:
        goal = np.array(cf.initialPosition) + np.array([0, 0, Z])
        cf.goTo(goal=goal, yaw=0, duration=2.0, relative=False)
    timeHelper.sleep(2.5)
    allcfs.goTo(goal=[1.0, 0, 0], yaw=0, duration=3.0)
    timeHelper.sleep(3.5)
    cf = allcfs.crazyflies[0]
    cf.goTo(goal=[0.5, 0.5, 1.0], yaw=np.pi/4, duration=3.0, relative=False)
    timeHelper.sleep(3.5)
    cf.goTo(goal=[0.2, 0, 0], yaw=0, duration=1.5, relative=True)
    timeHelper.sleep(2.0)
    allcfs.land(targetHeight=0.04, duration=2.0)
    timeHelper.sleep(2.5)

if __name__ == '__main__':
    main()
```

--------------------------------

### Execute ROS 2 Services for Drone Control

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Demonstrates how to invoke ROS 2 services to control Crazyflie drones, including takeoff, landing, emergency stops, and trajectory execution.

```bash
# Takeoff service
ros2 service call /cf231/takeoff crazyflie_interfaces/srv/Takeoff \
  "{group_mask: 0, height: 1.0, duration: {sec: 2, nanosec: 0}}"

# GoTo service
ros2 service call /cf231/go_to crazyflie_interfaces/srv/GoTo \
  "{group_mask: 0, relative: false, goal: {x: 0.5, y: 0.5, z: 1.0}, yaw: 0.0, duration: {sec: 3, nanosec: 0}}"

# Emergency stop
ros2 service call /all/emergency std_srvs/srv/Empty

# Arm brushless motors
ros2 service call /cf231/arm crazyflie_interfaces/srv/Arm "{arm: true}"
```

--------------------------------

### Crazyflie C++ Wrapper for Sending Setpoints

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/howto/howto.md

Demonstrates how to integrate the CRTP setpoint sending functionality into the `crazyflie_cpp` library by adding a new method to the `Crazyflie` class.

```APIDOC
## `crazyflie_cpp` Wrapper for Sending Setpoints

### Description
This section describes how to add a method to the `Crazyflie` class in the `crazyflie_cpp` library to simplify sending full state setpoint requests. The wrapper method abstracts the underlying CRTP packet construction.

### `Crazyflie` Class Method

#### Declaration (in `crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h`)
```c++
void sendFullStateSetpoint(
  float x, float y, float z,
  float vx, float vy, float vz,
  float ax, float ay, float az,
  float qx, float qy, float qz, float qw,
  float rollRate, float pitchRate, float yawRate);
```

### Implementation (in `crazyflie_cpp/src/Crazyflie.cpp`)

#### Method
`void Crazyflie::sendFullStateSetpoint(...)`

#### Description
This method constructs a `crtpFullStateSetpointRequest` object using the provided floating-point parameters and then sends this packet over the radio using the `sendPacket` method.

#### Code Example
```c++
void Crazyflie::sendFullStateSetpoint(
        float x, float y, float z,
        float vx, float vy, float vz,
        float ax, float ay, float az,
        float qx, float qy, float qz, float qw,
        float rollRate, float pitchRate, float yawRate)
{
  crtpFullStateSetpointRequest request(
        x, y, z,
        vx, vy, vz,
        ax, ay, az,
        qx, qy, qz, qw,
        rollRate, pitchRate, yawRate);
  sendPacket(request);
}
```

### Usage Notes
- The `sendPacket` method is a templated overload that automatically handles the casting of the struct pointer to `uint8_t *`.
- This wrapper provides a high-level interface, abstracting the details of CRTP packet formatting and data compression.
```

--------------------------------

### Upload and Execute Polynomial Trajectories in Python

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Demonstrates loading a trajectory from a CSV file, uploading it to the Crazyflie swarm, and executing it with configurable timescale and direction parameters.

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

    # Load trajectory from CSV file
    traj = Trajectory()
    traj.loadcsv(Path(__file__).parent / 'data/figure8.csv')
    print(f"Trajectory duration: {traj.duration}s, pieces: {traj.n_pieces()}")

    # Upload trajectory to each Crazyflie with ID 0
    TRAJECTORY_ID = 0
    for cf in allcfs.crazyflies:
        cf.uploadTrajectory(trajectoryId=TRAJECTORY_ID, pieceOffset=0, trajectory=traj)

    allcfs.takeoff(targetHeight=1.0, duration=2.0)
    timeHelper.sleep(2.5)

    # Navigate to starting positions
    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)

    # Execute trajectory (broadcast to all)
    TIMESCALE = 1.0  # 1.0 = normal speed, 2.0 = half speed
    allcfs.startTrajectory(
        trajectoryId=TRAJECTORY_ID,
        timescale=TIMESCALE,
        reverse=False,
        relative=True  # trajectory relative to current position
    )
    timeHelper.sleep(traj.duration * TIMESCALE + 1.0)

    # Execute in reverse
    allcfs.startTrajectory(trajectoryId=TRAJECTORY_ID, timescale=TIMESCALE, reverse=True, relative=True)
    timeHelper.sleep(traj.duration * TIMESCALE + 1.0)

    allcfs.land(targetHeight=0.06, duration=2.0)
    timeHelper.sleep(3.0)

if __name__ == '__main__':
    main()
```

--------------------------------

### Implement Crazyflie::sendFullStateSetpoint Method (C++)

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/howto/howto.md

Provides the C++ implementation for the `sendFullStateSetpoint` method within the `Crazyflie` class. This implementation constructs a `crtpFullStateSetpointRequest` object using the provided parameters and then sends this packet over the radio using the `sendPacket` method.

```c++
void Crazyflie::sendFullStateSetpoint(
        float x, float y, float z,
        float vx, float vy, float vz,
        float ax, float ay, float az,
        float qx, float qy, float qz, float qw,
        float rollRate, float pitchRate, float yawRate)
{
  crtpFullStateSetpointRequest request(
        x, y, z,
        vx, vy, vz,
        ax, ay, az,
        qx, qy, qz, qw,
        rollRate, pitchRate, yawRate);
  sendPacket(request);
}
```

--------------------------------

### Configure Firmware Logging for RVIZ2

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/tutorials.md

YAML configuration snippet to enable pose logging in the Crazyflie firmware for visualization in RVIZ2.

```yaml
firmware_logging:
    enabled: true
    default_topics:
    pose:
        frequency: 10 # Hz
```

--------------------------------

### GoTo Service

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Commands a robot to move to a specific position.

```APIDOC
## POST /cf231/go_to

### Description
Commands the specified robot to move to a target position.

### Method
ROS 2 Service Call

### Endpoint
`/cf231/go_to`

### Parameters
#### Request Body
- **group_mask** (integer) - Required - Mask for the group of robots to control.
- **relative** (boolean) - Required - If true, the goal is relative to the current position.
- **goal** (object) - Required - The target position.
  - **x** (float) - Target x-coordinate.
  - **y** (float) - Target y-coordinate.
  - **z** (float) - Target z-coordinate.
- **yaw** (float) - Optional - Target yaw angle in radians.
- **duration** (object) - Required - The duration of the movement.
  - **sec** (integer) - Seconds component of the duration.
  - **nanosec** (integer) - Nanoseconds component of the duration.

### Request Example
```bash
ros2 service call /cf231/go_to crazyflie_interfaces/srv/GoTo \ 
  "{group_mask: 0, relative: false, goal: {x: 0.5, y: 0.5, z: 1.0}, yaw: 0.0, duration: {sec: 3, nanosec: 0}}"
```

### Response
#### Success Response (200)
This service does not return a response body upon success.
```

--------------------------------

### POST /control/fullState

Source: https://context7.com/imrclab/crazyswarm2/llms.txt

Streams high-frequency full-state setpoints to the drone for aggressive flight control.

```APIDOC
## POST /control/fullState

### Description
Sends position, velocity, acceleration, yaw, and angular velocity setpoints. Requires a call to notifySetpointsStop when finished.

### Method
POST

### Endpoint
/control/fullState

### Parameters
#### Request Body
- **pos** (array) - Required - [x, y, z] position.
- **vel** (array) - Required - [vx, vy, vz] velocity.
- **acc** (array) - Required - [ax, ay, az] acceleration.
- **yaw** (float) - Required - Yaw angle.
- **omega** (array) - Required - [wx, wy, wz] angular velocity.

### Request Example
{
  "pos": [0, 0, 1.0],
  "vel": [0, 0, 0],
  "acc": [0, 0, 0],
  "yaw": 0,
  "omega": [0, 0, 0]
}

### Response
#### Success Response (200)
- **status** (string) - Setpoint received.
```

--------------------------------

### Enable Pose Estimation Logging (YAML)

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/usage.md

This YAML configuration enables firmware logging for pose estimation, setting the frequency for pose updates. This is useful for visualizing Crazyflie poses in tools like rviz.

```yaml
firmware_logging:
    enabled: true
    default_topics:
        pose:
        frequency: 10 # Hz
```

--------------------------------

### Global Crazyflie Configuration (YAML)

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/usage.md

Configures parameters and logging for all connected Crazyflies globally. This includes firmware logging settings for default and custom topics, and firmware parameters for commander and stabilizer.

```yaml
all:
    firmware_logging:
        enabled: false
        default_topics:
            pose:
            frequency: 10 # Hz
        #custom_topics:
        #  topic_name1:
        #    frequency: 10 # Hz
        #    vars: ["stateEstimateZ.x", "stateEstimateZ.y", "stateEstimateZ.z", "pm.vbat"]
        #  topic_name2:
        #    frequency: 1 # Hz
        #    vars: ["stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"]
    firmware_params:
        commander:
            enHighLevel: 1
        stabilizer:
            estimator: 2 # 1: complementary, 2: kalman
            controller: 2 # 1: PID, 2: mellinger
```

--------------------------------

### Debug Firmware Bindings with LLDB on MacOS

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/internals.md

Commands to launch a Python test script in LLDB and set a breakpoint on a firmware function. LLDB handles pending breakpoints automatically upon library loading.

```bash
lldb -- python -m pytest test_highLevel.py
(lldb) br set -f planner.c -n plan_takeoff
(lldb) run
```

--------------------------------

### Define ROS 2 Launch File for Crazyswarm

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/installation.md

A Python launch file that includes the base Crazyswarm launch description and passes custom configuration file paths for crazyflies and motion capture.

```python
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

package_name = 'crazyflie_test'

def generate_launch_description():
    crazyflies_yaml_path = os.path.join(get_package_share_directory(package_name), 'config', 'crazyflies.yaml')
    motion_capture_yaml_path = os.path.join(get_package_share_directory(package_name), 'config', 'motion_capture.yaml')

    return LaunchDescription([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('crazyflie'), 'launch'), '/launch.py']),
            launch_arguments={'crazyflies_yaml_file': crazyflies_yaml_path, 'motion_capture_yaml_file': motion_capture_yaml_path}.items(),
        ),
    ])
```

--------------------------------

### Add sendFullStateSetpoint Method to Crazyflie Class (C++)

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/howto/howto.md

Demonstrates adding a new public method `sendFullStateSetpoint` to the `Crazyflie` class in `crazyflie_cpp`. This method serves as a wrapper to simplify sending full state setpoint packets by internally constructing the `crtpFullStateSetpointRequest` object and calling the `sendPacket` method.

```c++
void sendFullStateSetpoint(
  float x, float y, float z,
  float vx, float vy, float vz,
  float ax, float ay, float az,
  float qx, float qy, float qz, float qw,
  float rollRate, float pitchRate, float yawRate);
```

--------------------------------

### Alternative Motion Capture Configuration (YAML)

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/usage.md

This YAML snippet shows an alternative configuration for motion capture, specifically setting the tracking mode to 'vendor'. This is used when opting for vendor-specific software.

```yaml
robot_types:
    cf21:
        motion_capture:
            tracking: "vendor"
```

--------------------------------

### Manage Runtime Logging Blocks

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/howto.md

Shows how to dynamically add or remove logging blocks at runtime using ROS 2 services. This functionality is specific to the cflib backend.

```bash
ros2 launch crazyflie launch.py backend:=cflib
ros2 service call /cf2/add_logging crazyflie_interfaces/srv/AddLogging "{topic_name: 'topic_test', frequency: 10, vars: ['stateEstimate.x','stateEstimate.y','stateEstimate.z']}"
ros2 service call /cf2/add_logging crazyflie_interfaces/srv/AddLogging "{topic_name: 'pose', frequency: 10}"
ros2 service call /cf2/remove_logging crazyflie_interfaces/srv/RemoveLogging "{topic_name: 'topic_test'}"
ros2 service call /cf2/remove_logging crazyflie_interfaces/srv/RemoveLogging "{topic_name: 'pose'}"
```

--------------------------------

### Enable Firmware Collision Avoidance

Source: https://github.com/imrclab/crazyswarm2/blob/main/docs2/howto.md

Configures the Buffered Voronoi Cell collision avoidance algorithm either via YAML configuration or programmatically through the Python API.

```yaml
all:
    firmware_params:
        colAv:
            enable: 1
```

```python
swarm = Crazyswarm()
allcfs = swarm.allcfs
allcfs.setParam("colAv.enable", 1)
```