Showing preview only (1,854K chars total). Download the full file or copy to clipboard to get everything.
Repository: IMRCLab/crazyswarm2
Branch: main
Commit: 1b75fa88cbcd
Files: 360
Total size: 1.7 MB
Directory structure:
gitextract_puxxq204/
├── .github/
│ └── workflows/
│ ├── ci-docs2.yml
│ ├── ci-ros2-mac.yml
│ ├── ci-ros2-win.yml
│ ├── ci-ros2.yml
│ ├── systemtests.yml
│ └── systemtests_sim.yml
├── .gitignore
├── .gitmodules
├── LICENSE
├── README.md
├── crazyflie/
│ ├── CHANGELOG.rst
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── aideck_streamer.yaml
│ │ ├── config.rviz
│ │ ├── crazyflies.yaml
│ │ ├── motion_capture.yaml
│ │ ├── server.yaml
│ │ └── teleop.yaml
│ ├── launch/
│ │ ├── launch.py
│ │ ├── launch_teleop2.py
│ │ └── teleop_launch.py
│ ├── package.xml
│ ├── scripts/
│ │ ├── aideck_streamer.py
│ │ ├── cfmult.py
│ │ ├── chooser.py
│ │ ├── crazyflie_server.py
│ │ ├── flash.py
│ │ ├── gui.py
│ │ ├── simple_mapper_multiranger.py
│ │ └── vel_mux.py
│ ├── src/
│ │ ├── crazyflie_server.cpp
│ │ └── teleop.cpp
│ └── urdf/
│ ├── cf2_assembly.stl
│ ├── cf2_assembly_with_props.dae
│ └── crazyflie_description.urdf
├── crazyflie_examples/
│ ├── CHANGELOG.rst
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── nav2_params.yaml
│ │ ├── nav2_view.rviz
│ │ └── slam_params.yaml
│ ├── crazyflie_examples/
│ │ ├── __init__.py
│ │ ├── arming.py
│ │ ├── cmd_full_state.py
│ │ ├── data/
│ │ │ ├── figure8.csv
│ │ │ └── multi_trajectory/
│ │ │ ├── traj0.csv
│ │ │ └── traj1.csv
│ │ ├── figure8.py
│ │ ├── goto_unicast.py
│ │ ├── group_mask.py
│ │ ├── hello_world.py
│ │ ├── infinite_flight.py
│ │ ├── multi_trajectory.py
│ │ ├── nice_hover.py
│ │ ├── set_param.py
│ │ └── swap.py
│ ├── data/
│ │ ├── map.data
│ │ ├── map.pgm
│ │ ├── map.posegraph
│ │ └── map.yaml
│ ├── launch/
│ │ ├── keyboard_velmux_launch.py
│ │ ├── launch.py
│ │ ├── multiranger_mapping_launch.py
│ │ ├── multiranger_nav2_launch.py
│ │ └── multiranger_simple_mapper_launch.py
│ ├── package.xml
│ ├── resource/
│ │ └── crazyflie_examples
│ ├── setup.cfg
│ └── test/
│ ├── test_flake8.py
│ └── test_pep257.py
├── crazyflie_interfaces/
│ ├── CHANGELOG.rst
│ ├── CMakeLists.txt
│ ├── msg/
│ │ ├── ConnectionStatistics.msg
│ │ ├── ConnectionStatisticsArray.msg
│ │ ├── FullState.msg
│ │ ├── Hover.msg
│ │ ├── LogBlock.msg
│ │ ├── LogDataGeneric.msg
│ │ ├── Position.msg
│ │ ├── Status.msg
│ │ ├── TrajectoryPolynomialPiece.msg
│ │ └── VelocityWorld.msg
│ ├── package.xml
│ └── srv/
│ ├── AddLogging.srv
│ ├── Arm.srv
│ ├── GoTo.srv
│ ├── Land.srv
│ ├── NotifySetpointsStop.srv
│ ├── RemoveLogging.srv
│ ├── SetGroupMask.srv
│ ├── StartTrajectory.srv
│ ├── Stop.srv
│ ├── Takeoff.srv
│ ├── UpdateParams.srv
│ └── UploadTrajectory.srv
├── crazyflie_py/
│ ├── CHANGELOG.rst
│ ├── crazyflie_py/
│ │ ├── __init__.py
│ │ ├── crazyflie.py
│ │ ├── crazyswarm_py.py
│ │ ├── genericJoystick.py
│ │ ├── joystick.py
│ │ ├── keyboard.py
│ │ ├── linuxjsdev.py
│ │ ├── uav_trajectory.py
│ │ └── util.py
│ ├── package.xml
│ ├── resource/
│ │ └── crazyflie_py
│ ├── setup.cfg
│ ├── setup.py
│ └── test/
│ ├── test_flake8.py
│ └── test_pep257.py
├── crazyflie_sim/
│ ├── CHANGELOG.rst
│ ├── CMakeLists.txt
│ ├── crazyflie_sim/
│ │ ├── __init__.py
│ │ ├── backend/
│ │ │ ├── data/
│ │ │ │ ├── dynobench/
│ │ │ │ │ └── crazyflie2.yaml
│ │ │ │ ├── neuralswarm2/
│ │ │ │ │ ├── phi_G.pth
│ │ │ │ │ ├── phi_L.pth
│ │ │ │ │ ├── phi_S.pth
│ │ │ │ │ ├── rho_L.pth
│ │ │ │ │ └── rho_S.pth
│ │ │ │ └── pinocchio/
│ │ │ │ └── crazyflie2.urdf
│ │ │ ├── dynobench.py
│ │ │ ├── neuralswarm.py
│ │ │ ├── none.py
│ │ │ ├── np.py
│ │ │ └── pinocchio.py
│ │ ├── crazyflie_server.py
│ │ ├── crazyflie_sil.py
│ │ ├── sim_data_types.py
│ │ └── visualization/
│ │ ├── README.md
│ │ ├── blender.py
│ │ ├── data/
│ │ │ ├── README.md
│ │ │ └── model/
│ │ │ ├── cf.mtl
│ │ │ └── cf.obj
│ │ ├── pdf.py
│ │ ├── record_states.py
│ │ └── rviz.py
│ ├── package.xml
│ ├── resource/
│ │ └── crazyflie_sim
│ ├── setup.cfg
│ └── test/
│ ├── test_flake8.py
│ └── test_pep257.py
├── docs/
│ ├── .gitignore
│ ├── Makefile
│ ├── api.rst
│ ├── changelog.rst
│ ├── conf.py
│ ├── configuration.rst
│ ├── generate_install_deps_code.py
│ ├── generated/
│ │ └── .gitignore
│ ├── gettingstarted.rst
│ ├── glossary.rst
│ ├── hardware.rst
│ ├── howto/
│ │ ├── git_integration.rstinclude
│ │ ├── howto.rst
│ │ └── streaming_setpoint.rstinclude
│ ├── index.rst
│ ├── installation.rst
│ ├── internals.rst
│ ├── requirements.txt
│ └── tutorials/
│ ├── hover.rstinclude
│ └── tutorials.rst
├── docs2/
│ ├── .gitignore
│ ├── Makefile
│ ├── conf.py
│ ├── faq.rst
│ ├── howto.rst
│ ├── index.rst
│ ├── installation.rst
│ ├── overview.rst
│ ├── requirements.txt
│ ├── tutorials.rst
│ └── usage.rst
├── ros_ws/
│ └── src/
│ └── crazyswarm/
│ ├── launch/
│ │ ├── USC.yaml
│ │ ├── allCrazyflies.yaml
│ │ ├── figure8_smooth.csv
│ │ ├── hover_swarm.launch
│ │ ├── mocap_helper.launch
│ │ ├── object_tracker.yaml
│ │ ├── rqt.perspective
│ │ └── test.rviz
│ └── scripts/
│ ├── backgroundComputation.py
│ ├── cmdVelocityCircle.py
│ ├── collisionAvoidance.py
│ ├── collisionAvoidanceHighConflict.py
│ ├── conftest.py
│ ├── crossing2_pps/
│ │ ├── pp1.csv
│ │ └── pp2.csv
│ ├── crossing4_pps/
│ │ ├── pp1.csv
│ │ ├── pp2.csv
│ │ ├── pp3.csv
│ │ └── pp4.csv
│ ├── csv_sequence.py
│ ├── example_cmd_pos.py
│ ├── graphVisualization.py
│ ├── individual_hover.py
│ ├── led_colors.py
│ ├── pytest.ini
│ ├── sequence_trajectories/
│ │ ├── 1/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 2/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 3/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 4/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 5/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 6/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ └── 7/
│ │ ├── 1.csv
│ │ ├── 10.csv
│ │ ├── 11.csv
│ │ ├── 12.csv
│ │ ├── 13.csv
│ │ ├── 14.csv
│ │ ├── 15.csv
│ │ ├── 16.csv
│ │ ├── 17.csv
│ │ ├── 18.csv
│ │ ├── 19.csv
│ │ ├── 2.csv
│ │ ├── 3.csv
│ │ ├── 4.csv
│ │ ├── 5.csv
│ │ ├── 6.csv
│ │ ├── 7.csv
│ │ ├── 8.csv
│ │ └── 9.csv
│ ├── swap6v.py
│ ├── swap6v_pps/
│ │ ├── pp1.csv
│ │ ├── pp2.csv
│ │ ├── pp3.csv
│ │ ├── pp4.csv
│ │ ├── pp5.csv
│ │ └── pp6.csv
│ ├── takeoff.csv
│ ├── test_collisionAvoidance.py
│ ├── test_highLevel.py
│ ├── test_lowLevel.py
│ ├── test_ros.py
│ ├── test_simOnly.py
│ ├── test_videoOutput.py
│ ├── test_yamlString.py
│ ├── udp_multicast.py
│ ├── waypoints.csv
│ ├── waypoints.py
│ └── waypoints_simple.py
├── rosinstall
└── systemtests/
├── SDplotting/
│ ├── cfusdlog.py
│ ├── data_helper.py
│ ├── plot.py
│ ├── save.py
│ └── settings.yaml
├── figure8_ideal_traj.csv
├── mcap_handler.py
├── multi_trajectory_ideal_traj0.csv
├── plotter_class.py
└── test_flights.py
================================================
FILE CONTENTS
================================================
================================================
FILE: .github/workflows/ci-docs2.yml
================================================
name: Docs
on:
push:
branches: [ main ]
pull_request:
branches: [ main ]
jobs:
build:
runs-on: ubuntu-latest
steps:
- name: Checkout
uses: actions/checkout@v2
# Handle dependencies here instead of conda_env.yaml
# because some packages are conda-forge only,
# which makes the env build much more slowly.
- name: Install dependencies
run: |
sudo apt update
sudo apt install texlive-latex-extra dvipng
sudo pip3 install -U setuptools
sudo pip3 install -U -r docs2/requirements.txt --ignore-installed PyYAML
- name: Build Docs
run: |
cd docs2
make html
- name: Disable github Jekyll
run: |
touch docs2/_build/html/.nojekyll
- name: Deploy
if: github.ref == 'refs/heads/main'
uses: JamesIves/github-pages-deploy-action@v4
with:
folder: docs2/_build/html # The folder the action should deploy.
================================================
FILE: .github/workflows/ci-ros2-mac.yml
================================================
name: ROS2 (Mac)
# Disable for now
on:
push:
branches-ignore:
- '**'
# on:
# push:
# branches: [ main ]
# pull_request:
# branches: [ main ]
jobs:
build: # Docker is not supported on macOS and Windows.
runs-on: macOS-latest
steps:
- uses: ros-tooling/setup-ros@v0.2
with:
required-ros-distributions: galactic
# There is a bug in vcstools, where submodules are not pulled recursively
# See https://github.com/dirk-thomas/vcstool/issues/205
# This is fixed in master, but not in the latest release
# Pull the latest version here
- name: install dependencies
run: |
sudo pip3 install git+https://github.com/dirk-thomas/vcstool.git@7d1329f296cef4b767bf7ba0bf53a29dd3d3019c
- name: Install dependencies
run: |
brew install libusb
- name: build and test ROS 2
uses: ros-tooling/action-ros-ci@v0.2
with:
package-name: crazyflie crazyflie_interfaces
target-ros2-distro: galactic
colcon-defaults: |
{
"build": {
"packages-select": [
"crazyflie",
"crazyflie_interfaces"
]
}
}
================================================
FILE: .github/workflows/ci-ros2-win.yml
================================================
name: ROS2 (Windows)
# Disable for now
on:
push:
branches-ignore:
- '**'
# on:
# push:
# branches: [ main ]
# pull_request:
# branches: [ main ]
jobs:
build: # Docker is not supported on macOS and Windows.
runs-on: windows-latest
steps:
- uses: ros-tooling/setup-ros@v0.2
with:
required-ros-distributions: galactic
# # There is a bug in vcstools, where submodules are not pulled recursively
# # See https://github.com/dirk-thomas/vcstool/issues/205
# # This is fixed in master, but not in the latest release
# # Pull the latest version here
# - name: install dependencies
# run: |
# pip3 install git+https://github.com/dirk-thomas/vcstool.git@7d1329f296cef4b767bf7ba0bf53a29dd3d3019c
# - name: Install dependencies
# run: |
# brew install libusb
- name: build and test ROS 2
uses: ros-tooling/action-ros-ci@v0.2
with:
package-name: crazyflie crazyflie_interfaces
target-ros2-distro: galactic
colcon-defaults: |
{
"build": {
"packages-select": [
"crazyflie",
"crazyflie_interfaces"
]
}
}
================================================
FILE: .github/workflows/ci-ros2.yml
================================================
name: ROS2 (Ubuntu)
on:
push:
branches: [ main ]
pull_request:
branches: [ main ]
# Based on example provided at https://github.com/ros-tooling/setup-ros
jobs:
build_docker: # On Linux, use docker
runs-on: ubuntu-latest
strategy:
fail-fast: False
matrix:
ros_distribution:
- humble
- jazzy
- rolling
# Define the Docker image(s) associated with each ROS distribution.
include:
# Humble Hawksbill (May 2022 - May 2027)
- docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest
ros_distribution: humble
ros_version: 2
# Jazzy Jalisco (May 2024 - May 2029)
- docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-ros-base-latest
ros_distribution: jazzy
ros_version: 2
# Rolling Ridley (June 2020 - Present)
- docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest
ros_distribution: rolling
ros_version: 2
container:
image: ${{ matrix.docker_image }}
steps:
- name: install dependencies
run: |
sudo apt update
sudo apt install -y libboost-program-options-dev libusb-1.0-0-dev
- name: install pip dependencies
# TODO: would be better to follow https://answers.ros.org/question/370666/how-to-declare-an-external-python-dependency-in-ros2/
# but this requires some upstream changes
# colcon still does not seem to properly support venv, so we use a workaround to install
# a python package globally by disabling the externally managed flag that is default on Ubuntu 24.04
run: |
sudo rm -f /usr/lib/python3.12/EXTERNALLY-MANAGED
pip install rowan
- uses: actions/checkout@v2
- name: build and test ROS 2
uses: ros-tooling/action-ros-ci@v0.3
with:
package-name: |
crazyflie
crazyflie_examples
crazyflie_interfaces
crazyflie_py
crazyflie_sim
target-ros2-distro: ${{ matrix.ros_distribution }}
vcs-repo-file-url: rosinstall
================================================
FILE: .github/workflows/systemtests.yml
================================================
name: System Tests
on:
push:
branches: [ "feature-systemtests-better" ]
# manual trigger
workflow_dispatch:
jobs:
build:
runs-on: threadripper
steps:
- name: Create workspace
id: step1
run: |
cd ros2_ws/src || mkdir -p ros2_ws/src
- name: Checkout motion capture package
id: step2
run: |
cd ros2_ws/src
ls motion_capture_tracking || git clone --branch ros2 --recursive https://github.com/IMRCLab/motion_capture_tracking.git
- name: Checkout Crazyswarm2
id: step3
uses: actions/checkout@v4
with:
path: ros2_ws/src/crazyswarm2
submodules: 'recursive'
- name: Build workspace
id: step4
run: |
source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
- name: Flight test
id: step5
run: |
cd ros2_ws
source /opt/ros/humble/setup.bash
. install/local_setup.bash
export ROS_LOCALHOST_ONLY=1
export ROS_DOMAIN_ID=99
python3 src/crazyswarm2/systemtests/test_flights.py
- name: Upload files
id: step6
if: '!cancelled()'
uses: actions/upload-artifact@v3
with:
name: pdf_rosbags_and_logs
path: |
ros2_ws/results
# - name: build and test ROS 2
# uses: ros-tooling/action-ros-ci@v0.3
# with:
# package-name: |
# crazyflie
# crazyflie_examples
# crazyflie_interfaces
# crazyflie_py
# crazyflie_sim
# target-ros2-distro: humble
# vcs-repo-file-url: rosinstall
================================================
FILE: .github/workflows/systemtests_sim.yml
================================================
name: System Tests Simulation
on:
push:
branches: [ main ]
pull_request:
branches: [ main ]
# manual trigger
workflow_dispatch:
jobs:
build:
runs-on: threadripper
steps:
- name: Build firmware
id: step1
run: |
ls crazyflie-firmware || git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git
cd crazyflie-firmware
git pull
git submodule sync
git submodule update --init --recursive
make cf2_defconfig
make bindings_python
cd build
python3 setup.py install --user
- name: Create workspace
id: step2
run: |
cd ros2_ws/src || mkdir -p ros2_ws/src
- name: Checkout motion capture package
id: step3
run: |
cd ros2_ws/src
ls motion_capture_tracking || git clone --branch ros2 --recursive https://github.com/IMRCLab/motion_capture_tracking.git
cd motion_capture_tracking
git pull
git submodule sync
git submodule update --recursive --init
- name: Checkout Crazyswarm2
id: step4
uses: actions/checkout@v4
with:
path: ros2_ws/src/crazyswarm2
submodules: 'recursive'
- name: Build workspace
id: step5
run: |
source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
- name: Flight test
id: step6
run: |
cd ros2_ws
source /opt/ros/humble/setup.bash
. install/local_setup.bash
export PYTHONPATH="${PYTHONPATH}:/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/crazyflie-firmware/build/"
export ROS_LOCALHOST_ONLY=1
export ROS_DOMAIN_ID=99
python3 src/crazyswarm2/systemtests/test_flights.py --sim -v #-v is verbose argument of unittest
- name: Upload files
id: step7
if: '!cancelled()'
uses: actions/upload-artifact@v4
with:
name: pdf_rosbags_and_logs
path: |
ros2_ws/results
================================================
FILE: .gitignore
================================================
.DS_Store
crazyswarm2_sim_webots/
*.pyc
*.o
*~
================================================
FILE: .gitmodules
================================================
[submodule "crazyflie/deps/crazyflie_tools"]
path = crazyflie/deps/crazyflie_tools
url = https://github.com/whoenig/crazyflie_tools.git
================================================
FILE: LICENSE
================================================
The MIT License (MIT)
Copyright (c) 2014 whoenig
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================================================
FILE: README.md
================================================
* Local CI: [](https://github.com/IMRCLab/crazyswarm2/actions/workflows/ci-ros2.yml)
* Rolling Dev CI : [](https://build.ros2.org/job/Rdev__crazyflie__ubuntu_noble_amd64/)
* Jazzy Dev CI: [](https://build.ros2.org/job/Jdev__crazyswarm2__ubuntu_noble_amd64/)
* Humble Dev CI: [](https://build.ros2.org/job/Hdev__crazyswarm2__ubuntu_jammy_amd64/)
# Crazyswarm2
A ROS 2-based stack for Bitcraze Crazyflie multirotor robots.
The documentation is available here: https://imrclab.github.io/crazyswarm2/.
## Troubleshooting
Please start a [Discussion](https://github.com/IMRCLab/crazyswarm2/discussions) for...
- Getting Crazyswarm2 to work with your hardware setup.
- Advice on how to use it to achieve your goals.
- Rough ideas for a new feature.
Please open an [Issue](https://github.com/IMRCLab/crazyswarm2/issues) if you believe that fixing your problem will involve a **change in the Crazyswarm2 source code**, rather than your own configuration files. For example...
- Bug reports.
- New feature proposals with details.
================================================
FILE: crazyflie/CHANGELOG.rst
================================================
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package crazyflie
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.0.3 (2025-07-09)
------------------
* Improve package.xml (separate maintainer tags, update year)
* Contributors: Kimberly N. McGuire
1.0.2 (2025-07-02)
------------------
1.0.1 (2025-06-30)
------------------
* Fix build errors and dependencies on ROS Build Farm
* Contributors: Kimberly N. McGuire, Wolfgang Hönig
1.0.0 (2025-06-21)
------------------
* First official release.
* Contributors: Christian Llanes, Julien Thevenoz, Khaled Wahba, Kimberly N. McGuire, Pablo, Wolfgang Hönig, Zach Williams, ben-jarvis, nan cai, phanfeld, torobo
================================================
FILE: crazyflie/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.8)
project(crazyflie)
#if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
# add_compile_options(-Wall -Wextra -Wpedantic)
#endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_srvs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(crazyflie_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(motion_capture_tracking_interfaces REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(ros_environment REQUIRED)
if (DEFINED ENV{ROS_DISTRO})
set(ROS_DISTRO_VALUE "$ENV{ROS_DISTRO}")
else()
message(WARNING "ROS_DISTRO environment variable not set. C++ macro will be empty or default.")
set(ROS_DISTRO_VALUE "unknown_distro") # Or an empty string, or "NO_ROS_DISTRO_SET"
endif()
# add dependencies
add_subdirectory(deps/crazyflie_tools)
# add_subdirectory(externalDependencies/libmotioncapture)
include_directories(
deps/crazyflie_tools/crazyflie_cpp/include
deps/crazyflie_tools/crazyflie_cpp/crazyflie-link-cpp/include
# externalDependencies/libobjecttracker/include
# externalDependencies/libmotioncapture/include
${EIGEN3_INCLUDE_DIRS}
# ${PCL_INCLUDE_DIRS}
)
add_executable(teleop
src/teleop.cpp
)
ament_target_dependencies(teleop
rclcpp
sensor_msgs
std_srvs
crazyflie_interfaces
geometry_msgs
)
add_executable(crazyflie_server
src/crazyflie_server.cpp
)
target_link_libraries(crazyflie_server
crazyflie_cpp
)
if("${ROS_DISTRO_VALUE}" STREQUAL "humble")
target_compile_definitions(crazyflie_server PRIVATE ROS_DISTRO_HUMBLE)
endif()
ament_target_dependencies(crazyflie_server
rclcpp
tf2_ros
sensor_msgs
nav_msgs
std_srvs
crazyflie_interfaces
motion_capture_tracking_interfaces
)
# # scan
# add_executable(scan
# src/scan.cpp
# )
# target_link_libraries(scan
# crazyflie_cpp
# ${Boost_LIBRARIES}
# )
# target_compile_features(scan PUBLIC c_std_99 cxx_std_17)
#add_executable(motion_capture_tracking_node
# src/motion_capture_tracking_node.cpp
#)
#target_link_libraries(motion_capture_tracking_node
# libobjecttracker
# libmotioncapture
#)
#ament_target_dependencies(motion_capture_tracking_node
# rclcpp
#)
#target_include_directories(motion_capture_tracking_node PUBLIC
# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>)
#target_compile_features(motion_capture_tracking_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
# Install C++ executables
install(TARGETS
# crazyflie_tools
comCheck
scan
listParams
listLogVariables
listMemories
reboot
battery
version
console
log
setParam
downloadUSDLogfile
#
teleop
crazyflie_server
DESTINATION lib/${PROJECT_NAME})
# Install Python executables
install(PROGRAMS
scripts/crazyflie_server.py
scripts/chooser.py
scripts/vel_mux.py
scripts/cfmult.py
scripts/simple_mapper_multiranger.py
scripts/aideck_streamer.py
scripts/gui.py
scripts/flash.py
DESTINATION lib/${PROJECT_NAME}
)
# Install launch, config, and urdf files.
install(DIRECTORY
launch
config
urdf
DESTINATION share/${PROJECT_NAME}/
)
# if(BUILD_TESTING)
# find_package(ament_lint_auto REQUIRED)
# # the following line skips the linter which checks for copyrights
# # uncomment the line when a copyright and license is not present in all source files
# #set(ament_cmake_copyright_FOUND TRUE)
# # the following line skips cpplint (only works in a git repo)
# # uncomment the line when this package is not in a git repo
# #set(ament_cmake_cpplint_FOUND TRUE)
# ament_lint_auto_find_test_dependencies()
# endif()
ament_package()
================================================
FILE: crazyflie/config/aideck_streamer.yaml
================================================
image_topic: /camera/image
camera_info_topic: /camera/camera_info
deck_ip: "192.168.4.1"
deck_port: 5000
image_width: 324
image_height: 324
camera_name: crazyflie
camera_matrix:
rows: 3
cols: 3
data: [181.87464, 0. , 162.52301,
0. , 182.58129, 160.79418,
0. , 0. , 1. ]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.070366, -0.006434, -0.002691, -0.001983, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [1., 0., 0.,
0., 1., 0.,
0., 0., 1.]
projection_matrix:
rows: 3
cols: 4
data: [169.24555, 0. , 161.54541, 0. ,
0. , 169.97813, 159.07974, 0. ,
0. , 0. , 1. , 0. ]
================================================
FILE: crazyflie/config/config.rviz
================================================
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 549
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
cf231:
Value: true
world:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
world:
cf231:
{}
Update Interval: 0
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cf231/robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
cf231:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Mass Properties:
Inertia: false
Mass: false
Name: CF231
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 3.1647839546203613
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.028163839131593704
Y: -0.049007341265678406
Z: -0.025782346725463867
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.4847976565361023
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.9185757637023926
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 74
Y: 60
================================================
FILE: crazyflie/config/crazyflies.yaml
================================================
# named list of all robots
fileversion: 3
robots:
cf231:
enabled: true
uri: radio://0/80/2M/E7E7E7E7E7
initial_position: [0.0, 0.0, 0.0]
type: cf21 # see robot_types
# firmware_params:
# kalman:
# pNAcc_xy: 1.0 # default 0.5
# firmware_logging:
# enabled: true
# custom_topics:
# topic_name3:
# frequency: 1
# vars: ["acc.x", "acc.y"]
# reference_frame: "odom_cf231" # use a custom reference frame if no global coordinate system is available
cf5:
enabled: false
uri: radio://0/80/2M/E7E7E7E705
initial_position: [0.0, -0.5, 0.0]
type: cf21 # see robot_types
# firmware_params:
# kalman:
# pNAcc_xy: 1.0 # default 0.5
#firmware_logging:
# custom_topics:
# topic_name3:
# frequency: 1
# vars: ["acc.x", "acc.y", "acc.z"]
# reference_frame: "odom_cf5"
# Definition of the various robot types
robot_types:
cf21:
motion_capture:
tracking: "librigidbodytracker" # one of "vendor", "librigidbodytracker"
# only if enabled; see motion_capture.yaml
marker: default_single_marker
dynamics: default
big_quad: false
battery:
voltage_warning: 3.8 # V
voltage_critical: 3.7 # V
# firmware_params:
# kalman:
# pNAcc_xy: 1.0 # default 0.5
#firmware_logging:
# enabled: true
# default_topics:
# pose:
# frequency: 1 # Hz
# custom_topics:
# topic_name3:
# frequency: 1
# vars: ["acc.x", "acc.y", "acc.z"]
# reference_frame: "odom"
cf21_mocap_deck:
motion_capture:
tracking: "librigidbodytracker" # one of "vendor", "librigidbodytracker"
# only if enabled; see motion_capture.yaml
marker: mocap_deck
dynamics: default
big_quad: false
battery:
voltage_warning: 3.8 # V
voltage_critical: 3.7 # V
# firmware_params:
# kalman:
# pNAcc_xy: 1.0 # default 0.5
#firmware_logging:
# enabled: true
# default_topics:
# pose:
# frequency: 1 # Hz
# custom_topics:
# topic_name3:
# frequency: 1
# vars: ["acc.x", "acc.y", "acc.z"]
# reference_frame: "mocap"
# global settings for all robots
all:
# firmware logging for all drones (use robot_types/type_name to set per type, or
# robots/drone_name to set per drone)
firmware_logging:
enabled: true
default_topics:
# remove to disable default topic
pose:
frequency: 10 # Hz
status:
frequency: 1 # 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 parameters for all drones (use robot_types/type_name to set per type, or
# robots/drone_name to set per drone)
firmware_params:
commander:
enHighLevel: 1
stabilizer:
estimator: 2 # 1: complementary, 2: kalman
controller: 2 # 1: PID, 2: mellinger
# ring:
# effect: 16 # 6: double spinner, 7: solid color, 16: packetRate
# solidBlue: 255 # if set to solid color
# solidGreen: 0 # if set to solid color
# solidRed: 0 # if set to solid color
# headlightEnable: 0
locSrv:
extPosStdDev: 1e-3
extQuatStdDev: 0.5e-1
# kalman:
# resetEstimation: 1
# reference frame for on-board state-estimate of all drones (use robot_types/type_name to set per type, or
# robots/drone_name to set per drone)
reference_frame: "world"
broadcasts:
num_repeats: 15 # number of times broadcast commands are repeated
delay_between_repeats_ms: 1 # delay in milliseconds between individual repeats
================================================
FILE: crazyflie/config/motion_capture.yaml
================================================
/motion_capture_tracking:
ros__parameters:
# one of "optitrack", "optitrack_closed_source", "vicon", "qualisys", "nokov", "vrpn", "motionanalysis"
type: "optitrack"
# Specify the hostname or IP of the computer running the motion capture software
hostname: "141.23.110.143"
topics:
frame_id: "mocap"
poses:
qos:
mode: "sensor"
deadline: 100.0 # Hz
tf:
child_frame_id: "{}_mocap"
marker_configurations:
default: # for standard Crazyflie
offset: [0.0, 0.0, 0.0]
points:
p0: [ 0.0, 0.0, 0.022] # top
p1: [-0.042, 0.042, 0.0 ] # back left (M3)
p2: [-0.042, -0.042, 0.0 ] # back right (M2)
p3: [ 0.042, -0.042, 0.0 ] # front right (M1)
default_single_marker:
offset: [0.0, -0.01, -0.04]
points:
p0: [0.0177184,0.0139654,0.0557585]
mocap_deck:
offset: [0.0, 0.0, -0.01]
points:
p0: [0.03, 0.0, 0.0] # front
p1: [0.00, -0.03, 0.0] # right
p2: [-0.015, 0.0, 0.0] # back
p3: [0.00, 0.03, 0.0] # left
medium_frame:
offset: [0.0, 0.0, -0.03]
points:
p0: [-0.00896228,-0.000716753,0.0716129]
p1: [-0.0156318,0.0997402,0.0508162]
p2: [0.0461693,-0.0881012,0.0380672]
p3: [-0.0789959,-0.0269793,0.0461144]
big_frame:
offset: [0.0, 0.0, -0.06]
points:
p0: [0.0558163,-0.00196302,0.0945539]
p1: [-0.0113941,0.00945842,0.0984811]
p2: [-0.0306277,0.0514879,0.0520456]
p3: [0.0535816,-0.0400775,0.0432799]
dynamics_configurations:
default:
max_velocity: [2, 2, 3] # m/s
max_angular_velocity: [20, 20, 10] # rad/s
max_roll: 1.4 #rad
max_pitch: 1.4 #rad
max_fitness_score: 0.001
# Rigid bodies will be automatically generated by the launch file
# rigid_bodies:
================================================
FILE: crazyflie/config/server.yaml
================================================
/crazyflie_server:
ros__parameters:
warnings:
frequency: 1.0 # report/run checks once per second
motion_capture:
warning_if_rate_outside: [80.0, 120.0]
communication:
max_unicast_latency: 10.0 # ms
min_unicast_ack_rate: 0.9
min_unicast_receive_rate: 0.9 # requires status topic to be enabled
min_broadcast_receive_rate: 0.9 # requires status topic to be enabled
publish_stats: false
firmware_params:
query_all_values_on_connect: False
# simulation related
sim:
max_dt: 0 #0.1 # artificially limit the step() function (set to 0 to disable)
backend: np # see backend folder for a list
visualizations: # see visualization folder for a list
rviz:
enabled: true
pdf:
enabled: false
output_file: "result.pdf"
record_states:
enabled: false
output_dir: "state_info"
logging_time: 0.2 # how many seconds to leave between logs
file_formats: ["csv", "np"] # csv, np
blender:
enabled: false
fps: 1 # frames per second
cycle_bg: false # if true, pictures will cycle through different environemt background images (useful for synthetic image generation). Otherwise a single environment background image will be used
cf_cameras: # names of crazyflies with cameras on them if enabled in `crazyflies.yaml`
cf231:
calibration:
tvec: [0,0,0]
rvec: [1.2092,-1.2092,1.2092] # 0 deg tilt (cameras looks in front of crazyflie)
cf5:
calibration:
tvec: [0,0,0]
rvec: [ 0.61394313, -0.61394313, 1.48218982] # 45 deg tilt
controller: mellinger # none, pid, mellinger
================================================
FILE: crazyflie/config/teleop.yaml
================================================
/teleop:
ros__parameters:
frequency: 100 # set to 0, to disable manual flight modes
mode: "high_level" # one of high_level, cmd_rpy, cmd_vel_world
cmd_rpy:
roll:
axis: 5
max: 30.0 # deg
deadband: 0.0
pitch:
axis: 4
max: 30.0 # deg
deadband: 0.0
yawrate:
axis: 1
max: 200.0 # deg/s
deadband: 5.0 # deg/s
thrust:
axis: 2
max: 60000.0 # PWM
deadband: 0.0
cmd_vel_world:
x_velocity:
axis: 5
max: 0.5 # m/s
deadband: 0.1 # m/s
y_velocity:
axis: 4
max: 0.5 # m/s
deadband: 0.1 # m/s
z_velocity:
axis: 2
max: 0.5 # m/s
deadband: 0.1 # m/s
yaw_velocity:
axis: 1
max: 0.5 # rad/s
deadband: 0.1 # rad/s
# workspace limits
x_limit: [-1.0, 1.0] # m
y_limit: [-1.0, 1.0] # m
z_limit: [0.0, 2.5] # m
initial_position:
x: 0.0
y: 0.0
z: 0.10
auto_yaw_rate: 0.0 # rad/s, use 0.0 for manual yaw control
takeoff:
duration: 2.0
height: 0.5
button: 7 # 7 start
land:
button: 6 # 6 back
emergency:
button: 1 # 1 red
arm:
button: 3 # yellow
================================================
FILE: crazyflie/launch/launch.py
================================================
import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch_ros.actions import Node
from launch.conditions import LaunchConfigurationEquals
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
def parse_yaml(context):
# Load the crazyflies YAML file
crazyflies_yaml = LaunchConfiguration('crazyflies_yaml_file').perform(context)
with open(crazyflies_yaml, 'r') as file:
crazyflies = yaml.safe_load(file)
# store the fileversion
fileversion = 1
if "fileversion" in crazyflies:
fileversion = crazyflies["fileversion"]
# server params
server_yaml = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'server.yaml')
with open(server_yaml, 'r') as ymlfile:
server_yaml_content = yaml.safe_load(ymlfile)
server_params = [crazyflies] + [server_yaml_content['/crazyflie_server']['ros__parameters']]
# robot description
urdf = os.path.join(
get_package_share_directory('crazyflie'),
'urdf',
'crazyflie_description.urdf')
with open(urdf, 'r') as f:
robot_desc = f.read()
server_params[1]['robot_description'] = robot_desc
# construct motion_capture_configuration
motion_capture_yaml = LaunchConfiguration('motion_capture_yaml_file').perform(context)
with open(motion_capture_yaml, 'r') as ymlfile:
motion_capture_content = yaml.safe_load(ymlfile)
motion_capture_params = motion_capture_content['/motion_capture_tracking']['ros__parameters']
motion_capture_params['rigid_bodies'] = dict()
for key, value in crazyflies['robots'].items():
type = crazyflies['robot_types'][value['type']]
if value['enabled'] and \
((fileversion == 1 and type['motion_capture']['enabled']) or \
((fileversion >= 2 and type['motion_capture']['tracking'] == "librigidbodytracker"))):
motion_capture_params['rigid_bodies'][key] = {
'initial_position': value['initial_position'],
'marker': type['motion_capture']['marker'],
'dynamics': type['motion_capture']['dynamics'],
}
# copy relevent settings to server params
server_params[1]['poses_qos_deadline'] = motion_capture_params['topics']['poses']['qos']['deadline']
return [
Node(
package='motion_capture_tracking',
executable='motion_capture_tracking_node',
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('backend'), "' != 'sim' and '", LaunchConfiguration('mocap'), "' == 'True'"])),
name='motion_capture_tracking',
output='screen',
parameters= [motion_capture_params],
),
Node(
package='crazyflie',
executable='crazyflie_server.py',
condition=LaunchConfigurationEquals('backend','cflib'),
name='crazyflie_server',
output='screen',
parameters= server_params,
),
Node(
package='crazyflie',
executable='crazyflie_server',
condition=LaunchConfigurationEquals('backend','cpp'),
name='crazyflie_server',
output='screen',
parameters= server_params,
prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']),
),
Node(
package='crazyflie_sim',
executable='crazyflie_server',
condition=LaunchConfigurationEquals('backend','sim'),
name='crazyflie_server',
output='screen',
emulate_tty=True,
parameters= server_params,
)]
def generate_launch_description():
default_crazyflies_yaml_path = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'crazyflies.yaml')
default_motion_capture_yaml_path = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'motion_capture.yaml')
default_rviz_config_path = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'config.rviz')
telop_yaml_path = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'teleop.yaml')
return LaunchDescription([
DeclareLaunchArgument('crazyflies_yaml_file',
default_value=default_crazyflies_yaml_path),
DeclareLaunchArgument('motion_capture_yaml_file',
default_value=default_motion_capture_yaml_path),
DeclareLaunchArgument('rviz_config_file',
default_value=default_rviz_config_path),
DeclareLaunchArgument('backend', default_value='cpp'),
DeclareLaunchArgument('debug', default_value='False'),
DeclareLaunchArgument('rviz', default_value='False'),
DeclareLaunchArgument('gui', default_value='True'),
DeclareLaunchArgument('teleop', default_value='True'),
DeclareLaunchArgument('mocap', default_value='True'),
DeclareLaunchArgument('teleop_yaml_file', default_value=''),
OpaqueFunction(function=parse_yaml),
Node(
condition=LaunchConfigurationEquals('teleop', 'True'),
package='crazyflie',
executable='teleop',
name='teleop',
remappings=[
('emergency', 'all/emergency'),
('arm', 'all/arm'),
('takeoff', 'all/takeoff'),
('land', 'all/land'),
# uncomment to manually control (and update teleop.yaml)
# ('cmd_vel_legacy', 'cf6/cmd_vel_legacy'),
# ('cmd_full_state', 'cf6/cmd_full_state'),
# ('notify_setpoints_stop', 'cf6/notify_setpoints_stop'),
],
parameters= [PythonExpression(["'" + telop_yaml_path +"' if '", LaunchConfiguration('teleop_yaml_file'), "' == '' else '", LaunchConfiguration('teleop_yaml_file'), "'"])],
),
Node(
condition=LaunchConfigurationEquals('teleop', 'True'),
package='joy',
executable='joy_node',
name='joy_node' # by default id=0
),
Node(
condition=LaunchConfigurationEquals('rviz', 'True'),
package='rviz2',
namespace='',
executable='rviz2',
name='rviz2',
arguments=['-d', LaunchConfiguration('rviz_config_file')],
parameters=[{
"use_sim_time": PythonExpression(["'", LaunchConfiguration('backend'), "' == 'sim'"]),
}]
),
Node(
condition=LaunchConfigurationEquals('gui', 'True'),
package='crazyflie',
namespace='',
executable='gui.py',
name='gui',
parameters=[{
"use_sim_time": PythonExpression(["'", LaunchConfiguration('backend'), "' == 'sim'"]),
}]
),
])
================================================
FILE: crazyflie/launch/launch_teleop2.py
================================================
import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
# load crazyflies
crazyflies_yaml = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'crazyflies.yaml')
with open(crazyflies_yaml, 'r') as ymlfile:
crazyflies = yaml.safe_load(ymlfile)
server_params = crazyflies
# construct motion_capture_configuration
motion_capture_yaml = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'motion_capture.yaml')
with open(motion_capture_yaml, 'r') as ymlfile:
motion_capture = yaml.safe_load(ymlfile)
motion_capture_params = motion_capture["/motion_capture_tracking"]["ros__parameters"]
motion_capture_params["rigid_bodies"] = dict()
for key, value in crazyflies["robots"].items():
type = crazyflies["robot_types"][value["type"]]
motion_capture_params["rigid_bodies"][key] = {
"initial_position": value["initial_position"],
"marker": type["marker"],
"dynamics": type["dynamics"],
}
# teleop params
teleop_params = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'teleop.yaml')
teleop_5_params = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'teleop_5.yaml')
return LaunchDescription([
Node(
package='motion_capture_tracking',
executable='motion_capture_tracking_node',
name='motion_capture_tracking',
output='screen',
parameters=[motion_capture_params]
),
Node(
package='crazyflie',
executable='teleop',
name='teleop',
remappings=[
('takeoff', 'cf231/takeoff'),
('land', 'cf231/land'),
('cmd_vel_legacy', 'cf231/cmd_vel_legacy'),
('cmd_full_state', 'cf231/cmd_full_state'),
('notify_setpoints_stop', 'cf231/notify_setpoints_stop'),
('joy', 'cf231/joy'),
],
parameters=[teleop_params]
),
Node(
package='crazyflie',
executable='teleop',
name='teleop',
remappings=[
('takeoff', 'cf5/takeoff'),
('land', 'cf5/land'),
('cmd_vel_legacy', 'cf5/cmd_vel_legacy'),
('cmd_full_state', 'cf5/cmd_full_state'),
('notify_setpoints_stop', 'cf5/notify_setpoints_stop'),
('joy', 'cf5/joy'),
],
parameters=[teleop_5_params]
),
Node(
package='joy',
executable='joy_node',
name='joy_node',
remappings=[('joy', 'cf231/joy')],
output='screen',
parameters=[{'device_id':0}] # new joystick
),
Node(
package='joy',
executable='joy_node',
name='joy_node',
remappings=[('joy', 'cf5/joy')],
parameters=[{'device_id':1}] # old
),
Node(
package='crazyflie',
executable='crazyflie_server',
name='crazyflie_server',
output='screen',
parameters=[server_params]
),
])
================================================
FILE: crazyflie/launch/teleop_launch.py
================================================
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='crazyflie',
executable='teleop',
name='teleop'
),
Node(
package='joy',
executable='joy_node',
name='joy_node'
),
Node(
package='crazyflie',
executable='crazyflie_server',
name='crazyflie_server'
)
])
================================================
FILE: crazyflie/package.xml
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>crazyflie</name>
<version>1.0.3</version>
<description>ROS 2 Package for Bitcraze Crazyflie robots</description>
<maintainer email="hoenig@tu-berlin.de">Wolfgang Hönig</maintainer>
<maintainer email="kimberleymcguire@gmail.com">Kimberly N. McGuire</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_python</buildtool_depend>
<depend>rclcpp</depend>
<depend>tf2_ros</depend>
<depend>std_srvs</depend>
<depend>sensor_msgs</depend>
<depend>crazyflie_interfaces</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>motion_capture_tracking_interfaces</depend>
<depend>eigen</depend>
<depend>boost</depend>
<depend>libusb-1.0-dev</depend>
<depend>ros_environment</depend>
<exec_depend>tf_transformations</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
================================================
FILE: crazyflie/scripts/aideck_streamer.py
================================================
#!/usr/bin/env python3
import socket,os,struct, time
import numpy as np
import cv2
import yaml
from ament_index_python.packages import get_package_share_directory
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from rcl_interfaces.msg import ParameterDescriptor, ParameterType
class ImageStreamerNode(Node):
def __init__(self):
super().__init__("image_node")
# declare config path parameter
self.declare_parameter(
name="config_path",
value=os.path.join(
get_package_share_directory('crazyflie'),
'config',
'aideck_streamer.yaml'
)
)
config_path = self.get_parameter("config_path").value
with open(config_path) as f:
config = yaml.load(f, Loader=yaml.FullLoader)
# declare topic names
self.declare_parameter(
name="image_topic",
value=config["image_topic"],
descriptor=ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description="Image topic to publish to.",
),
)
self.declare_parameter(
name="camera_info_topic",
value=config["camera_info_topic"],
descriptor=ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description="Camera info topic to subscribe to.",
),
)
# declare aideck ip and port
self.declare_parameter(
name='deck_ip',
value=config['deck_ip'],
)
self.declare_parameter(
name='deck_port',
value=config['deck_port'],
)
# define variables from ros2 parameters
image_topic = (
self.get_parameter("image_topic").value
)
self.get_logger().info(f"Image topic: {image_topic}")
info_topic = (
self.get_parameter("camera_info_topic").value
)
self.get_logger().info(f"Image info topic: {info_topic}")
# create messages and publishers
self.image_msg = Image()
self.camera_info_msg = self._construct_from_yaml(config)
self.image_publisher = self.create_publisher(Image, image_topic, 10)
self.info_publisher = self.create_publisher(CameraInfo, info_topic, 10)
# set up connection to AI Deck
deck_ip = self.get_parameter("deck_ip").value
deck_port = int(self.get_parameter("deck_port").value)
self.get_logger().info("Connecting to socket on {}:{}...".format(deck_ip, deck_port))
self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.client_socket.connect((deck_ip, deck_port))
self.get_logger().info("Socket connected")
self.image = None
self.rx_buffer = bytearray()
# set up timers for callbacks
timer_period = 0.01
self.rx_timer = self.create_timer(timer_period, self.receive_callback)
self.tx_timer = self.create_timer(timer_period, self.publish_callback)
def _construct_from_yaml(self, config):
camera_info = CameraInfo()
camera_info.header.frame_id = config['camera_name']
camera_info.header.stamp = self.get_clock().now().to_msg()
camera_info.width = int(config['image_width'])
camera_info.height = int(config['image_height'])
camera_info.distortion_model = config['distortion_model']
camera_info.d = config['distortion_coefficients']['data']
camera_info.k = config['camera_matrix']['data']
camera_info.r = config['rectification_matrix']['data']
camera_info.p = config['projection_matrix']['data']
return camera_info
def _rx_bytes(self, size):
data = bytearray()
while len(data) < size:
data.extend(self.client_socket.recv(size-len(data)))
return data
def receive_callback(self):
# first get the info
packetInfoRaw = self._rx_bytes(4)
[length, routing, function] = struct.unpack('<HBB', packetInfoRaw)
# receive the header
imgHeader = self._rx_bytes(length - 2)
[magic, width, height, depth, format, size] = struct.unpack('<BHHBBI', imgHeader)
# if magic is correct, get new image
if magic == 0xBC:
imgStream = bytearray()
while len(imgStream) < size:
packetInfoRaw = self._rx_bytes(4)
[length, dst, src] = struct.unpack('<HBB', packetInfoRaw)
#print("Chunk size is {} ({:02X}->{:02X})".format(length, src, dst))
chunk = self._rx_bytes(length - 2)
imgStream.extend(chunk)
raw_img = np.frombuffer(imgStream, dtype=np.uint8)
raw_img.shape = (width, height)
self.image = cv2.cvtColor(raw_img, cv2.COLOR_BayerBG2RGBA)
def publish_callback(self):
if self.image is not None:
self.image_msg.header.frame_id = self.camera_info_msg.header.frame_id
self.image_msg.header.stamp = self.get_clock().now().to_msg()
self.camera_info_msg.header.stamp = self.image_msg.header.stamp
width, height, channels = self.image.shape
self.image_msg.height = height
self.image_msg.width = width
self.image_msg.encoding = 'rgba8'
self.image_msg.step = width * channels # number of bytes each row in the array will occupy
self.image_msg.is_bigendian = 0 # TODO: implement automatic check depending on system
self.image_msg.data = self.image.flatten().data
self.image_publisher.publish(self.image_msg)
self.info_publisher.publish(self.camera_info_msg)
self.image = None
def main(args=None):
rclpy.init(args=args)
node = ImageStreamerNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
================================================
FILE: crazyflie/scripts/cfmult.py
================================================
#!/usr/bin/env python3
"""
Helper script to do something to multiple crazyflies on the same channel
simultaneously. Specifically, running ``crazyflie`` ROS package scripts
such as *battery*, *reboot*, and *sysoff* for multiple crazyflies instead
of running them manually for different UID's.
NOTE: this serves a similar pupose to ``chooser.py`` but can be ran from
the command line.
"""
import argparse
from pathlib import Path
import subprocess
from ruamel.yaml import YAML
SUBPROC_TIMEOUT = 1
VALID_SUBCMDS = {
"battery",
"reboot",
"sysoff",
"version",
"listLogVariables",
"listParams",
"listMemories",
"flash",
}
def main():
parser = argparse.ArgumentParser(
description="Wrapper for 'ros2 run crazyflie ...' to multiplex '...' across crazyflies"
)
parser.add_argument("subcommand", help="Subcommand of crazyflie to be ran",
choices=VALID_SUBCMDS)
sub_parser = parser.add_subparsers()
man_parser = sub_parser.add_parser("manual", help="Manually specify channels and UID's")
man_parser.add_argument("-c", "--channel", default=80,
help="Channel of this Crazyflie(s)")
man_parser.add_argument("-u", "--uids", nargs="+", required=True,
help="Last 1-2 characters of UID's of each of the crazyflies in hex.")
yaml_parser = sub_parser.add_parser("yaml", help="Load radio addressed via yaml file")
yaml_parser.add_argument("-C", "--configpath",
default=Path(__file__).parent.parent.resolve() / "config",
help="Path to the configuration *.yaml files")
parser.add_argument("--file_name", help="File name for flashing the crazyflie",
type=str, default="")
args = parser.parse_args()
args.subcommand = [args.subcommand]
if args.subcommand[0] == "sysoff":
args.subcommand = ["reboot", "--mode=sysoff"]
# Determine which URI's to mess with.
if getattr(args, 'configpath', None):
uris = _read_yaml_uris(Path(args.configpath).resolve())
else:
if not all(len(uid) <= 2 for uid in args.uids):
raise ValueError("UID must be a 1-2 element string")
uris = [f"radio://0/{args.channel}/2M/E7E7E7E7{uid.zfill(2)}" for uid in args.uids]
# Run for all URI's determined.
for uri in uris:
if args.subcommand[0] == "flash":
SUBPROC_TIMEOUT = 20
cmd = [
"ros2",
"run",
"crazyflie",
"flash.py",
f"--uri={uri}",
f"--file_name={args.file_name}"
]
else:
SUBPROC_TIMEOUT = 1
cmd = [
"ros2",
"run",
"crazyflie",
*args.subcommand,
f"--uri={uri}",
]
print(f"{' '.join(cmd)}")
subprocess.run(cmd, timeout=SUBPROC_TIMEOUT)
def _read_yaml_uris(configpath):
if not configpath.is_dir():
raise ValueError(f"configpath {configpath} input does not exist.")
yamlpath = configpath / "crazyflies.yaml"
if not yamlpath.is_file():
raise FileNotFoundError(f"{yamlpath} not found in {configpath}.")
yaml = YAML()
cfg = yaml.load(yamlpath)
return [val['uri'] for val in cfg['robots'].values() if val['enabled']]
if __name__ == "__main__":
main()
================================================
FILE: crazyflie/scripts/chooser.py
================================================
#!/usr/bin/env python3
import argparse
try:
import Tkinter
except ImportError:
import tkinter as Tkinter
from ruamel.yaml import YAML
import pathlib
import os
import subprocess
import re
import time
import threading
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument(
"--configpath",
type=str,
default=os.path.join(os.path.dirname(os.path.realpath(__file__)), "../config/crazyflies.yaml"),
help="Path to the configuration .yaml file")
parser.add_argument(
"--stm32Fw",
type=str,
default=os.path.join(os.path.dirname(os.path.realpath(__file__)), "../../../../crazyflie-firmware/cf2.bin"),
help="Path to cf2.bin")
parser.add_argument(
"--nrf51Fw",
type=str,
default=os.path.join(os.path.dirname(os.path.realpath(__file__)), "../../../../crazyflie2-nrf-firmware/cf2_nrf.bin"),
help="Path to cf2_nrf.bin")
args = parser.parse_args()
if not os.path.exists(args.configpath):
print("ERROR: Could not find yaml configuration file in configpath ({}).".format(args.configpath))
exit()
if not os.path.exists(args.stm32Fw):
print("WARNING: Could not find STM32 firmware ({}).".format(args.stm32Fw))
if not os.path.exists(args.nrf51Fw):
print("WARNING: Could not find NRF51 firmware ({}).".format(args.nrf51Fw))
def selected_cfs():
nodes = {name: node for name, node in cfg["robots"].items() if widgets[name].checked.get()}
return nodes
def save():
for name, node in cfg["robots"].items():
if widgets[name].checked.get():
node["enabled"] = True
else:
node["enabled"] = False
with open(args.configpath, 'w') as outfile:
yaml.dump(cfg, outfile)
yaml = YAML()
cfg = yaml.load(pathlib.Path(args.configpath))
cfTypes = cfg["robot_types"]
enabled = [name for name in cfg["robots"].keys() if cfg["robots"][name]["enabled"] == True]
# compute absolute pixel coordinates from the initial positions
positions = [node["initial_position"] for node in cfg["robots"].values()]
DOWN_DIR = [-1, 0]
RIGHT_DIR = [0, -1]
def dot(a, b):
return a[0] * b[0] + a[1] * b[1]
pixel_x = [120 * dot(pos, RIGHT_DIR) for pos in positions]
pixel_y = [120 * dot(pos, DOWN_DIR) for pos in positions]
xmin, ymin = min(pixel_x), min(pixel_y)
xmax, ymax = max(pixel_x), max(pixel_y)
# construct the main window
top = Tkinter.Tk()
top.title('Crazyflie Chooser')
# construct the frame containing the absolute-positioned checkboxes
width = xmax - xmin + 50 # account for checkbox + text width
height = ymax - ymin + 50 # account for checkbox + text height
frame = Tkinter.Frame(top, width=width, height=height)
class CFWidget(Tkinter.Frame):
def __init__(self, parent, name):
Tkinter.Frame.__init__(self, parent)
self.checked = Tkinter.BooleanVar()
checkbox = Tkinter.Checkbutton(self, variable=self.checked, command=save,
padx=0, pady=0)
checkbox.grid(row=0, column=0, sticky='E')
nameLabel = Tkinter.Label(self, text=name, padx=0, pady=0)
nameLabel.grid(row=0, column=1, sticky='W')
self.batteryLabel = Tkinter.Label(self, text="", fg="#999999", padx=0, pady=0)
self.batteryLabel.grid(row=1, column=0, columnspan=2, sticky='E')
self.versionLabel = Tkinter.Label(self, text="", fg="#999999", padx=0, pady=0)
self.versionLabel.grid(row=2, column=0, columnspan=2, sticky='E')
# construct all the checkboxes
widgets = {}
for (id, node), x, y in zip(cfg["robots"].items(), pixel_x, pixel_y):
w = CFWidget(frame, str(id))
w.place(x = x - xmin, y = y - ymin)
w.checked.set(id in enabled)
widgets[id] = w
# dragging functionality - TODO alt-drag to deselect
drag_start = None
drag_startstate = None
def minmax(a, b):
return min(a, b), max(a, b)
def mouseDown(event):
global drag_start, drag_startstate
drag_start = (event.x_root, event.y_root)
drag_startstate = [cf.checked.get() for cf in widgets.values()]
def mouseUp(event):
save()
def drag(event, select):
x, y = event.x_root, event.y_root
dragx0, dragx1 = minmax(drag_start[0], x)
dragy0, dragy1 = minmax(drag_start[1], y)
def dragcontains(widget):
x0 = widget.winfo_rootx()
y0 = widget.winfo_rooty()
x1 = x0 + widget.winfo_width()
y1 = y0 + widget.winfo_height()
return not (x0 > dragx1 or x1 < dragx0 or y0 > dragy1 or y1 < dragy0)
# depending on interation over dicts being consistent
for initial, cf in zip(drag_startstate, widgets.values()):
if dragcontains(cf):
cf.checked.set(select)
else:
cf.checked.set(initial)
top.bind('<ButtonPress-1>', mouseDown)
top.bind('<ButtonPress-3>', mouseDown)
top.bind('<B1-Motion>', lambda event: drag(event, True))
top.bind('<B3-Motion>', lambda event: drag(event, False))
top.bind('<ButtonRelease-1>', mouseUp)
top.bind('<ButtonRelease-3>', mouseUp)
# buttons for clearing/filling all checkboxes
def clear():
for box in widgets.values():
box.checked.set(False)
save()
def fill():
for box in widgets.values():
box.checked.set(True)
save()
def mkbutton(parent, name, command):
button = Tkinter.Button(parent, text=name, command=command)
button.pack(side='left')
buttons = Tkinter.Frame(top)
mkbutton(buttons, "Clear", clear)
mkbutton(buttons, "Fill", fill)
# construct bottom buttons for utility scripts
def sysOff():
nodes = selected_cfs()
for name, crazyflie in nodes.items():
uri = crazyflie["uri"]
subprocess.call(["ros2 run crazyflie reboot --uri " + uri + " --mode sysoff"], shell=True)
def reboot():
nodes = selected_cfs()
for name, crazyflie in nodes.items():
uri = crazyflie["uri"]
print(name)
subprocess.call(["ros2 run crazyflie reboot --uri " + uri], shell=True)
def flashSTM():
nodes = selected_cfs()
for name, crazyflie in nodes.items():
uri = crazyflie["uri"]
print("Flash STM32 FW to {}".format(uri))
subprocess.call(["ros2 run crazyflie flash --uri " + uri + " --target stm32 --filename " + args.stm32Fw], shell=True)
def flashNRF():
nodes = selected_cfs()
for name, crazyflie in nodes.items():
uri = crazyflie["uri"]
print("Flash NRF51 FW to {}".format(uri))
subprocess.call(["ros2 run crazyflie flash --uri " + uri + " --target nrf51 --filename " + args.nrf51Fw], shell=True)
def checkBattery():
# reset color
for id, w in widgets.items():
w.batteryLabel.config(foreground='#999999')
# query each CF
nodes = selected_cfs()
for name, crazyflie in nodes.items():
uri = crazyflie["uri"]
cfType = crazyflie["type"]
bigQuad = cfTypes[cfType]["big_quad"]
try:
if not bigQuad:
voltage = subprocess.check_output(["ros2 run crazyflie battery --uri " + uri], shell=True)
else:
voltage = subprocess.check_output(["ros2 run crazyflie battery --uri " + uri + " --external 1"], shell=True)
except subprocess.CalledProcessError:
voltage = None # CF not available
color = '#000000'
if voltage is not None:
voltage = float(voltage)
if voltage < cfTypes[cfType]["battery"]["voltage_warning"]:
color = '#FF8800'
if voltage < cfTypes[cfType]["battery"]["voltage_critical"]:
color = '#FF0000'
widgetText = "{:.2f} v".format(voltage)
else:
widgetText = "Err"
widgets[name].batteryLabel.config(foreground=color, text=widgetText)
# def checkVersion():
# for id, w in widgets.items():
# w.versionLabel.config(foreground='#999999')
# proc = subprocess.Popen(
# ['python3', SCRIPTDIR + 'version.py'], stdout=subprocess.PIPE)
# versions = dict()
# versionsCount = dict()
# versionForMost = None
# versionForMostCount = 0
# for line in iter(proc.stdout.readline, ''):
# print(line)
# match = re.search("(\d+): ([0-9a-fA-F]+),(\d),([0-9a-fA-F]+)", line)
# if match:
# addr = int(match.group(1))
# v1 = match.group(2)
# modified = int(match.group(3)) == 1
# v2 = match.group(4)
# v = (v1,v2)
# versions[addr] = v
# if v in versionsCount:
# versionsCount[v] += 1
# else:
# versionsCount[v] = 1
# if versionsCount[v] > versionForMostCount:
# versionForMostCount = versionsCount[v]
# versionForMost = v
# for addr, v in versions.items():
# color = '#000000'
# if v != versionForMost:
# color = '#FF0000'
# widgets[addr].versionLabel.config(foreground=color, text=str(v[0])[0:3] + "," + str(v[1])[0:3])
scriptButtons = Tkinter.Frame(top)
mkbutton(scriptButtons, "battery", checkBattery)
# currently not supported
# mkbutton(scriptButtons, "version", checkVersion)
mkbutton(scriptButtons, "sysOff", sysOff)
mkbutton(scriptButtons, "reboot", reboot)
# mkbutton(scriptButtons, "flash (STM)", flashSTM)
# mkbutton(scriptButtons, "flash (NRF)", flashNRF)
# start background threads
def checkBatteryLoop():
while True:
# rely on GIL
checkBattery()
time.sleep(10.0) # seconds
# checkBatteryThread = threading.Thread(target=checkBatteryLoop)
# checkBatteryThread.daemon = True # so it exits when the main thread exit
# checkBatteryThread.start()
# place the widgets in the window and start
buttons.pack()
frame.pack(padx=10, pady=10)
scriptButtons.pack()
top.mainloop()
================================================
FILE: crazyflie/scripts/crazyflie_server.py
================================================
#!/usr/bin/env python3
"""
A crazyflie server for communicating with several crazyflies
based on the official crazyflie python library from
Bitcraze AB
2022 - K. N. McGuire (Bitcraze AB)
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from rclpy.duration import Duration
import time
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 Poly4D
from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging
from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop
from crazyflie_interfaces.srv import Arm
from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType
from crazyflie_interfaces.msg import Position, Status, Hover, LogDataGeneric, FullState, VelocityWorld
from motion_capture_tracking_interfaces.msg import NamedPoseArray
from std_srvs.srv import Empty
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped, TransformStamped
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
import tf_transformations
from tf2_ros import TransformBroadcaster
from functools import partial
from math import degrees, radians, pi, isnan
type_cf_param_to_ros_param = {
"uint8_t": ParameterType.PARAMETER_INTEGER,
"uint16_t": ParameterType.PARAMETER_INTEGER,
"uint32_t": ParameterType.PARAMETER_INTEGER,
"int8_t": ParameterType.PARAMETER_INTEGER,
"int16_t": ParameterType.PARAMETER_INTEGER,
"int32_t": ParameterType.PARAMETER_INTEGER,
"FP16": ParameterType.PARAMETER_DOUBLE,
"float": ParameterType.PARAMETER_DOUBLE,
"double": ParameterType.PARAMETER_DOUBLE,
}
type_cf_param_to_index = {
'uint8_t': 0x08,
'uint16_t': 0x09,
'uint32_t': 0x0A,
'uint64_t': 0x0B,
'int8_t': 0x00,
'int16_t': 0x01,
'int32_t': 0x02,
'int64_t': 0x03,
'FP16': 0x05,
'float': 0x06,
'double': 0x07
}
class CrazyflieServer(Node):
def __init__(self):
super().__init__(
"crazyflie_server",
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
)
# Turn ROS parameters into a dictionary
self._ros_parameters = self._param_to_dict(self._parameters)
self.uris = []
# for logging, assign a all -> all mapping
self.cf_dict = {
'all': 'all'
}
self.uri_dict = {}
self.type_dict = {}
# Assign default topic types, variables and callbacks
self.default_log_type = {"pose": PoseStamped,
"scan": LaserScan,
"odom": Odometry,
"status": Status}
self.default_log_vars = {"pose": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z',
'stabilizer.roll', 'stabilizer.pitch', 'stabilizer.yaw'],
"scan": ['range.front', 'range.left', 'range.back', 'range.right'],
"odom": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z',
'stabilizer.yaw', 'stabilizer.roll', 'stabilizer.pitch',
'kalman.statePX', 'kalman.statePY', 'kalman.statePZ',
'gyro.z', 'gyro.x', 'gyro.y'],
"status": ['supervisor.info', 'pm.vbatMV', 'pm.state',
'radio.rssi']}
self.default_log_fnc = {"pose": self._log_pose_data_callback,
"scan": self._log_scan_data_callback,
"odom": self._log_odom_data_callback,
"status": self._log_status_data_callback}
world_tf_name = "world"
robot_yaml_version = 0
try:
robot_yaml_version = self._ros_parameters["fileversion"]
except KeyError:
self.get_logger().info("No fileversion found in crazyflies.yaml, assuming version 0")
# Check if the Crazyflie library is initialized
robot_data = self._ros_parameters["robots"]
# Init a transform broadcaster
self.tfbr = TransformBroadcaster(self)
# Create easy lookup tables for uri, name and types
for crazyflie in robot_data:
if robot_data[crazyflie]["enabled"]:
type_cf = robot_data[crazyflie]["type"]
# do not include virtual objects
connection = self._ros_parameters['robot_types'][type_cf].get(
"connection", "crazyflie")
if connection == "crazyflie":
uri = robot_data[crazyflie]["uri"]
self.uris.append(uri)
self.cf_dict[uri] = crazyflie
self.uri_dict[crazyflie] = uri
self.type_dict[uri] = type_cf
# Setup Swarm class cflib with connection callbacks and open the links
factory = CachedCfFactory(rw_cache="./cache")
self.swarm = Swarm(self.uris, factory=factory)
self.swarm.fully_connected_crazyflie_cnt = 0
self.swarm.connected_crazyflie_cnt = 0
# Check if parameter values needs to be uploaded and put on ROS 2 params
self.swarm.query_all_values_on_connect = self._ros_parameters["firmware_params"]["query_all_values_on_connect"]
# Initialize logging, services and parameters for each crazyflie
for link_uri in self.uris:
# Connect callbacks for different connection states of the crazyflie
self.swarm._cfs[link_uri].cf.fully_connected.add_callback(
self._fully_connected)
self.swarm._cfs[link_uri].cf.connected.add_callback(
self._connected)
self.swarm._cfs[link_uri].cf.disconnected.add_callback(
self._disconnected)
self.swarm._cfs[link_uri].cf.connection_failed.add_callback(
self._connection_failed
)
# link statistics from CFlib
self.swarm._cfs[link_uri].status = {}
self.swarm._cfs[link_uri].status["latency"] = 0.0
self.swarm._cfs[link_uri].cf.link_statistics.latency_updated.add_callback(partial(self._latency_callback, uri=link_uri))
self.swarm._cfs[link_uri].status["num_rx_unicast"] = 0.0
self.swarm._cfs[link_uri].cf.link_statistics.uplink_rate_updated.add_callback(partial(self._uplink_rate_callback, uri=link_uri))
self.swarm._cfs[link_uri].status["num_tx_unicast"] = 0.0
self.swarm._cfs[link_uri].cf.link_statistics.downlink_rate_updated.add_callback(partial(self._downlink_rate_callback, uri=link_uri))
# check if logging is enabled at startup
self.swarm._cfs[link_uri].logging = {}
cf_name = self.cf_dict[link_uri]
cf_type = self.type_dict[link_uri]
logging_enabled = False
try:
logging_enabled = self._ros_parameters['all']["firmware_logging"]["enabled"]
except KeyError:
pass
try:
logging_enabled = self._ros_parameters['robot_types'][cf_type]["firmware_logging"]["enabled"]
except KeyError:
pass
try:
logging_enabled = self._ros_parameters['robots'][cf_name]["firmware_logging"]["enabled"]
except KeyError:
pass
self.swarm._cfs[link_uri].logging["enabled"] = logging_enabled
# check if predefine log blocks can be logged and setup crazyflie logblocks and ROS 2 publishers
for default_log_name in self.default_log_type:
prefix = default_log_name
topic_type = self.default_log_type[default_log_name]
list_logvar = self.default_log_vars[default_log_name]
self._init_default_logblocks(
prefix, link_uri, list_logvar, logging_enabled, topic_type)
# Check for any custom_log topics
custom_logging_enabled = False
custom_log_topics = {}
try:
custom_log_topics = self._ros_parameters['all']["firmware_logging"]["custom_topics"]
custom_logging_enabled = True
except KeyError:
pass
try:
custom_log_topics.update(
self._ros_parameters['robot_types'][cf_type]["firmware_logging"]["custom_topics"])
custom_logging_enabled = True
except KeyError:
pass
try:
custom_log_topics.update(
self._ros_parameters['robots'][cf_name]["firmware_logging"]["custom_topics"])
custom_logging_enabled = True
except KeyError:
pass
self.swarm._cfs[link_uri].logging["custom_log_topics"] = {}
self.swarm._cfs[link_uri].logging["custom_log_groups"] = {}
self.swarm._cfs[link_uri].logging["custom_log_publisher"] = {}
# Setup log blocks for each custom log and ROS 2 publisher topics
if custom_logging_enabled:
for log_group_name in custom_log_topics:
frequency = custom_log_topics[log_group_name]["frequency"]
lg_custom = LogConfig(
name=log_group_name, period_in_ms=1000 / frequency)
for log_name in custom_log_topics[log_group_name]["vars"]:
lg_custom.add_variable(log_name)
# Don't know which type this needs to be in until we get the full toc
self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_group_name] = "empty publisher"
self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name] = {
}
self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name]["log_config"] = lg_custom
self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name]["vars"] = custom_log_topics[log_group_name]["vars"]
self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name][
"frequency"] = custom_log_topics[log_group_name]["frequency"]
reference_frame = world_tf_name
# if larger then 3, then the reference frame is not set in the yaml file
if robot_yaml_version >= 3:
try:
reference_frame =self._ros_parameters['all']["reference_frame"]
except KeyError:
pass
try:
reference_frame =self._ros_parameters['robot_types'][robot_data[cf_name]['type']]["reference_frame"]
except KeyError:
pass
try:
reference_frame =self._ros_parameters['robots'][cf_name]["reference_frame"]
except KeyError:
pass
self.swarm._cfs[link_uri].reference_frame = reference_frame
# Now all crazyflies are initialized, open links!
try:
self.time_open_link = self.get_clock().now().nanoseconds * 1e-9
self.swarm.open_links()
except Exception as e:
# Close node if one of the Crazyflies can not be found
self.get_logger().info("Error!: One or more Crazyflies can not be found. ")
self.get_logger().info("Check if you got the right URIs, if they are turned on" +
" or if your script have proper access to a Crazyradio PA")
exit()
def _init_topics_and_services(self):
# Create services for the entire swarm and each individual crazyflie
for uri in self.cf_dict:
if uri == "all":
continue
name = self.cf_dict[uri]
pub = self.create_publisher(String, name + '/robot_description',
rclpy.qos.QoSProfile(
depth=1,
durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL))
msg = String()
msg.data = self._ros_parameters['robot_description'].replace("$NAME", name)
pub.publish(msg)
self.create_service(
Empty, name +
"/emergency", partial(self._emergency_callback, uri=uri)
)
self.create_service(
Arm, name +
"/arm", partial(self._arm_callback, uri=uri)
)
self.create_service(
Takeoff, name +
"/takeoff", partial(self._takeoff_callback, uri=uri)
)
self.create_service(
Land, name + "/land", partial(self._land_callback, uri=uri)
)
self.create_service(
GoTo, name + "/go_to", partial(self._go_to_callback, uri=uri)
)
self.create_service(
StartTrajectory, name +
"/start_trajectory", partial(
self._start_trajectory_callback, uri=uri)
)
self.create_service(
UploadTrajectory, name +
"/upload_trajectory", partial(
self._upload_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 +
"/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed,
uri=uri), 10
)
self.create_subscription(
Position, name +
"/cmd_position", partial(self._cmd_position_changed, uri=uri), 10
)
self.create_subscription(
VelocityWorld, name +
"/cmd_velocity_world", partial(self._cmd_velocity_world_changed, uri=uri), 10
)
self.create_subscription(
Hover, name +
"/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10
)
self.create_subscription(
FullState, name +
"/cmd_full_state", partial(self._cmd_full_state_changed, uri=uri), 10
)
qos_profile = QoSProfile(reliability =QoSReliabilityPolicy.BEST_EFFORT,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1,
deadline = Duration(seconds=0, nanoseconds=1e9/100.0))
self.create_subscription(
NamedPoseArray, "/poses",
self._poses_changed, qos_profile
)
self.create_service(Arm, "all/arm", self._arm_callback)
self.create_service(Takeoff, "all/takeoff", self._takeoff_callback)
self.create_service(Land, "all/land", self._land_callback)
self.create_service(GoTo, "all/go_to", self._go_to_callback)
self.create_service(
StartTrajectory, "all/start_trajectory", self._start_trajectory_callback)
# This is the last service to announce and can be used to check if the server is fully available
self.create_service(Empty, "all/emergency", self._emergency_callback)
def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_enabled, topic_type):
"""
Prepare default logblocks as defined in crazyflies.yaml
"""
cf_name = self.cf_dict[link_uri]
cf_type = self.type_dict[link_uri]
logging_enabled = False
logging_freq = 10
try:
logging_freq = self._ros_parameters['all'][
"firmware_logging"]["default_topics"][prefix]["frequency"]
logging_enabled = True
except KeyError:
pass
try:
logging_freq = self._ros_parameters['robot_types'][cf_type][
"firmware_logging"]["default_topics"][prefix]["frequency"]
logging_enabled = True
except KeyError:
pass
try:
logging_freq = self._ros_parameters['robots'][cf_name][
"firmware_logging"]["default_topics"][prefix]["frequency"]
logging_enabled = True
except KeyError:
pass
lg = LogConfig(
name=prefix, period_in_ms=1000 / logging_freq)
for logvar in list_logvar:
if prefix == "odom":
lg.add_variable(logvar, "FP16")
else:
lg.add_variable(logvar)
self.swarm._cfs[link_uri].logging[prefix +
"_logging_enabled"] = logging_enabled
self.swarm._cfs[link_uri].logging[prefix +
"_logging_freq"] = logging_freq
self.swarm._cfs[link_uri].logging[prefix + "_log_config"] = lg
if logging_enabled and global_logging_enabled:
self.swarm._cfs[link_uri].logging[prefix + "_publisher"] = self.create_publisher(
topic_type, self.cf_dict[link_uri] + "/" + prefix, 10)
else:
self.swarm._cfs[link_uri].logging[prefix + "_publisher"] = "empty"
def _param_to_dict(self, param_ros):
"""
Turn ROS 2 parameters from the node into a dict
"""
tree = {}
for item in param_ros:
t = tree
for part in item.split('.'):
if part == item.split('.')[-1]:
t = t.setdefault(part, param_ros[item].value)
else:
t = t.setdefault(part, {})
return tree
def _latency_callback(self, latency, uri=""):
"""
Called when the latency of the Crazyflie is updated
"""
self.swarm._cfs[uri].status["latency"] = latency
def _uplink_rate_callback(self, uplink_rate, uri=""):
"""
Called when the uplink rate of the Crazyflie is updated
"""
self.swarm._cfs[uri].status["num_rx_unicast"] = uplink_rate
def _downlink_rate_callback(self, downlink_rate, uri=""):
"""
Called when the uplink rate of the Crazyflie is updated
"""
self.swarm._cfs[uri].status["num_tx_unicast"] = downlink_rate
def _connected(self, link_uri):
"""
Called when the toc of the parameters and
logs has been received of the Crazyflie
"""
self.get_logger().info(f"[{self.cf_dict[link_uri]}] is connected!")
self.swarm.connected_crazyflie_cnt += 1
if self.swarm.connected_crazyflie_cnt == len(self.cf_dict) - 1:
self.time_all_crazyflie_connected = self.get_clock().now().nanoseconds * 1e-9
self.get_logger().info(f"All Crazyflies are connected! It took {self.time_all_crazyflie_connected - self.time_open_link} seconds")
self._init_topics_and_services()
self._init_logging()
if not self.swarm.query_all_values_on_connect:
self._init_parameters()
self.add_on_set_parameters_callback(self._parameters_callback)
else:
return
def _fully_connected(self, link_uri):
"""
Called the full log toc and parameter + values
has been received from the Crazyflie
"""
self.get_logger().info(f"[{self.cf_dict[link_uri]}] is fully connected!")
self.swarm.fully_connected_crazyflie_cnt += 1
# use len(self.cf_dict) - 1, since cf_dict contains "all" as well
if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict) - 1:
self.time_all_crazyflie_connected = self.get_clock().now().nanoseconds * 1e-9
self.get_logger().info(f"All Crazyflies are fully connected! It took {self.time_all_crazyflie_connected - self.time_open_link} seconds")
if self.swarm.query_all_values_on_connect:
self._init_parameters()
self.add_on_set_parameters_callback(self._parameters_callback)
else:
return
def _disconnected(self, link_uri):
self.get_logger().info(f"[{self.cf_dict[link_uri]}] is disconnected!")
def _connection_failed(self, link_uri, msg):
self.get_logger().info(f"[{self.cf_dict[link_uri]}] connection Failed")
self.swarm.close_links()
def _init_logging(self):
"""
Sets up all the log blocks for the crazyflie and
all the ROS 2 publisher and parameters for logging
at startup
"""
for link_uri in self.uris:
cf_handle = self.swarm._cfs[link_uri]
cf = cf_handle.cf
# Start logging for predefined logging
for default_log_name in self.default_log_type:
prefix = default_log_name
if cf_handle.logging[prefix + "_logging_enabled"] and cf_handle.logging["enabled"]:
callback_fnc = self.default_log_fnc[prefix]
self._init_default_logging(prefix, link_uri, callback_fnc)
# Start logging for costum logging blocks
cf_handle.l_toc = cf.log.toc.toc
if len(cf_handle.logging["custom_log_groups"]) != 0 and cf_handle.logging["enabled"]:
for log_group_name, log_group_dict in cf_handle.logging["custom_log_groups"].items():
self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_group_name] = self.create_publisher(
LogDataGeneric, self.cf_dict[link_uri] + "/" + log_group_name, 10)
lg_custom = log_group_dict['log_config']
try:
cf.log.add_config(lg_custom)
lg_custom.data_received_cb.add_callback(
partial(self._log_custom_data_callback, uri=link_uri))
lg_custom.error_cb.add_callback(
self._log_error_callback)
lg_custom.start()
except KeyError as e:
self.get_logger().info(f'[{self.cf_dict[link_uri]}] Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
self.get_logger().info(
f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.')
self.get_logger().info(f"[{self.cf_dict[link_uri]}] setup custom logging")
self.create_service(
RemoveLogging, self.cf_dict[link_uri] + "/remove_logging", partial(self._remove_logging, uri=link_uri))
self.create_service(
AddLogging, self.cf_dict[link_uri] + "/add_logging", partial(self._add_logging, uri=link_uri))
self.get_logger().info("All Crazyflies logging are initialized.")
def _init_default_logging(self, prefix, link_uri, callback_fnc):
"""
Sets up all the default log blocks and ROS 2 publishers for the crazyflie
"""
cf_handle = self.swarm._cfs[link_uri]
cf = cf_handle.cf
lg = cf_handle.logging[prefix + "_log_config"]
try:
cf.log.add_config(lg)
lg.data_received_cb.add_callback(
partial(callback_fnc, uri=link_uri))
lg.error_cb.add_callback(self._log_error_callback)
lg.start()
frequency = cf_handle.logging[prefix + "_logging_freq"]
self.declare_parameter(
self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency)
self.get_logger().info(
f"[{self.cf_dict[link_uri]}] setup logging for {prefix} at freq {frequency}")
except KeyError as e:
self.get_logger().error(f'[{self.cf_dict[link_uri]}] Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
self.get_logger().error(
f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.')
def _log_scan_data_callback(self, timestamp, data, logconf, uri):
"""
Once multiranger range is retrieved from the Crazyflie,
send out the ROS 2 topic for Scan
"""
cf_name = self.cf_dict[uri]
max_range = 3.49
front_range = float(data.get('range.front'))/1000.0
left_range = float(data.get('range.left'))/1000.0
back_range = float(data.get('range.back'))/1000.0
right_range = float(data.get('range.right'))/1000.0
if front_range > max_range:
front_range = float("inf")
if left_range > max_range:
left_range = float("inf")
if right_range > max_range:
right_range = float("inf")
if back_range > max_range:
back_range = float("inf")
self.ranges = [back_range, right_range, front_range, left_range]
msg = LaserScan()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = cf_name
msg.range_min = 0.01
msg.range_max = 3.49
msg.ranges = self.ranges
msg.angle_min = -0.5 * 2 * pi
msg.angle_max = 0.25 * 2 * pi
msg.angle_increment = 1.0 * pi/2
try:
self.swarm._cfs[uri].logging["scan_publisher"].publish(msg)
except:
self.get_logger().info("Could not publish scan message, stopping scan log")
self.swarm._cfs[uri].logging["scan_log_config"].stop()
def _log_pose_data_callback(self, timestamp, data, logconf, uri):
"""
Once pose data is retrieved from the Crazyflie,
send out the ROS 2 topic for Pose
"""
cf_name = self.cf_dict[uri]
x = data.get('stateEstimate.x')
y = data.get('stateEstimate.y')
z = data.get('stateEstimate.z')
roll = radians(data.get('stabilizer.roll'))
pitch = radians(-1.0 * data.get('stabilizer.pitch'))
yaw = radians(data.get('stabilizer.yaw'))
q = tf_transformations.quaternion_from_euler(roll, pitch, yaw)
msg = PoseStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = self.swarm._cfs[uri].reference_frame
msg.pose.position.x = x
msg.pose.position.y = y
msg.pose.position.z = z
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1]
msg.pose.orientation.z = q[2]
msg.pose.orientation.w = q[3]
try:
self.swarm._cfs[uri].logging["pose_publisher"].publish(msg)
except:
self.get_logger().info("Could not publish pose message, stopping pose log")
self.swarm._cfs[uri].logging["pose_log_config"].stop()
t_base = TransformStamped()
t_base.header.stamp = self.get_clock().now().to_msg()
t_base.header.frame_id = self.swarm._cfs[uri].reference_frame
t_base.child_frame_id = cf_name
t_base.transform.translation.x = x
t_base.transform.translation.y = y
t_base.transform.translation.z = z
t_base.transform.rotation.x = q[0]
t_base.transform.rotation.y = q[1]
t_base.transform.rotation.z = q[2]
t_base.transform.rotation.w = q[3]
try:
self.tfbr.sendTransform(t_base)
except:
self.get_logger().info("Could not publish pose tf")
def _log_odom_data_callback(self, timestamp, data, logconf, uri):
"""
Once pose and velocity data is retrieved from the Crazyflie,
send out the ROS 2 topic for Odometry in 2D (no z-axis)
"""
cf_name = self.cf_dict[uri]
x = data.get('stateEstimate.x')
y = data.get('stateEstimate.y')
z = data.get('stateEstimate.z')
yaw = radians(data.get('stabilizer.yaw'))
roll = radians(data.get('stabilizer.roll'))
pitch = radians(data.get('stabilizer.pitch'))
vx = data.get('kalman.statePX')
vy = data.get('kalman.statePY')
vz = data.get('kalman.statePZ')
yawrate = data.get('gyro.z')
rollrate = data.get('gyro.x')
pitchrate = data.get('gyro.y')
q = tf_transformations.quaternion_from_euler(roll, pitch, yaw)
msg = Odometry()
msg.child_frame_id = cf_name
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = self.swarm._cfs[uri].reference_frame
msg.pose.pose.position.x = x
msg.pose.pose.position.y = y
msg.pose.pose.position.z = z
msg.pose.pose.orientation.x = q[0]
msg.pose.pose.orientation.y = q[1]
msg.pose.pose.orientation.z = q[2]
msg.pose.pose.orientation.w = q[3]
msg.twist.twist.linear.x = vx
msg.twist.twist.linear.y = vy
msg.twist.twist.linear.z = vz
msg.twist.twist.angular.z = yawrate
msg.twist.twist.angular.y = pitchrate
msg.twist.twist.angular.x = rollrate
try:
self.swarm._cfs[uri].logging["odom_publisher"].publish(msg)
except:
self.get_logger().info("Could not publish odom message, stopping odom log")
self.swarm._cfs[uri].logging["odom_log_config"].stop()
t_base = TransformStamped()
t_base.header.stamp = self.get_clock().now().to_msg()
t_base.header.frame_id = self.swarm._cfs[uri].reference_frame
t_base.child_frame_id = cf_name
t_base.transform.translation.x = x
t_base.transform.translation.y = y
t_base.transform.translation.z = z
t_base.transform.rotation.x = q[0]
t_base.transform.rotation.y = q[1]
t_base.transform.rotation.z = q[2]
t_base.transform.rotation.w = q[3]
try:
self.tfbr.sendTransform(t_base)
except:
self.get_logger().info("Could not publish odom tf")
def _log_status_data_callback(self, timestamp, data, logconf, uri):
"""
Send out the ROS 2 status topic
"""
msg = Status()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = self.swarm._cfs[uri].reference_frame
# From logging statistics
msg.supervisor_info = data.get('supervisor.info')
msg.battery_voltage = data.get('pm.vbatMV') / 1000.0
msg.pm_state = data.get('pm.state')
msg.rssi = data.get('radio.rssi')
# From link statistics class
msg.latency_unicast = int(self.swarm._cfs[uri].status["latency"])
msg.num_rx_unicast = int(self.swarm._cfs[uri].status["num_rx_unicast"])
msg.num_tx_unicast = int(self.swarm._cfs[uri].status["num_tx_unicast"])
try:
self.swarm._cfs[uri].logging["status_publisher"].publish(msg)
except:
self.get_logger().info("Could not publish status message, stopping status log")
self.swarm._cfs[uri].logging["status_log_config"].stop()
def _log_custom_data_callback(self, timestamp, data, logconf, uri):
"""
Once custom log block is retrieved from the Crazyflie,
send out the ROS 2 topic for that same type of log
"""
msg = LogDataGeneric()
msg.header.stamp = self.get_clock().now().to_msg()
msg.timestamp = timestamp
for log_name in data:
msg.values.append(data.get(log_name))
try:
self.swarm._cfs[uri].logging["custom_log_publisher"][logconf.name].publish(
msg)
except:
self.get_logger().info("Could not publish custom {logconf.name} message, stopping custom log")
self.swarm._cfs[uri].logging["custom_log_groups"][logconf.name]["log_config"].stop()
def _log_error_callback(self, logconf, msg):
print('Error when logging %s: %s' % (logconf.name, msg))
def _init_parameters(self):
"""
Once custom log block is retrieved from the Crazyflie,
send out the ROS 2 topic for that same type of log
"""
set_param_to_ROS = self.swarm.query_all_values_on_connect
for link_uri in self.uris:
cf = self.swarm._cfs[link_uri].cf
p_toc = cf.param.toc.toc
for group in sorted(p_toc.keys()):
for param in sorted(p_toc[group].keys()):
name = group + "." + param
# Check the parameter type
elem = p_toc[group][param]
type_cf_param = elem.ctype
parameter_descriptor = ParameterDescriptor(
type=type_cf_param_to_ros_param[type_cf_param])
# Check ros parameters if an parameter should be set
# Parameter sets for individual robots has priority,
# then robot types, then all (all robots)
param_value = None
try:
param_value = self._ros_parameters["all"]["firmware_params"][group][param]
except KeyError:
pass
try:
param_value = self._ros_parameters["robot_types"][self.cf_dict[link_uri]
]["firmware_params"][group][param]
except KeyError:
pass
try:
param_value = self._ros_parameters["robots"][self.cf_dict[link_uri]
]["firmware_params"][group][param]
except KeyError:
pass
if param_value is not None:
# If value is found in initial parameters,
# set crazyflie firmware value and declare value in ROS 2 parameter
# Note: currently this is not possible to get the most recent from the
# crazyflie with get_value due to threading.
cf.param.set_value_raw(name, type_cf_param_to_index[type_cf_param], param_value)
self.get_logger().info(
f"[{self.cf_dict[link_uri]}] {name} is set to {param_value}"
)
if set_param_to_ROS:
self.declare_parameter(
self.cf_dict[link_uri] +
".params." + group + "." + param,
value=param_value,
descriptor=parameter_descriptor,
)
else:
# If value is not found in initial parameter set
# get crazyflie paramter value and declare that value in ROS 2 parameter
# Only do this if this has been indicated by the user
if set_param_to_ROS is True:
if type_cf_param_to_ros_param[type_cf_param] is ParameterType.PARAMETER_INTEGER:
cf_param_value = int(cf.param.get_value(name))
elif type_cf_param_to_ros_param[type_cf_param] is ParameterType.PARAMETER_DOUBLE:
cf_param_value = float(cf.param.get_value(name))
self.declare_parameter(
self.cf_dict[link_uri] +
".params." + group + "." + param,
value=cf_param_value,
descriptor=parameter_descriptor,
)
# Based on the parameters from the last Crazyflie, set params for all
# Warning: if any of the other crazyflies have different parameters
# this will result in an error
try:
self.declare_parameter(
"all.params." + group + "." + param,
value=cf_param_value,
descriptor=parameter_descriptor,
)
except Exception as e:
continue
self.get_logger().info("All Crazyflies parameters are initialized.")
def _parameters_callback(self, params):
"""
Sets up all the parameters for the crazyflie and
translates it to ROS 2 paraemeters at startup
"""
for param in params:
param_split = param.name.split(".")
if param_split[0] == "all":
if param_split[1] == "params":
name_param = param_split[2] + "." + param_split[3]
try:
for link_uri in self.uris:
cf = self.swarm._cfs[link_uri].cf.param.set_value(
name_param, param.value
)
self.get_logger().info(
f"[{self.cf_dict[link_uri]}] {name_param} is set to {param.value}"
)
return SetParametersResult(successful=True)
except Exception as e:
self.get_logger().info(str(e))
return SetParametersResult(successful=False)
elif param_split[0] in self.cf_dict.values():
cf_name = param_split[0]
if param_split[1] == "params":
name_param = param_split[2] + "." + param_split[3]
try:
self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value(
name_param, param.value
)
self.get_logger().info(
f"[{self.uri_dict[cf_name]}] {name_param} is set to {param.value}"
)
return SetParametersResult(successful=True)
except Exception as e:
self.get_logger().info(str(e))
return SetParametersResult(successful=False)
if param_split[1] == "logs":
return SetParametersResult(successful=True)
return SetParametersResult(successful=False)
def _emergency_callback(self, request, response, uri="all"):
if uri == "all":
for link_uri in self.uris:
self.swarm._cfs[link_uri].cf.loc.send_emergency_stop()
else:
self.swarm._cfs[uri].cf.loc.send_emergency_stop()
return response
def _arm_callback(self, request, response, uri="all"):
"""
Service callback to arm or disarm the Crazyflie
"""
arm_bool = request.arm
self.get_logger().info(
f"[{self.cf_dict[uri]}] Arm request is {arm_bool} "
)
if uri == "all":
for link_uri in self.uris:
self.swarm._cfs[link_uri].cf.platform.send_arming_request(
arm_bool
)
else:
self.swarm._cfs[uri].cf.platform.send_arming_request(
arm_bool
)
return response
def _takeoff_callback(self, request, response, uri="all"):
"""
Service callback to take the crazyflie land to
a certain height in high level commander
"""
duration = float(request.duration.sec) + \
float(request.duration.nanosec / 1e9)
self.get_logger().info(
f"[{self.cf_dict[uri]}] takeoff(height={request.height} m,"
+ f"duration={duration} s,"
+ f"group_mask={request.group_mask})"
)
if uri == "all":
for link_uri in self.uris:
self.swarm._cfs[link_uri].cf.high_level_commander.takeoff(
request.height, duration
)
else:
self.swarm._cfs[uri].cf.high_level_commander.takeoff(
request.height, duration
)
return response
def _land_callback(self, request, response, uri="all"):
"""
Service callback to make the crazyflie land to
a certain height in high level commander
"""
duration = float(request.duration.sec) + \
float(request.duration.nanosec / 1e9)
self.get_logger().info(
f"[{self.cf_dict[uri]}] land(height={request.height} m,"
+ f"duration={duration} s,"
+ f"group_mask={request.group_mask})"
)
if uri == "all":
for link_uri in self.uris:
self.swarm._cfs[link_uri].cf.high_level_commander.land(
request.height, duration, group_mask=request.group_mask
)
else:
self.swarm._cfs[uri].cf.high_level_commander.land(
request.height, duration, group_mask=request.group_mask
)
return response
def _go_to_callback(self, request, response, uri="all"):
"""
Service callback to have the crazyflie go to
a certain position in high level commander
"""
duration = float(request.duration.sec) + \
float(request.duration.nanosec / 1e9)
self.get_logger().info(
"[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)"
% (
self.cf_dict[uri],
request.goal.x,
request.goal.y,
request.goal.z,
request.yaw,
duration,
request.relative,
request.group_mask,
)
)
if uri == "all":
for link_uri in self.uris:
self.swarm._cfs[link_uri].cf.high_level_commander.go_to(
request.goal.x,
request.goal.y,
request.goal.z,
request.yaw,
duration,
relative=request.relative,
group_mask=request.group_mask,
)
else:
self.swarm._cfs[uri].cf.high_level_commander.go_to(
request.goal.x,
request.goal.y,
request.goal.z,
request.yaw,
duration,
relative=request.relative,
group_mask=request.group_mask,
)
return response
def _notify_setpoints_stop_callback(self, request, response, uri="all"):
self.get_logger().info(f"[{self.cf_dict[uri]}] Received notify setpoint stop")
if uri == "all":
for link_uri in self.uris:
self.swarm._cfs[link_uri].cf.commander.send_notify_setpoint_stop()
else:
self.swarm._cfs[uri].cf.commander.send_notify_setpoint_stop()
return response
def _upload_trajectory_callback(self, request, response, uri="all"):
id = request.trajectory_id
offset = request.piece_offset
lenght = len(request.pieces)
total_duration = 0
self.get_logger().info("[%s] upload_trajectory(id=%d,offset=%d, lenght=%d)" % (
self.cf_dict[uri],
id,
offset,
lenght,
))
trajectory = []
for i in range(lenght):
piece = request.pieces[i]
px = Poly4D.Poly(piece.poly_x)
py = Poly4D.Poly(piece.poly_y)
pz = Poly4D.Poly(piece.poly_z)
pyaw = Poly4D.Poly(piece.poly_yaw)
duration = float(piece.duration.sec) + \
float(piece.duration.nanosec)/1e9
trajectory.append(Poly4D(duration, px, py, pz, pyaw))
total_duration = 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 failed")
upload_success_all = False
else:
self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory(
id, offset, len(trajectory))
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 failed")
response.success = False
return response
self.swarm._cfs[uri].cf.high_level_commander.define_trajectory(
id, offset, len(trajectory))
return response
def _start_trajectory_callback(self, request, response, uri="all"):
id = request.trajectory_id
ts = request.timescale
rel = request.relative
rev = request.reversed
gm = request.group_mask
self.get_logger().info("[%s] start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" % (
self.cf_dict[uri],
id,
ts,
rel,
rev,
gm
))
if uri == "all":
for link_uri in self.uris:
self.swarm._cfs[link_uri].cf.high_level_commander.start_trajectory(
id, ts, rel, rev, gm)
else:
self.swarm._cfs[uri].cf.high_level_commander.start_trajectory(
id, ts, rel, rev, gm)
return response
def _poses_changed(self, msg):
"""
Topic update callback to the motion capture lib's
poses topic to send through the external position
to the crazyflie
"""
poses = msg.poses
for pose in poses:
name = pose.name
x = pose.pose.position.x
y = pose.pose.position.y
z = pose.pose.position.z
quat = pose.pose.orientation
if name in self.uri_dict.keys():
uri = self.uri_dict[name]
# self.get_logger().info(f"{uri}: send extpos {x}, {y}, {z} to {name}")
if isnan(quat.x):
self.swarm._cfs[uri].cf.extpos.send_extpos(
x, y, z)
else:
self.swarm._cfs[uri].cf.extpos.send_extpose(
x, y, z, quat.x, quat.y, quat.z, quat.w)
def _cmd_vel_legacy_changed(self, msg, uri=""):
"""
Topic update callback to control the attitude and thrust
of the crazyflie with teleop
"""
if not hasattr(self.swarm._cfs[uri], 'legacy_initialized'):
self.swarm._cfs[uri].cf.commander.send_setpoint(0.0, 0.0, 0.0, 0)
time.sleep(0.01)
self.swarm._cfs[uri].legacy_initialized = True
roll = msg.linear.y
pitch = -msg.linear.x
yawrate = msg.angular.z
thrust = int(min(max(msg.linear.z, 0), 60000))
self.swarm._cfs[uri].cf.commander.send_setpoint(
roll, pitch, yawrate, thrust)
def _cmd_position_changed(self, msg, uri=""):
"""
Topic update callback to control the position command
of the crazyflie
"""
x = msg.x
y = msg.y
z = msg.z
yaw = msg.yaw
self.swarm._cfs[uri].cf.commander.send_position_setpoint(
x, y, z, yaw)
def _cmd_velocity_world_changed(self, msg, uri=""):
"""
Topic update callback to control the world velocity command
of the crazyflie
"""
vel = msg.vel
vx = vel.x
vy = vel.y
vz = vel.z
yaw_rate = msg.yaw_rate
self.swarm._cfs[uri].cf.commander.send_velocity_world_setpoint(
vx, vy, vz, yaw_rate)
def _cmd_hover_changed(self, msg, uri=""):
"""
Topic update callback to control the hover command
of the crazyflie from the velocity multiplexer (vel_mux)
"""
vx = msg.vx
vy = msg.vy
z = msg.z_distance
yawrate = -1.0*degrees(msg.yaw_rate)
self.swarm._cfs[uri].cf.commander.send_hover_setpoint(
vx, vy, yawrate, z)
def _cmd_full_state_changed(self, msg, uri=""):
"""
Topic update callback to full state cmd topic
"""
pos = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
vel = [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z]
acc = [msg.acc.x, msg.acc.y, msg.acc.z]
q = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w]
roll_rate = msg.twist.angular.x
pitch_rate = msg.twist.angular.y
yaw_rate = msg.twist.angular.z
self.swarm._cfs[uri].cf.commander.send_full_state_setpoint(pos, vel, acc, q, roll_rate, pitch_rate, yaw_rate)
def _remove_logging(self, request, response, uri="all"):
"""
Service callback to remove logging blocks of the crazyflie
"""
topic_name = request.topic_name
if topic_name in self.default_log_type.keys():
try:
self.undeclare_parameter(
self.cf_dict[uri] + ".logs." + topic_name + ".frequency.")
self.swarm._cfs[uri].logging[topic_name + "_log_config"].stop()
self.destroy_publisher(
self.swarm._cfs[uri].logging[topic_name + "_publisher"])
self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging")
except rclpy.exceptions.ParameterNotDeclaredException:
self.get_logger().info(
f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ")
response.success = False
return response
else:
try:
self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"].stop(
)
for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"]:
self.destroy_publisher(
self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name])
self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging")
except rclpy.exceptions.ParameterNotDeclaredException:
self.get_logger().info(
f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ")
response.success = False
return response
response.success = True
return response
def _add_logging(self, request, response, uri="all"):
"""
Service callback to add logging blocks of the crazyflie
"""
topic_name = request.topic_name
frequency = request.frequency
variables = request.vars
if topic_name in self.default_log_type.keys():
try:
self.declare_parameter(
self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency)
self.swarm._cfs[uri].logging[topic_name + "_publisher"] = self.create_publisher(
self.default_log_type[topic_name], self.cf_dict[uri] + "/" + topic_name, 10)
self.swarm._cfs[uri].logging[topic_name +
"_log_config"].period_in_ms = 1000 / frequency
self.swarm._cfs[uri].logging[topic_name +
"_log_config"].start()
self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging")
except rclpy.exceptions.ParameterAlreadyDeclaredException:
self.get_logger().info(
f"[{self.cf_dict[uri]}] The content the logging of {topic_name} has already started ")
response.success = False
return response
else:
try:
self.declare_parameter(
self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency)
self.declare_parameter(
self.cf_dict[uri] + ".logs." + topic_name + ".vars.", variables)
lg_custom = LogConfig(
name=topic_name, period_in_ms=1000 / frequency)
for log_name in variables:
lg_custom.add_variable(log_name)
self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] = self.create_publisher(
LogDataGeneric, self.cf_dict[uri] + "/" + topic_name, 10)
self.swarm._cfs[uri].cf.log.add_config(lg_custom)
lg_custom.data_received_cb.add_callback(
partial(self._log_custom_data_callback, uri=uri))
lg_custom.error_cb.add_callback(self._log_error_callback)
lg_custom.start()
self.swarm._cfs[uri].logging["custom_log_groups"][topic_name] = {}
self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"] = lg_custom
self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"] = variables
self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["frequency"] = frequency
self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging")
except KeyError as e:
self.get_logger().error(
f"[{self.cf_dict[uri]}] Failed to add {topic_name} logging")
self.get_logger().error(str(e) + "is not in TOC")
self.undeclare_parameter(
self.cf_dict[uri] + ".logs." + topic_name + ".frequency.")
self.undeclare_parameter(
self.cf_dict[uri] + ".logs." + topic_name + ".vars.")
response.success = False
return response
except rclpy.exceptions.ParameterAlreadyDeclaredException:
self.get_logger().error(
f"[{self.cf_dict[uri]}] The content or part of the logging of {topic_name} has already started ")
response.success = False
return response
response.success = True
return response
def main(args=None):
cflib.crtp.init_drivers()
rclpy.init(args=args)
crazyflie_server = CrazyflieServer()
rclpy.spin(crazyflie_server)
crazyflie_server.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
================================================
FILE: crazyflie/scripts/flash.py
================================================
#!/usr/bin/env python3
"""
A flash utility script to flash crazyflies with the latest or custom firmware
2024 - K. N. McGuire (Bitcraze AB)
"""
import rclpy
from rclpy.node import Node
import cflib.crtp # noqa
from cflib.bootloader import Bootloader, Target
from cflib.bootloader.boottypes import BootVersion
import argparse
import os
class Flash(Node):
def __init__(self, uri, file_name):
super().__init__('flash')
self.get_logger().info(f"Flashing {uri} with {file_name}")
base_file_name = os.path.basename(file_name)
if base_file_name.endswith("zip") and base_file_name.startswith("firmware-cf2"):
targets = []
elif base_file_name.endswith("bin") and base_file_name.startswith("cf2"):
targets = [Target("cf2", 'stm32', 'fw', [], [])]
else:
self.get_logger().error(f"Unsupported file type or name. Only cf2*.bin or firmware-cf2*.zip supported")
return
bl = Bootloader(uri)
try:
bl.flash_full(None, file_name, True, targets)
except Exception as e:
self.get_logger().error(f"Failed to flash, Error {str(e)}")
finally:
if bl:
bl.close()
return
def main(args=None):
cflib.crtp.init_drivers()
rclpy.init(args=args)
parser = argparse.ArgumentParser(description="This is a sample script")
parser.add_argument('--uri', type=str, default="radio://0/80/2M/E7E7E7E7E7", help='unique resource identifier')
parser.add_argument('--file_name', type=str, default="cf2.bin", help='')
# Parse the arguments
args = parser.parse_args()
print("URI: ", args.uri)
flash_node = Flash(args.uri, args.file_name)
flash_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
================================================
FILE: crazyflie/scripts/gui.py
================================================
#!/usr/bin/env python3
import threading
import time
from pathlib import Path
from functools import partial
import rclpy
from std_srvs.srv import Empty
from geometry_msgs.msg import Twist
from rcl_interfaces.msg import Log
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
# from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from crazyflie_interfaces.msg import Status
import rowan
from nicegui import Client, app, events, ui, ui_run, Tailwind
class NiceGuiNode(Node):
def __init__(self) -> None:
super().__init__('nicegui')
# wait until the crazyflie_server is up and running
self.emergencyService = self.create_client(Empty, 'all/emergency')
self.emergencyService.wait_for_service()
# find all crazyflies
self.cfnames = []
for srv_name, srv_types in self.get_service_names_and_types():
if 'crazyflie_interfaces/srv/StartTrajectory' in srv_types:
# remove '/' and '/start_trajectory'
cfname = srv_name[1:-17]
if cfname != 'all':
self.cfnames.append(cfname)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 1)
self.sub_log = self.create_subscription(Log, 'rosout', self.on_rosout, rclpy.qos.QoSProfile(
depth=1000,
durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL))
self.logs = dict()
self.supervisor_labels = dict()
self.battery_labels = dict()
self.radio_labels = dict()
self.robotmodels = dict()
self.normal_style = Tailwind().text_color('black').font_weight('normal')
self.red_style = Tailwind().text_color('red-600').font_weight('bold')
with Client.auto_index_client:
with ui.row().classes('items-stretch'):
with ui.card().classes('w-full h-full'):
ui.label('Visualization').classes('text-2xl')
with ui.scene(800, 400, on_click=self.on_vis_click) as scene:
for name in self.cfnames:
robot = scene.stl('/urdf/cf2_assembly.stl').scale(1.0).material('#ff0000').with_name(name)
self.robotmodels[name] = robot
# augment with some additional fields
robot.status_ok = False
robot.battery_ok = False
robot.status_watchdog = time.time()
robot.supervisor_text = ""
robot.battery_text = ""
robot.radio_text = ""
scene.camera.x = 0
scene.camera.y = -1
scene.camera.z = 2
scene.camera.look_at_x = 0
scene.camera.look_at_y = 0
scene.camera.look_at_z = 0
with ui.row().classes('w-full h-lvh'):
with ui.tabs().classes('w-full') as tabs:
self.tabs = []
for name in ["all"] + self.cfnames:
self.tabs.append(ui.tab(name))
with ui.tab_panels(tabs, value=self.tabs[0], on_change=self.on_tab_change).classes('w-full') as self.tabpanels:
for name, tab in zip(["all"] + self.cfnames, self.tabs):
with ui.tab_panel(tab):
self.logs[name] = ui.log().classes('w-full h-96 no-wrap')
self.supervisor_labels[name] = ui.label("")
self.battery_labels[name] = ui.label("")
self.radio_labels[name] = ui.label("")
for name in self.cfnames:
self.create_subscription(Status, name + '/status', partial(self.on_status, name=name), 1)
# Call on_timer function
update_rate = 30 # Hz
self.timer = self.create_timer(
1.0/update_rate,
self.on_timer,
clock=rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.SYSTEM_TIME))
def on_rosout(self, msg: Log) -> None:
# filter by crazyflie and add to the correct log
if msg.name == "crazyflie_server":
if msg.msg.startswith("["):
idx = msg.msg.find("]")
name = msg.msg[1:idx]
# if it was an "all" category, add only to CFs
if name == 'all':
for logname ,log in self.logs.items():
if logname != "all":
log.push(msg.msg)
elif name in self.logs:
self.logs[name].push(msg.msg[idx+2:])
# add all possible messages to the 'all' tab
self.logs['all'].push(msg.msg)
def on_timer(self) -> None:
for name, robotmodel in self.robotmodels.items():
ros_time = rclpy.time.Time() # get the latest
robot_status_ok = robotmodel.status_ok and robotmodel.battery_ok
robot_status_text = ""
if self.tf_buffer.can_transform("world", name, ros_time):
t = self.tf_buffer.lookup_transform(
"world",
name,
ros_time)
transform_time = rclpy.time.Time.from_msg(t.header.stamp)
transform_age = self.get_clock().now() - transform_time
# latest transform is older than a second indicates a problem
if transform_age.nanoseconds * 1e-9 > 1:
robot_status_ok = False
robot_status_text += "old transform; "
else:
pos = t.transform.translation
robotmodel.move(pos.x, pos.y, pos.z)
robotmodel.rotate(*rowan.to_euler([
t.transform.rotation.w,
t.transform.rotation.x,
t.transform.rotation.y,
t.transform.rotation.z], "xyz"))
else:
# no available transform indicates a problem
robot_status_ok = False
robot_status_text += "unavailable transform; "
# no status update for a while, indicate a problem
if time.time() - robotmodel.status_watchdog > 2.0:
robot_status_ok = False
robot_status_text += "no recent status update; "
self.supervisor_labels[name].set_text(robot_status_text)
self.battery_labels[name].set_text("N.A.")
self.radio_labels[name].set_text("N.A.")
else:
self.supervisor_labels[name].set_text(robot_status_text + robotmodel.supervisor_text)
self.battery_labels[name].set_text(robotmodel.battery_text)
self.radio_labels[name].set_text(robotmodel.radio_text)
# any issues detected -> mark red, otherwise green
if robot_status_ok:
robotmodel.material('#00ff00')
else:
robotmodel.material('#ff0000')
if robotmodel.battery_ok:
self.normal_style.apply(self.battery_labels[name])
else:
self.red_style.apply(self.battery_labels[name])
def on_vis_click(self, e: events.SceneClickEventArguments):
hit = e.hits[0]
name = hit.object_name or hit.object_id
ui.notify(f'You clicked on the {name}')
if name == 'ground':
self.tabpanels.value = 'all'
else:
self.tabpanels.value = name
def on_status(self, msg, name) -> None:
status_ok = True
is_flying = False
supervisor_text = ""
if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_BE_ARMED:
supervisor_text += "can be armed; "
if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_ARMED:
supervisor_text += "is armed; "
if msg.supervisor_info & Status.SUPERVISOR_INFO_AUTO_ARM:
supervisor_text += "auto-arm; "
if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_FLY:
supervisor_text += "can fly; "
else:
status_ok = False
if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_FLYING:
supervisor_text += "is flying; "
is_flying = True
if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_TUMBLED:
supervisor_text += "is tumbled; "
status_ok = False
if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_LOCKED:
supervisor_text += "is locked; "
status_ok = False
self.robotmodels[name].supervisor_text = supervisor_text
battery_text = f'{msg.battery_voltage:.2f} V'
battery_ok = True
# TODO (WH): We could read the voltage limits from the firmware (parameter) or crazyflies.yaml
# In the firmware, anything below 3.2 is warning, anything below 3.0 is critical
if (is_flying and msg.battery_voltage < 3.2) or (not is_flying and msg.battery_voltage < 3.8):
battery_ok = False
if msg.pm_state == Status.PM_STATE_BATTERY:
battery_text += " (on battery)"
elif msg.pm_state == Status.PM_STATE_CHARGING:
battery_text += " (charging)"
elif msg.pm_state == Status.PM_STATE_CHARGED:
battery_text += " (charged)"
elif msg.pm_state == Status.PM_STATE_LOW_POWER:
battery_text += " (low power)"
battery_ok = False
elif msg.pm_state == Status.PM_STATE_SHUTDOWN:
battery_text += " (shutdown)"
battery_ok = False
self.robotmodels[name].battery_text = battery_text
radio_text = f'{msg.rssi} dBm; Unicast: {msg.num_rx_unicast} / {msg.num_tx_unicast}; Broadcast: {msg.num_rx_broadcast} / {msg.num_tx_broadcast}; Latency: {msg.latency_unicast} ms'
self.robotmodels[name].radio_text = radio_text
# save status here
self.robotmodels[name].status_ok = status_ok
self.robotmodels[name].battery_ok = battery_ok
# store the time when we last received any status
self.robotmodels[name].status_watchdog = time.time()
def on_tab_change(self, arg):
for name, robotmodel in self.robotmodels.items():
if name != arg.value:
robotmodel.scale(1)
if arg.value in self.robotmodels:
self.robotmodels[arg.value].scale(2)
def main() -> None:
# NOTE: This function is defined as the ROS entry point in setup.py, but it's empty to enable NiceGUI auto-reloading
pass
def ros_main() -> None:
rclpy.init()
node = NiceGuiNode()
try:
rclpy.spin(node)
except ExternalShutdownException:
pass
app.add_static_files("/urdf",
str((Path(__file__).parent.parent.parent / "share" / "crazyflie" / "urdf").resolve()),
follow_symlink=True)
app.on_startup(lambda: threading.Thread(target=ros_main).start())
ui_run.APP_IMPORT_STRING = f'{__name__}:app' # ROS2 uses a non-standard module name, so we need to specify it here
ui.run(uvicorn_reload_dirs=str(Path(__file__).parent.resolve()), favicon='🤖')
================================================
FILE: crazyflie/scripts/simple_mapper_multiranger.py
================================================
#!/usr/bin/env python3
""" This simple mapper is loosely based on both the bitcraze cflib point cloud example
https://github.com/bitcraze/crazyflie-lib-python/blob/master/examples/multiranger/multiranger_pointcloud.py
and the webots epuck simple mapper example:
https://github.com/cyberbotics/webots_ros2
Originally from https://github.com/knmcguire/crazyflie_ros2_experimental/
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import TransformStamped
from tf2_ros import StaticTransformBroadcaster
import tf_transformations
import math
import numpy as np
from bresenham import bresenham
GLOBAL_SIZE_X = 20.0
GLOBAL_SIZE_Y = 20.0
MAP_RES = 0.1
class SimpleMapperMultiranger(Node):
def __init__(self):
super().__init__('simple_mapper_multiranger')
self.declare_parameter('robot_prefix', '/cf231')
robot_prefix = self.get_parameter('robot_prefix').value
self.odom_subscriber = self.create_subscription(
Odometry, robot_prefix + '/odom', self.odom_subscribe_callback, 10)
self.ranges_subscriber = self.create_subscription(
LaserScan, robot_prefix + '/scan', self.scan_subscribe_callback, 10)
self.position = [0.0, 0.0, 0.0]
self.angles = [0.0, 0.0, 0.0]
self.ranges = [0.0, 0.0, 0.0, 0.0]
self.range_max = 3.5
self.tfbr = StaticTransformBroadcaster(self)
t_map = TransformStamped()
t_map.header.stamp = self.get_clock().now().to_msg()
t_map.header.frame_id = 'map'
t_map.child_frame_id = robot_prefix + '/odom'
t_map.transform.translation.x = 0.0
t_map.transform.translation.y = 0.0
t_map.transform.translation.z = 0.0
self.tfbr.sendTransform(t_map)
self.position_update = False
self.map = [-1] * int(GLOBAL_SIZE_X / MAP_RES) * \
int(GLOBAL_SIZE_Y / MAP_RES)
self.map_publisher = self.create_publisher(OccupancyGrid, robot_prefix + '/map',
qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST,))
self.get_logger().info(f"Simple mapper set for crazyflie " + robot_prefix +
f" using the odom and scan topic")
def odom_subscribe_callback(self, msg):
self.position[0] = msg.pose.pose.position.x
self.position[1] = msg.pose.pose.position.y
self.position[2] = msg.pose.pose.position.z
q = msg.pose.pose.orientation
euler = tf_transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])
self.angles[0] = euler[0]
self.angles[1] = euler[1]
self.angles[2] = euler[2]
self.position_update = True
def scan_subscribe_callback(self, msg):
self.ranges = msg.ranges
self.range_max = msg.range_max
data = self.rotate_and_create_points()
points_x = []
points_y = []
#
if self.position_update is False:
return
for i in range(len(data)):
point_x = int((data[i][0] - GLOBAL_SIZE_X / 2.0) / MAP_RES)
point_y = int((data[i][1] - GLOBAL_SIZE_Y / 2.0) / MAP_RES)
points_x.append(point_x)
points_y.append(point_y)
position_x_map = int(
(self.position[0] - GLOBAL_SIZE_X / 2.0) / MAP_RES)
position_y_map = int(
(self.position[1] - GLOBAL_SIZE_Y / 2.0) / MAP_RES)
for line_x, line_y in bresenham(position_x_map, position_y_map, point_x, point_y):
self.map[line_y * int(GLOBAL_SIZE_X / MAP_RES) + line_x] = 0
self.map[point_y * int(GLOBAL_SIZE_X / MAP_RES) + point_x] = 100
msg = OccupancyGrid()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'map'
msg.info.resolution = MAP_RES
msg.info.width = int(GLOBAL_SIZE_X / MAP_RES)
msg.info.height = int(GLOBAL_SIZE_Y / MAP_RES)
msg.info.origin.position.x = - GLOBAL_SIZE_X / 2.0
msg.info.origin.position.y = - GLOBAL_SIZE_Y / 2.0
msg.data = self.map
self.map_publisher.publish(msg)
def rotate_and_create_points(self):
data = []
o = self.position
roll = self.angles[0]
pitch = self.angles[1]
yaw = self.angles[2]
r_back = self.ranges[0]
r_right = self.ranges[1]
r_front = self.ranges[2]
r_left = self.ranges[3]
if (r_left < self.range_max and r_left != 0.0):
left = [o[0], o[1] + r_left, o[2]]
data.append(self.rot(roll, pitch, yaw, o, left))
if (r_right < self.range_max and r_right != 0.0):
right = [o[0], o[1] - r_right, o[2]]
data.append(self.rot(roll, pitch, yaw, o, right))
if (r_front < self.range_max and r_front != 0.0):
front = [o[0] + r_front, o[1], o[2]]
data.append(self.rot(roll, pitch, yaw, o, front))
if (r_back < self.range_max and r_back != 0.0):
back = [o[0] - r_back, o[1], o[2]]
data.append(self.rot(roll, pitch, yaw, o, back))
return data
def rot(self, roll, pitch, yaw, origin, point):
cosr = math.cos((roll))
cosp = math.cos((pitch))
cosy = math.cos((yaw))
sinr = math.sin((roll))
sinp = math.sin((pitch))
siny = math.sin((yaw))
roty = np.array([[cosy, -siny, 0],
[siny, cosy, 0],
[0, 0, 1]])
rotp = np.array([[cosp, 0, sinp],
[0, 1, 0],
[-sinp, 0, cosp]])
rotr = np.array([[1, 0, 0],
[0, cosr, -sinr],
[0, sinr, cosr]])
rotFirst = np.dot(rotr, rotp)
rot = np.array(np.dot(rotFirst, roty))
tmp = np.subtract(point, origin)
tmp2 = np.dot(rot, tmp)
return np.add(tmp2, origin)
def main(args=None):
rclpy.init(args=args)
simple_mapper_multiranger = SimpleMapperMultiranger()
rclpy.spin(simple_mapper_multiranger)
rclpy.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
================================================
FILE: crazyflie/scripts/vel_mux.py
================================================
#!/usr/bin/env python3
"""
A Twist message handler that get incoming twist messages from
external packages and handles proper takeoff, landing and
hover commands of connected crazyflie in the crazyflie_server
node
2022 - K. N. McGuire (Bitcraze AB)
"""
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from crazyflie_interfaces.srv import Takeoff, Land, NotifySetpointsStop
from crazyflie_interfaces.msg import Hover
import time
class VelMux(Node):
def __init__(self):
super().__init__('vel_mux')
self.declare_parameter('hover_height', 0.5)
self.declare_parameter('robot_prefix', '/cf')
self.declare_parameter('incoming_twist_topic', '/cmd_vel')
self.hover_height = self.get_parameter('hover_height').value
robot_prefix = self.get_parameter('robot_prefix').value
incoming_twist_topic = self.get_parameter('incoming_twist_topic').value
self.subscription = self.create_subscription(
Twist,
incoming_twist_topic,
self.cmd_vel_callback,
10)
self.msg_cmd_vel = Twist()
self.received_first_cmd_vel = False
timer_period = 0.1
self.timer = self.create_timer(timer_period, self.timer_callback)
self.takeoff_client = self.create_client(Takeoff, robot_prefix + '/takeoff')
self.publisher_hover = self.create_publisher(Hover, robot_prefix + '/cmd_hover', 10)
self.land_client = self.create_client(Land, robot_prefix + '/land')
self.notify_client = self.create_client(NotifySetpointsStop, robot_prefix + '/notify_setpoints_stop')
self.cf_has_taken_off = False
self.takeoff_client.wait_for_service()
self.land_client.wait_for_service()
self.get_logger().info(f"Velocity Multiplexer set for {robot_prefix}"+
f" with height {self.hover_height} m using the {incoming_twist_topic} topic")
def cmd_vel_callback(self, msg):
self.msg_cmd_vel = msg
# This is to handle the zero twist messages from teleop twist keyboard closing
# or else the crazyflie would constantly take off again.
msg_is_zero = msg.linear.x == 0.0 and msg.linear.y == 0.0 and msg.angular.z == 0.0 and msg.linear.z == 0.0
if msg_is_zero is False and self.received_first_cmd_vel is False and msg.linear.z >= 0.0:
self.received_first_cmd_vel = True
def timer_callback(self):
if self.received_first_cmd_vel and self.cf_has_taken_off is False:
req = Takeoff.Request()
req.height = self.hover_height
req.duration = rclpy.duration.Duration(seconds=2.0).to_msg()
self.takeoff_client.call_async(req)
self.cf_has_taken_off = True
time.sleep(2.0)
if self.received_first_cmd_vel and self.cf_has_taken_off:
if self.msg_cmd_vel.linear.z >= 0:
msg = Hover()
msg.vx = self.msg_cmd_vel.linear.x
msg.vy = self.msg_cmd_vel.linear.y
msg.yaw_rate = self.msg_cmd_vel.angular.z
msg.z_distance = self.hover_height
self.publisher_hover.publish(msg)
else:
req = NotifySetpointsStop.Request()
self.notify_client.call_async(req)
req = Land.Request()
req.height = 0.1
req.duration = rclpy.duration.Duration(seconds=2.0).to_msg()
self.land_client.call_async(req)
time.sleep(2.0)
self.cf_has_taken_off = False
self.received_first_cmd_vel = False
def main(args=None):
rclpy.init(args=args)
vel_mux = VelMux()
rclpy.spin(vel_mux)
vel_mux.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
================================================
FILE: crazyflie/src/crazyflie_server.cpp
================================================
#include <memory>
#include <vector>
#include <regex>
#include <crazyflie_cpp/Crazyflie.h>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include "std_srvs/srv/empty.hpp"
#include "crazyflie_interfaces/srv/start_trajectory.hpp"
#include "crazyflie_interfaces/srv/takeoff.hpp"
#include "crazyflie_interfaces/srv/land.hpp"
#include "crazyflie_interfaces/srv/go_to.hpp"
#include "crazyflie_interfaces/srv/notify_setpoints_stop.hpp"
#include "crazyflie_interfaces/srv/arm.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "crazyflie_interfaces/srv/upload_trajectory.hpp"
#include "motion_capture_tracking_interfaces/msg/named_pose_array.hpp"
#include "crazyflie_interfaces/msg/full_state.hpp"
#include "crazyflie_interfaces/msg/position.hpp"
#include "crazyflie_interfaces/msg/velocity_world.hpp"
#include "crazyflie_interfaces/msg/hover.hpp"
#include "crazyflie_interfaces/msg/status.hpp"
#include "crazyflie_interfaces/msg/log_data_generic.hpp"
#include "crazyflie_interfaces/msg/connection_statistics_array.hpp"
using std::placeholders::_1;
using std::placeholders::_2;
using crazyflie_interfaces::srv::StartTrajectory;
using crazyflie_interfaces::srv::Takeoff;
using crazyflie_interfaces::srv::Land;
using crazyflie_interfaces::srv::GoTo;
using crazyflie_interfaces::srv::UploadTrajectory;
using crazyflie_interfaces::srv::NotifySetpointsStop;
using crazyflie_interfaces::srv::Arm;
using std_srvs::srv::Empty;
using motion_capture_tracking_interfaces::msg::NamedPoseArray;
using crazyflie_interfaces::msg::FullState;
#ifdef ROS_DISTRO_HUMBLE
inline auto get_service_qos() { return rmw_qos_profile_services_default; }
#else
inline auto get_service_qos() { return rclcpp::ServicesQoS(); }
#endif
// Note on logging: we use a single logger with string prefixes
// A better way would be to use named child loggers, but these do not
// report to /rosout in humble, see https://github.com/ros2/rclpy/issues/1131
// Once we do not support humble anymore, consider switching to child loggers
// Helper class to convert crazyflie_cpp logging messages to ROS logging messages
class CrazyflieLogger : public Logger
{
public:
CrazyflieLogger(rclcpp::Logger logger, const std::string& prefix)
: Logger()
, logger_(logger)
, prefix_(prefix)
{
}
virtual ~CrazyflieLogger() {}
virtual void info(const std::string &msg)
{
RCLCPP_INFO(logger_, "%s %s", prefix_.c_str(), msg.c_str());
}
virtual void warning(const std::string &msg)
{
RCLCPP_WARN(logger_, "%s %s", prefix_.c_str(), msg.c_str());
}
virtual void error(const std::string &msg)
{
RCLCPP_ERROR(logger_, "%s %s", prefix_.c_str(), msg.c_str());
}
private:
rclcpp::Logger logger_;
std::string prefix_;
};
std::set<std::string> extract_names(
const std::map<std::string, rclcpp::ParameterValue> ¶meter_overrides,
const std::string &pattern)
{
std::set<std::string> result;
for (const auto &i : parameter_overrides)
{
if (i.first.find(pattern) == 0)
{
size_t start = pattern.size() + 1;
size_t end = i.first.find(".", start);
result.insert(i.first.substr(start, end - start));
}
}
return result;
}
// ROS wrapper for a single Crazyflie object
class CrazyflieROS
{
private:
struct logPose {
float x;
float y;
float z;
int32_t quatCompressed;
} __attribute__((packed));
struct logScan {
uint16_t front;
uint16_t left;
uint16_t back;
uint16_t right;
} __attribute__((packed));
struct logOdom {
int16_t x;
int16_t y;
int16_t z;
int32_t quatCompressed;
int16_t vx;
int16_t vy;
int16_t vz;
//int16_t rateRoll;
//int16_t ratePitch;
//int16_t rateYaw;
} __attribute__((packed));
struct logStatus {
// general status
uint16_t supervisorInfo; // supervisor.info
// battery related
// Note that using BQ-deck/Bolt one can actually have two batteries at the same time.
// vbat refers to the battery directly connected to the CF board and might not reflect
// the "external" battery on BQ/Bolt builds
uint16_t vbatMV; // pm.vbatMV
uint8_t pmState; // pm.state
// radio related
uint8_t rssi; // radio.rssi
uint16_t numRxBc; // radio.numRxBc
uint16_t numRxUc; // radio.numRxUc
} __attribute__((packed));
public:
CrazyflieROS(
const std::string& link_uri,
const std::string& cf_type,
const std::string& name,
rclcpp::Node* node,
rclcpp::CallbackGroup::SharedPtr callback_group_cf_cmd,
rclcpp::CallbackGroup::SharedPtr callback_group_cf_srv,
const CrazyflieBroadcaster* cfbc,
bool enable_parameters = true)
: logger_(node->get_logger())
, cf_logger_(logger_, "[" + name + "]")
, cf_(
link_uri,
cf_logger_,
std::bind(&CrazyflieROS::on_console, this, std::placeholders::_1))
, name_(name)
, node_(node)
, tf_broadcaster_(node)
, last_on_latency_(std::chrono::steady_clock::now())
, cfbc_(cfbc)
{
auto sub_opt_cf_cmd = rclcpp::SubscriptionOptions();
sub_opt_cf_cmd.callback_group = callback_group_cf_cmd;
// Services
auto service_qos = rmw_qos_profile_services_default;
service_emergency_ = node->create_service<Empty>(name + "/emergency", std::bind(&CrazyflieROS::emergency, this, _1, _2), get_service_qos(), callback_group_cf_srv);
service_start_trajectory_ = node->create_service<StartTrajectory>(name + "/start_trajectory", std::bind(&CrazyflieROS::start_trajectory, this, _1, _2), get_service_qos(), callback_group_cf_srv);
service_takeoff_ = node->create_service<Takeoff>(name + "/takeoff", std::bind(&CrazyflieROS::takeoff, this, _1, _2), get_service_qos(), callback_group_cf_srv);
service_land_ = node->create_service<Land>(name + "/land", std::bind(&CrazyflieROS::land, this, _1, _2), get_service_qos(), callback_group_cf_srv);
service_go_to_ = node->create_service<GoTo>(name + "/go_to", std::bind(&CrazyflieROS::go_to, this, _1, _2), get_service_qos(), callback_group_cf_srv);
service_upload_trajectory_ = node->create_service<UploadTrajectory>(name + "/upload_trajectory", std::bind(&CrazyflieROS::upload_trajectory, this, _1, _2), get_service_qos(), callback_group_cf_srv);
service_notify_setpoints_stop_ = node->create_service<NotifySetpointsStop>(name + "/notify_setpoints_stop", std::bind(&CrazyflieROS::notify_setpoints_stop, this, _1, _2), get_service_qos(), callback_group_cf_srv);
service_arm_ = node->create_service<Arm>(name + "/arm", std::bind(&CrazyflieROS::arm, this, _1, _2), get_service_qos(), callback_group_cf_srv);
// Topics
subscription_cmd_vel_legacy_ = node->create_subscription<geometry_msgs::msg::Twist>(name + "/cmd_vel_legacy", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieROS::cmd_vel_legacy_changed, this, _1), sub_opt_cf_cmd);
subscription_cmd_full_state_ = node->create_subscription<crazyflie_interfaces::msg::FullState>(name + "/cmd_full_state", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieROS::cmd_full_state_changed, this, _1), sub_opt_cf_cmd);
subscription_cmd_position_ = node->create_subscription<crazyflie_interfaces::msg::Position>(name + "/cmd_position", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieROS::cmd_position_changed, this, _1), sub_opt_cf_cmd);
subscription_cmd_velocity_world_ = node->create_subscription<crazyflie_interfaces::msg::VelocityWorld>(name + "/cmd_velocity_world", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieROS::cmd_velocity_world_changed, this, _1), sub_opt_cf_cmd);
subscription_cmd_hover_ = node->create_subscription<crazyflie_interfaces::msg::Hover>(name + "/cmd_hover", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieROS::cmd_hover_changed, this, _1), sub_opt_cf_cmd);
publisher_robot_description_ = node->create_publisher<std_msgs::msg::String>(name + "/robot_description",
rclcpp::QoS(1).transient_local());
{
auto msg = std::make_unique<std_msgs::msg::String>();
auto robot_desc = node->get_parameter("robot_description").get_parameter_value().get<std::string>();
msg->data = std::regex_replace(robot_desc, std::regex("\\$NAME"), name);
publisher_robot_description_->publish(std::move(msg));
}
// spinning timer
// used to process all incoming radio messages
spin_timer_ =
node->create_wall_timer(
std::chrono::milliseconds(1),
std::bind(&CrazyflieROS::spin_once, this), callback_group_cf_srv);
// link statistics
warning_freq_ = node->get_parameter("warnings.frequency").get_parameter_value().get<float>();
max_latency_ = node->get_parameter("warnings.communication.max_unicast_latency").get_parameter_value().get<float>();
min_ack_rate_ = node->get_parameter("warnings.communication.min_unicast_ack_rate").get_parameter_value().get<float>();
min_unicast_receive_rate_ = node->get_parameter("warnings.communication.min_unicast_receive_rate").get_parameter_value().get<float>();
min_broadcast_receive_rate_ = node->get_parameter("warnings.communication.min_broadcast_receive_rate").get_parameter_value().get<float>();
publish_stats_ = node->get_parameter("warnings.communication.publish_stats").get_parameter_value().get<bool>();
if (publish_stats_) {
publisher_connection_stats_ = node->create_publisher<crazyflie_interfaces::msg::ConnectionStatisticsArray>(name + "/connection_statistics", 10);
}
if (warning_freq_ >= 0.0) {
cf_.setLatencyCallback(std::bind(&CrazyflieROS::on_latency, this, std::placeholders::_1));
link_statistics_timer_ =
node->create_wall_timer(
std::chrono::milliseconds((int)(1000.0/warning_freq_)),
std::bind(&CrazyflieROS::on_link_statistics_timer, this), callback_group_cf_srv);
}
auto start = std::chrono::system_clock::now();
cf_.logReset();
auto node_parameters_iface = node->get_node_parameters_interface();
const std::map<std::string, rclcpp::ParameterValue> ¶meter_overrides =
node_parameters_iface->get_parameter_overrides();
// declares lambda, to be used as local function, which re-declares specified parameters for other nodes to query
auto declare_param = [¶meter_overrides, node](const std::string& param)
{
// rclcpp::ParameterValue value(parameter_overridesparam]);
node->declare_parameter(param, parameter_overrides.at(param));
};
declare_param("robots." + name + ".uri");
declare_param("robots." + name + ".initial_position");
// declares a lambda, to be used as local function
auto update_map = [¶meter_overrides](std::map<std::string, rclcpp::ParameterValue>& map, const std::string& pattern)
{
for (const auto &i : parameter_overrides) {
if (i.first.find(pattern) == 0) {
size_t start = pattern.size() + 1;
const auto group_and_name = i.first.substr(start);
map[group_and_name] = i.second;
}
}
};
auto update_value = [¶meter_overrides](rclcpp::ParameterValue& value, const std::string& pattern)
{
for (const auto &i : parameter_overrides) {
if (i.first.find(pattern) == 0) {
value = i.second;
}
}
};
if (enable_parameters) {
bool query_all_values_on_connect = node->get_parameter("firmware_params.query_all_values_on_connect").get_parameter_value().get<bool>();
int numParams = 0;
RCLCPP_INFO(logger_, "[%s] Requesting parameters...", name_.c_str());
cf_.requestParamToc(/*forceNoCache*/false, /*requestValues*/query_all_values_on_connect);
for (auto iter = cf_.paramsBegin(); iter != cf_.paramsEnd(); ++iter) {
auto entry = *iter;
std::string paramName = name + ".params." + entry.group + "." + entry.name;
switch (entry.type)
{
case Crazyflie::ParamTypeUint8:
if (query_all_values_on_connect) {
node->declare_parameter(paramName, cf_.getParam<uint8_t>(entry.id));
} else {
node->declare_parameter(paramName, rclcpp::PARAMETER_INTEGER);
}
break;
case Crazyflie::ParamTypeInt8:
if (query_all_values_on_connect) {
node->declare_parameter(paramName, cf_.getParam<int8_t>(entry.id));
} else {
node->declare_parameter(paramName, rclcpp::PARAMETER_INTEGER);
}
break;
case Crazyflie::ParamTypeUint16:
if (query_all_values_on_connect) {
node->declare_parameter(paramName, cf_.getParam<uint16_t>(entry.id));
} else {
node->declare_parameter(paramName, rclcpp::PARAMETER_INTEGER);
}
break;
case Crazyflie::ParamTypeInt16:
if (query_all_values_on_connect) {
node->declare_parameter(paramName, cf_.getParam<int16_t>(entry.id));
} else {
node->declare_parameter(paramName, rclcpp::PARAMETER_INTEGER);
}
break;
case Crazyflie::ParamTypeUint32:
if (query_all_values_on_connect) {
node->declare_parameter<int64_t>(paramName, cf_.getParam<uint32_t>(entry.id));
} else {
node->declare_parameter(paramName, rclcpp::PARAMETER_INTEGER);
}
break;
case Crazyflie::ParamTypeInt32:
if (query_all_values_on_connect) {
node->declare_parameter(paramName, cf_.getParam<int32_t>(entry.id));
} else {
node->declare_parameter(paramName, rclcpp::PARAMETER_INTEGER);
}
break;
case Crazyflie::ParamTypeFloat:
if (query_all_values_on_connect) {
node->declare_parameter(paramName, cf_.getParam<float>(entry.id));
} else {
node->declare_parameter(paramName, rclcpp::PARAMETER_DOUBLE);
}
break;
default:
RCLCPP_WARN(logger_, "[%s] Unknown param type for %s/%s", name_.c_str(), entry.group.c_str(), entry.name.c_str());
break;
}
// If there is no such parameter in all, add it
std::string allParamName = "all.params." + entry.group + "." + entry.name;
if (!node->has_parameter(allParamName)) {
if (entry.type == Crazyflie::ParamTypeFloat) {
node->declare_parameter(allParamName, rclcpp::PARAMETER_DOUBLE);
} else {
node->declare_parameter(allParamName, rclcpp::PARAMETER_INTEGER);
}
}
++numParams;
}
auto end1 = std::chrono::system_clock::now();
std::chrono::duration<double> elapsedSeconds1 = end1 - start;
RCLCPP_INFO(logger_, "[%s] reqParamTOC: %f s (%d params)", name_.c_str(), elapsedSeconds1.count(), numParams);
// Set parameters as specified in the configuration files, as in the following order
// 1.) check all/firmware_params
// 2.) check robot_types/<type_name>/firmware_params
// 3.) check robots/<robot_name>/firmware_params
// where the higher order is used if defined on multiple levels.
// <group>.<name> -> value map
std::map<std::string, rclcpp::ParameterValue> set_param_map;
// check global settings/firmware_params
update_map(set_param_map, "all.firmware_params");
// check robot_types/<type_name>/firmware_params
update_map(set_param_map, "robot_types." + cf_type + ".firmware_params");
// check robots/<robot_name>/firmware_params
update_map(set_param_map, "robots." + name_ + ".firmware_params");
// Update parameters
for (const auto&i : set_param_map) {
std::string paramName = name + ".params." + std::regex_replace(i.first, std::regex("\\."), ".");
change_parameter(rclcpp::Parameter(paramName, i.second));
}
}
// Reference Frame
{
rclcpp::ParameterValue reference_frame_value("world");
// Get logging configuration as specified in the configuration files, as in the following order
// 1.) check all/reference_frame
// 2.) check robot_types/<type_name>/reference_frame
// 3.) check robots/<robot_name>/reference_frame
// where the higher order is used if defined on multiple levels.
update_value(reference_frame_value, "all.reference_frame");
// check robot_types/<type_name>/reference_frame
update_value(reference_frame_value, "robot_types." + cf_type + ".reference_frame");
// check robots/<robot_name>/reference_frame
update_value(reference_frame_value, "robots." + name_ + ".reference_frame");
// extract reference frame
reference_frame_ = reference_frame_value.get<std::string>();
RCLCPP_INFO(logger_, "[%s] ref frame: %s", name_.c_str(), reference_frame_.c_str());
}
// Logging
{
// <group>.<name> -> value map
std::map<std::string, rclcpp::ParameterValue> log_config_map;
// Get logging configuration as specified in the configuration files, as in the following order
// 1.) check all/firmware_logging
// 2.) check robot_types/<type_name>/firmware_logging
// 3.) check robots/<robot_name>/firmware_logging
// where the higher order is used if defined on multiple levels.
update_map(log_config_map, "all.firmware_logging");
// check robot_types/<type_name>/firmware_logging
update_map(log_config_map, "robot_types." + cf_type + ".firmware_logging");
// check robots/<robot_name>/firmware_logging
update_map(log_config_map, "robots." + name_ + ".firmware_logging");
// check if logging is enabled for this drone
bool logging_enabled = log_config_map["enabled"].get<bool>();
if (logging_enabled) {
cf_.requestLogToc(/*forceNoCache*/);
for (const auto&i : log_config_map) {
// check if any of the default topics are enabled
if (i.first.find("default_topics.pose") == 0) {
int freq = log_config_map["default_topics.pose.frequency"].get<int>();
RCLCPP_INFO(logger_, "[%s] Logging to /pose at %d Hz", name_.c_str(), freq);
publisher_pose_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(name + "/pose", 10);
std::function<void(uint32_t, const logPose*)> cb = std::bind(&CrazyflieROS::on_logging_pose, this, std::placeholders::_1, std::placeholders::_2);
log_block_pose_.reset(new LogBlock<logPose>(
&cf_,{
{"stateEstimate", "x"},
{"stateEstimate", "y"},
{"stateEstimate", "z"},
{"stateEstimateZ", "quat"}
}, cb));
log_block_pose_->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds
}
else if (i.first.find("default_topics.scan") == 0) {
int freq = log_config_map["default_topics.scan.frequency"].get<int>();
RCLCPP_INFO(logger_, "[%s] Logging to /scan at %d Hz", name_.c_str(), freq);
publisher_scan_ = node->create_publisher<sensor_msgs::msg::LaserScan>(name + "/scan", 10);
std::function<void(uint32_t, const logScan*)> cb = std::bind(&CrazyflieROS::on_logging_scan, this, std::placeholders::_1, std::placeholders::_2);
log_block_scan_.reset(new LogBlock<logScan>(
&cf_,{
{"range", "front"},
{"range", "left"},
{"range", "back"},
{"range", "right"}
}, cb));
log_block_scan_->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds
}
else if (i.first.find("default_topics.odom") == 0) {
int freq = log_config_map["default_topics.odom.frequency"].get<int>();
RCLCPP_INFO(logger_, "[%s] Logging to /odom at %d Hz", name_.c_str(), freq);
publisher_odom_ = node->create_publisher<nav_msgs::msg::Odometry>(name + "/odom", 10);
std::function<void(uint32_t, const logOdom*)> cb = std::bind(&CrazyflieROS::on_logging_odom, this, std::placeholders::_1, std::placeholders::_2);
log_block_odom_.reset(new LogBlock<logOdom>(
&cf_,{
{"stateEstimateZ", "x"},
{"stateEstimateZ", "y"},
{"stateEstimateZ", "z"},
{"stateEstimateZ", "quat"},
{"stateEstimateZ", "vx"},
{"stateEstimateZ", "vy"},
{"stateEstimateZ", "vz"},
//{"stateEstimateZ", "rateRoll"},
//{"stateEstimateZ", "ratePitch"},
//{"stateEstimateZ", "rateYaw"}
}, cb));
log_block_odom_->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds
}
else if (i.first.find("default_topics.status") == 0) {
int freq = log_config_map["default_topics.status.frequency"].get<int>();
RCLCPP_INFO(logger_, "[%s] Logging to /status at %d Hz", name_.c_str(), freq);
publisher_status_ = node->create_publisher<crazyflie_interfaces::msg::Status>(name + "/status", 10);
std::function<void(uint32_t, const logStatus*)> cb = std::bind(&CrazyflieROS::on_logging_status, this, std::placeholders::_1, std::placeholders::_2);
std::list<std::pair<std::string, std::string> > logvars({
// general status
{"supervisor", "info"},
// battery related
{"pm", "vbatMV"},
{"pm", "state"},
// radio related
{"radio", "rssi"}
});
// check if this firmware version has radio.numRx{Bc,Uc}
status_has_radio_stats_ = false;
for (auto iter = cf_.logVariablesBegin(); iter != cf_.logVariablesEnd(); ++iter) {
auto entry = *iter;
if (entry.group == "radio" && entry.name == "numRxBc") {
logvars.push_back({"radio", "numRxBc"});
logvars.push_back({"radio", "numRxUc"});
status_has_radio_stats_ = true;
break;
}
}
// older firmware -> use other 16-bit variables
if (!status_has_radio_stats_) {
RCLCPP_WARN(logger_, "[%s] Older firmware. status/num_rx_broadcast and status/num_rx_unicast are set to zero.", name_.c_str());
logvars.push_back({"pm", "vbatMV"});
logvars.push_back({"pm", "vbatMV"});
}
log_block_status_.reset(new LogBlock<logStatus>(
&cf_,logvars, cb));
log_block_status_->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds
}
else if (i.first.find("custom_topics") == 0
&& i.first.rfind(".vars") != std::string::npos) {
std::string topic_name = i.first.substr(14, i.first.size() - 14 - 5);
int freq = log_config_map["custom_topics." + topic_name + ".frequency"].get<int>();
auto vars = log_config_map["custom_topics." + topic_name + ".vars"].get<std::vector<std::string>>();
RCLCPP_INFO(logger_, "[%s] Logging to %s at %d Hz", name_.c_str(), topic_name.c_str(), freq);
publishers_generic_.emplace_back(node->create_publisher<crazyflie_interfaces::msg::LogDataGeneric>(name + "/" + topic_name, 10));
std::function<void(uint32_t, const std::vector<float>*, void* userData)> cb = std::bind(
&CrazyflieROS::on_logging_custom,
this,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3);
log_blocks_generic_.emplace_back(new LogBlockGeneric(
&cf_,
vars,
(void*)&publishers_generic_.back(),
cb));
log_blocks_generic_.back()->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds
}
}
}
}
RCLCPP_INFO(logger_, "[%s] Requesting memories...", name_.c_str());
cf_.requestMemoryToc();
}
void spin_once()
{
// process all packets from the receive queue
cf_.processAllPackets();
}
std::string broadcastUri() const
{
return cf_.broadcastUri();
}
uint8_t id() const
{
return cf_.address() & 0xFF;
}
const Crazyflie::ParamTocEntry* paramTocEntry(const std::string& group, const std::string& name)
{
return cf_.getParamTocEntry(group, name);
}
const std::string& name() const
{
return name_;
}
void change_parameter(const rclcpp::Parameter& p)
{
std::string prefix = name_ + ".params.";
if (p.get_name().find(prefix) != 0) {
RCLCPP_ERROR(
logger_,
"[%s] Incorrect parameter update request for param \"%s\"", name_.c_str(), p.get_name().c_str());
return;
}
size_t pos = p.get_name().find(".", prefix.size());
std::string group(p.get_name().begin() + prefix.size(), p.get_name().begin() + pos);
std::string name(p.get_name().begin() + pos + 1, p.get_name().end());
RCLCPP_INFO(
logger_,
"[%s] Update parameter \"%s.%s\" to %s",
name_.c_str(),
group.c_str(),
name.c_str(),
p.value_to_string().c_str());
auto entry = cf_.getParamTocEntry(group, name);
if (entry) {
switch (entry->type)
{
case Crazyflie::ParamTypeUint8:
cf_.setParam<uint8_t>(entry->id, p.as_int());
break;
case Crazyflie::ParamTypeInt8:
cf_.setParam<int8_t>(entry->id, p.as_int());
break;
case Crazyflie::ParamTypeUint16:
cf_.setParam<uint16_t>(entry->id, p.as_int());
break;
case Crazyflie::ParamTypeInt16:
cf_.setParam<int16_t>(entry->id, p.as_int());
break;
case Crazyflie::ParamTypeUint32:
cf_.setParam<uint32_t>(entry->id, p.as_int());
break;
case Crazyflie::ParamTypeInt32:
cf_.setParam<int32_t>(entry->id, p.as_int());
break;
case Crazyflie::ParamTypeFloat:
if (p.get_type() == rclcpp::PARAMETER_INTEGER) {
cf_.setParam<float>(entry->id, (float)p.as_int());
} else {
cf_.setParam<float>(entry->id, p.as_double());
}
break;
}
} else {
RCLCPP_ERROR(logger_, "[%s] Could not find param %s/%s", name_.c_str(), group.c_str(), name.c_str());
}
}
private:
void cmd_full_state_changed(const crazyflie_interfaces::msg::FullState::SharedPtr msg)
{
float x = msg->pose.position.x;
float y = msg->pose.position.y;
float z = msg->pose.position.z;
float vx = msg->twist.linear.x;
float vy = msg->twist.linear.y;
float vz = msg->twist.linear.z;
float ax = msg->acc.x;
float ay = msg->acc.y;
float az = msg->acc.z;
float qx = msg->pose.orientation.x;
float qy = msg->pose.orientation.y;
float qz = msg->pose.orientation.z;
float qw = msg->pose.orientation.w;
float rollRate = msg->twist.angular.x;
float pitchRate = msg->twist.angular.y;
float yawRate = msg->twist.angular.z;
cf_.sendFullStateSetpoint(
x, y, z,
vx, vy, vz,
ax, ay, az,
qx, qy, qz, qw,
rollRate, pitchRate, yawRate);
}
void cmd_position_changed(const crazyflie_interfaces::msg::Position::SharedPtr msg) {
float x = msg->x;
float y = msg->y;
float z = msg->z;
float yaw = msg->yaw;
cf_.sendPositionSetpoint(x, y, z, yaw);
}
void cmd_velocity_world_changed(const crazyflie_interfaces::msg::VelocityWorld::SharedPtr msg) {
float vx = msg->vel.x;
float vy = msg->vel.y;
float vz = msg->vel.z;
float yaw_rate = msg->yaw_rate;
cf_.sendVelocityWorldSetpoint(vx, vy, vz, yaw_rate);
}
void cmd_hover_changed(const crazyflie_interfaces::msg::Hover::SharedPtr msg) {
float vx = msg->vx;
float vy = msg->vy;
float yawRate = -1.0 * msg->yaw_rate * 180.0 / M_PI; // Convert from radians to degrees
float z = msg->z_distance;
cf_.sendHoverSetpoint(vx, vy, yawRate, z);
}
void cmd_vel_legacy_changed(const geometry_msgs::msg::Twist::SharedPtr msg)
{
float roll = msg->linear.y;
float pitch = - (msg->linear.x);
float yawrate = msg->angular.z;
uint16_t thrust = std::min<uint16_t>(std::max<float>(msg->linear.z, 0.0), 60000);
// RCLCPP_INFO(logger_, "roll: %f, pitch: %f, yaw: %f, thrust: %u", roll, pitch, yawrate, (unsigned int)thrust);
cf_.sendSetpoint(roll, pitch, yawrate, thrust);
}
void on_console(const char *msg)
{
message_buffer_ += msg;
size_t pos = message_buffer_.find('\n');
if (pos != std::string::npos)
{
message_buffer_[pos] = 0;
RCLCPP_INFO(logger_, "[%s] %s", name_.c_str(), message_buffer_.c_str());
message_buffer_.erase(0, pos + 1);
}
}
void emergency(const std::shared_ptr<Empty::Request> request,
std::shared_ptr<Empty::Response> response)
{
RCLCPP_INFO(logger_, "[%s] emergency()", name_.c_str());
cf_.emergencyStop();
}
void start_trajectory(const std::shared_ptr<StartTrajectory::Request> request,
std::shared_ptr<StartTrajectory::Response> response)
{
RCLCPP_INFO(logger_, "[%s] start_trajectory(id=%d, timescale=%f, reversed=%d, relative=%d, group_mask=%d)",
name_.c_str(),
request->trajectory_id,
request->timescale,
request->reversed,
request->relative,
request->group_mask);
cf_.startTrajectory(request->trajectory_id,
request->timescale,
request->reversed,
request->relative,
request->group_mask);
}
void takeoff(const std::shared_ptr<Takeoff::Request> request,
std::shared_ptr<Takeoff::Response> response)
{
RCLCPP_INFO(logger_, "[%s] takeoff(height=%f m, duration=%f s, group_mask=%d)",
name_.c_str(),
request->height,
rclcpp::Duration(request->duration).seconds(),
request->group_mask);
cf_.takeoff(request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask);
}
void land(const std::shared_ptr<Land::Request> request,
std::shared_ptr<Land::Response> response)
{
RCLCPP_INFO(logger_, "[%s] land(height=%f m, duration=%f s, group_mask=%d)",
name_.c_str(),
request->height,
rclcpp::Duration(request->duration).seconds(),
request->group_mask);
cf_.land(request->height, rclcpp::Duration(request->duration).seconds(), request->group_mask);
}
void go_to(const std::shared_ptr<GoTo::Request> request,
std::shared_ptr<GoTo::Response> response)
{
RCLCPP_INFO(logger_, "[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)",
name_.c_str(),
request->goal.x, request->goal.y, request->goal.z, request->yaw,
rclcpp::Duration(request->duration).seconds(),
request->relative,
request->group_mask);
cf_.goTo(request->goal.x, request->goal.y, request->goal.z, request->yaw,
rclcpp::Duration(request->duration).seconds(),
request->relative, request->group_mask);
}
void upload_trajectory(const std::shared_ptr<UploadTrajectory::Request> request,
std::shared_ptr<UploadTrajectory::Response> response)
{
RCLCPP_INFO(logger_, "[%s] upload_trajectory(id=%d, offset=%d)",
name_.c_str(),
request->trajectory_id,
request->piece_offset);
std::vector<Crazyflie::poly4d> pieces(request->pieces.size());
for (size_t i = 0; i < pieces.size(); ++i)
{
if ( request->pieces[i].poly_x.size() != 8
|| request->pieces[i].poly_y.size() != 8
|| request->pieces[i].poly_z.size() != 8
|| request->pieces[i].poly_yaw.size() != 8)
{
RCLCPP_FATAL(logger_, "[%s] Wrong number of pieces!", name_.c_str());
return;
}
pieces[i].duration = rclcpp::Duration(request->pieces[i].duration).seconds();
for (size_t j = 0; j < 8; ++j)
{
pieces[i].p[0][j] = request->pieces[i].poly_x[j];
pieces[i].p[1][j] = request->pieces[i].poly_y[j];
pieces[i].p[2][j] = request->pieces[i].poly_z[j];
pieces[i].p[3][j] = request->pieces[i].poly_yaw[j];
}
}
cf_.uploadTrajectory(request->trajectory_id, request->piece_offset, pieces);
}
void notify_setpoints_stop(const std::shared_ptr<NotifySetpointsStop::Request> request,
std::shared_ptr<NotifySetpointsStop::Response> response)
{
RCLCPP_INFO(logger_, "[%s] notify_setpoints_stop(remain_valid_millisecs%d, group_mask=%d)",
name_.c_str(),
request->remain_valid_millisecs,
request->group_mask);
cf_.notifySetpointsStop(request->remain_valid_millisecs);
}
void arm(const std::shared_ptr<Arm::Request> request,
std::shared_ptr<Arm::Response> response)
{
RCLCPP_INFO(logger_, "[%s] arm(%d)",
name_.c_str(),
request->arm);
cf_.sendArmingRequest(request->arm);
}
void on_logging_pose(uint32_t time_in_ms, const logPose* data) {
if (publisher_pose_) {
geometry_msgs::msg::PoseStamped msg;
msg.header.stamp = node_->get_clock()->now();
msg.header.frame_id = reference_frame_;
msg.pose.position.x = data->x;
msg.pose.position.y = data->y;
msg.pose.position.z = data->z;
float q[4];
quatdecompress(data->quatCompressed, q);
msg.pose.orientation.x = q[0];
msg.pose.orientation.y = q[1];
msg.pose.orientation.z = q[2];
msg.pose.orientation.w = q[3];
publisher_pose_->publish(msg);
// send a transform for this pose
geometry_msgs::msg::TransformStamped msg2;
msg2.header = msg.header;
msg2.child_frame_id = name_;
msg2.transform.translation.x = data->x;
msg2.transform.translation.y = data->y;
msg2.transform.translation.z = data->z;
msg2.transform.rotation.x = q[0];
msg2.transform.rotation.y = q[1];
msg2.transform.rotation.z = q[2];
msg2.transform.rotation.w = q[3];
tf_broadcaster_.sendTransform(msg2);
}
}
void on_logging_scan(uint32_t time_in_ms, const logScan* data) {
if (publisher_scan_) {
const float max_range = 3.49;
float front_range = data->front / 1000.0f;
if (front_range > max_range) front_range = std::numeric_limits<float>::infinity();
float left_range = data->left / 1000.0f;
if (left_range > max_range) left_range = std::numeric_limits<float>::infinity();
float back_range = data->back / 1000.0f;
if (back_range > max_range) back_range = std::numeric_limits<float>::infinity();
float right_range = data->right / 1000.0f;
if (right_range > max_range) right_range = std::numeric_limits<float>::infinity();
sensor_msgs::msg::LaserScan msg;
msg.header.stamp = node_->get_clock()->now();
msg.header.frame_id = name_;
msg.range_min = 0.01;
msg.range_max = max_range;
msg.ranges.push_back(back_range);
msg.ranges.push_back(right_range);
msg.ranges.push_back(front_range);
msg.ranges.push_back(left_range);
msg.angle_min = -0.5 * 2 * M_PI;
msg.angle_max = 0.25 * 2 * M_PI;
msg.angle_increment = 1.0 * M_PI / 2;
publisher_scan_->publish(msg);
}
}
void on_logging_odom(uint32_t time_in_ms, const logOdom* data) {
if (publisher_odom_) {
nav_msgs::msg::Odometry msg;
msg.header.stamp = node_->get_clock()->now();
msg.header.frame_id = name_;
msg.pose.pose.position.x = data->x / 1000.0f;
msg.pose.pose.position.y = data->y / 1000.0f;
msg.pose.pose.position.z = data->z / 1000.0f;
float q[4];
quatdecompress(data->quatCompressed, q);
msg.pose.pose.orientation.x = q[0];
msg.pose.pose.orientation.y = q[1];
msg.pose.pose.orientation.z = q[2];
msg.pose.pose.orientation.w = q[3];
msg.twist.twist.linear.x = data->vx / 1000.0f;
msg.twist.twist.linear.y = data->vy / 1000.0f;
msg.twist.twist.linear.z = data->vz / 1000.0f;
//msg.twist.twist.angular.x = data->rateRoll / 1000.0f;
//msg.twist.twist.angular.y = data->ratePitch / 1000.0f;
//msg.twist.twist.angular.z = data->rateYaw / 1000.0f;
publisher_odom_->publish(msg);
}
}
void on_logging_status(uint32_t time_in_ms, const logStatus* data) {
if (publisher_status_) {
crazyflie_interfaces::msg::Status msg;
msg.header.stamp = node_->get_clock()->now();
msg.header.frame_id = name_;
msg.supervisor_info = data->supervisorInfo;
msg.battery_voltage = data->vbatMV / 1000.0f;
msg.pm_state = data->pmState;
msg.rssi = data->rssi;
if (status_has_radio_stats_) {
int32_t deltaRxBc = data->numRxBc - previous_numRxBc;
int32_t deltaRxUc = data->numRxUc - previous_numRxUc;
// handle overflow
if (deltaRxBc < 0) {
deltaRxBc += std::numeric_limits<uint16_t>::max();
}
if (deltaRxUc < 0) {
deltaRxUc += std::numeric_limits<uint16_t>::max();
}
msg.num_rx_broadcast = deltaRxBc;
msg.num_rx_unicast = deltaRxUc;
previous_numRxBc = data->numRxBc;
previous_numRxUc = data->numRxUc;
} else {
msg.num_rx_broadcast = 0;
msg.num_rx_unicast = 0;
}
// connection sent stats (unicast)
const auto statsUc = cf_.connectionStats();
size_t deltaTxUc = statsUc.sent_count - previous_stats_unicast_.sent_count;
msg.num_tx_unicast = deltaTxUc;
previous_stats_unicast_ = statsUc;
// connection sent stats (broadcast)
const auto statsBc = cfbc_->connectionStats();
size_t deltaTxBc = statsBc.sent_count - previous_stats_broadcast_.sent_count;
msg.num_tx_broadcast = deltaTxBc;
previous_stats_broadcast_ = statsBc;
msg.latency_unicast = last_latency_in_ms_;
publisher_status_->publish(msg);
// warnings
if (msg.num_rx_unicast > msg.num_tx_unicast * 1.05 /*allow some slack*/) {
RCLCPP_WARN(logger_, "[%s] Unexpected number of unicast packets. Sent: %d. Received: %d", name_.c_str(), msg.num_tx_unicast, msg.num_rx_unicast);
}
if (msg.num_tx_unicast > 0) {
float unicast_receive_rate = msg.num_rx_unicast / (float)msg.num_tx_unicast;
if (unicast_receive_rate < min_unicast_receive_rate_) {
RCLCPP_WARN(logger_, "[%s] Low unicast receive rate (%.2f < %.2f). Sent: %d. Received: %d", name_.c_str(), unicast_receive_rate, min_unicast_receive_rate_, msg.num_tx_unicast, msg.num_rx_unicast);
}
}
if (msg.num_rx_broadcast > msg.num_tx_broadcast * 1.05 /*allow some slack*/) {
RCLCPP_WARN(logger_, "[%s] Unexpected number of broadcast packets. Sent: %d. Received: %d", name_.c_str(), msg.num_tx_broadcast, msg.num_rx_broadcast);
}
if (msg.num_tx_broadcast > 0) {
float broadcast_receive_rate = msg.num_rx_broadcast / (float)msg.num_tx_broadcast;
if (broadcast_receive_rate < min_broadcast_receive_rate_) {
RCLCPP_WARN(logger_, "[%s] Low broadcast receive rate (%.2f < %.2f). Sent: %d. Received: %d", name_.c_str(), broadcast_receive_rate, min_broadcast_receive_rate_, msg.num_tx_broadcast, msg.num_rx_broadcast);
}
}
}
}
void on_logging_custom(uint32_t time_in_ms, const std::vector<float>* values, void* userData) {
auto pub = reinterpret_cast<rclcpp::Publisher<crazyflie_interfaces::msg::LogDataGeneric>::SharedPtr*>(userData);
crazyflie_interfaces::msg::LogDataGeneric msg;
msg.header.stamp = node_->get_clock()->now();
msg.header.frame_id = reference_frame_;
msg.timestamp = time_in_ms;
msg.values = *values;
(*pub)->publish(msg);
}
void on_link_statistics_timer()
{
cf_.triggerLatencyMeasurement();
auto now = std::chrono::steady_clock::now();
std::chrono::duration<double> elapsed = now - last_on_latency_;
if (elapsed.count() > 1.0 / warning_freq_) {
RCLCPP_WARN(logger_, "[%s] last latency update: %f s", name_.c_str(), elapsed.count());
}
auto stats = cf_.connectionStatsDelta();
if (stats.ack_count > 0) {
float ack_rate = stats.sent_count / stats.ack_count;
if (ack_rate < min_ack_rate_) {
RCLCPP_WARN(logger_, "[%s] Ack rate: %.1f %%", name_.c_str(), ack_rate * 100);
}
}
if (publish_stats_) {
crazyflie_interfaces::msg::ConnectionStatisticsArray msg;
msg.header.stamp = node_->get_clock()->now();
msg.header.frame_id = reference_frame_;
msg.stats.resize(1);
msg.stats[0].uri = cf_.uri();
msg.stats[0].sent_count = stats.sent_count;
msg.stats[0].sent_ping_count = stats.sent_ping_count;
msg.stats[0].receive_count = stats.receive_count;
msg.stats[0].enqueued_count = stats.enqueued_count;
msg.stats[0].ack_count = stats.ack_count;
publisher_connection_stats_->publish(msg);
}
}
void on_latency(uint64_t latency_in_us)
{
if (latency_in_us / 1000.0 > max_latency_) {
RCLCPP_WARN(logger_, "[%s] High latency: %.1f ms", name_.c_str(), latency_in_us / 1000.0);
}
last_on_latency_ = std::chrono::steady_clock::now();
last_latency_in_ms_ = (uint16_t)(latency_in_us / 1000.0);
}
private:
rclcpp::Logger logger_;
CrazyflieLogger cf_logger_;
Crazyflie cf_;
std::string message_buffer_;
std::string name_;
rclcpp::Node* node_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
rclcpp::Service<Empty>::SharedPtr service_emergency_;
rclcpp::Service<StartTrajectory>::SharedPtr service_start_trajectory_;
rclcpp::Service<Takeoff>::SharedPtr service_takeoff_;
rclcpp::Service<Land>::SharedPtr service_land_;
rclcpp::Service<GoTo>::SharedPtr service_go_to_;
rclcpp::Service<UploadTrajectory>::SharedPtr service_upload_trajectory_;
rclcpp::Service<NotifySetpointsStop>::SharedPtr service_notify_setpoints_stop_;
rclcpp::Service<Arm>::SharedPtr service_arm_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_cmd_vel_legacy_;
rclcpp::Subscription<crazyflie_interfaces::msg::FullState>::SharedPtr subscription_cmd_full_state_;
rclcpp::Subscription<crazyflie_interfaces::msg::Position>::SharedPtr subscription_cmd_position_;
rclcpp::Subscription<crazyflie_interfaces::msg::VelocityWorld>::SharedPtr subscription_cmd_velocity_world_;
rclcpp::Subscription<crazyflie_interfaces::msg::Hover>::SharedPtr subscription_cmd_hover_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_robot_description_;
// logging
std::string reference_frame_;
std::unique_ptr<LogBlock<logPose>> log_block_pose_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_pose_;
std::unique_ptr<LogBlock<logScan>> log_block_scan_;
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr publisher_scan_;
std::unique_ptr<LogBlock<logOdom>> log_block_odom_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr publisher_odom_;
std::unique_ptr<LogBlock<logStatus>> log_block_status_;
bool status_has_radio_stats_;
rclcpp::Publisher<crazyflie_interfaces::msg::Status>::SharedPtr publisher_status_;
uint16_t previous_numRxBc;
uint16_t previous_numRxUc;
bitcraze::crazyflieLinkCpp::Connection::Statistics previous_stats_unicast_;
bitcraze::crazyflieLinkCpp::Connection::Statistics previous_stats_broadcast_;
const CrazyflieBroadcaster* cfbc_;
std::list<std::unique_ptr<LogBlockGeneric>> log_blocks_generic_;
std::list<rclcpp::Publisher<crazyflie_interfaces::msg::LogDataGeneric>::SharedPtr> publishers_generic_;
// multithreading
rclcpp::CallbackGroup::SharedPtr callback_group_cf_;
rclcpp::TimerBase::SharedPtr spin_timer_;
// link statistics
rclcpp::TimerBase::SharedPtr link_statistics_timer_;
std::chrono::time_point<std::chrono::steady_clock> last_on_latency_;
uint16_t last_latency_in_ms_;
float warning_freq_;
float max_latency_;
float min_ack_rate_;
float min_unicast_receive_rate_;
float min_broadcast_receive_rate_;
bool publish_stats_;
rclcpp::Publisher<crazyflie_interfaces::msg::ConnectionStatisticsArray>::SharedPtr publisher_connection_stats_;
};
class CrazyflieServer : public rclcpp::Node
{
public:
CrazyflieServer()
: Node("crazyflie_server")
, logger_(get_logger())
{
// Create callback groups (each group can run in a separate thread)
callback_group_mocap_ = this->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
auto sub_opt_mocap = rclcpp::SubscriptionOptions();
sub_opt_mocap.callback_group = callback_group_mocap_;
callback_group_all_cmd_ = this->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
auto sub_opt_all_cmd = rclcpp::SubscriptionOptions();
sub_opt_all_cmd.callback_group = callback_group_all_cmd_;
callback_group_all_srv_ = this->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
callback_group_cf_cmd_ = this->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
callback_group_cf_srv_ = this->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
// declare global params
this->declare_parameter("all.broadcasts.num_repeats", 15);
this->declare_parameter("all.broadcasts.delay_between_repeats_ms", 1);
this->declare_parameter("firmware_params.query_all_values_on_connect", false);
broadcasts_num_repeats_ = this->get_parameter("all.broadcasts.num_repeats").get_parameter_value().get<int>();
broadcasts_delay_between_repeats_ms_ = this->get_parameter("all.broadcasts.delay_between_repeats_ms").get_parameter_value().get<int>();
mocap_enabled_ = false;
this->declare_parameter("robot_description", "");
// Warnings
this->declare_parameter("warnings.frequency", 1.0);
float freq = this->get_parameter("warnings.frequency").get_parameter_value().get<float>();
if (freq >= 0.0) {
watchdog_timer_ = this->create_wall_timer(std::chrono::milliseconds((int)(1000.0/freq)), std::bind(&CrazyflieServer::on_watchdog_timer, this), callback_group_all_srv_);
}
this->declare_parameter("warnings.motion_capture.warning_if_rate_outside", std::vector<double>({80.0, 120.0}));
auto rate_range = this->get_parameter("warnings.motion_capture.warning_if_rate_outside").get_parameter_value().get<std::vector<double>>();
mocap_min_rate_ = rate_range[0];
mocap_max_rate_ = rate_range[1];
this->declare_parameter("warnings.communication.max_unicast_latency", 10.0);
this->declare_parameter("warnings.communication.min_unicast_ack_rate", 0.9);
this->declare_parameter("warnings.communication.min_unicast_receive_rate", 0.9);
this->declare_parameter("warnings.communication.min_broadcast_receive_rate", 0.9);
this->declare_parameter("warnings.communication.publish_stats", false);
publish_stats_ = this->get_parameter("warnings.communication.publish_stats").get_parameter_value().get<bool>();
if (publish_stats_) {
publisher_connection_stats_ = this->create_publisher<crazyflie_interfaces::msg::ConnectionStatisticsArray>("all/connection_statistics", 10);
}
// load crazyflies from params
auto node_parameters_iface = this->get_node_parameters_interface();
const std::map<std::string, rclcpp::ParameterValue> ¶meter_overrides =
node_parameters_iface->get_parameter_overrides();
auto cf_names = extract_names(parameter_overrides, "robots");
for (const auto &name : cf_names) {
bool enabled = parameter_overrides.at("robots." + name + ".enabled").get<bool>();
if (enabled) {
// Lookup type
std::string cf_type = parameter_overrides.at("robots." + name + ".type").get<std::string>();
// Find the connection setting for the given type
const auto con = parameter_overrides.find("robot_types." + cf_type + ".connection");
std::string constr = "crazyflie";
if (con != parameter_overrides.end()) {
constr = con->second.get<std::string>();
}
// Find the mocap setting
const auto mocap_en = parameter_overrides.find("robot_types." + cf_type + ".motion_capture.enabled");
if (mocap_en != parameter_overrides.end()) {
if (mocap_en->second.get<bool>()) {
mocap_enabled_ = true;
}
}
// if it is a Crazyflie, try to connect
if (constr == "crazyflie") {
std::string uri = parameter_overrides.at("robots." + name + ".uri").get<std::string>();
auto broadcastUri = Crazyflie::broadcastUriFromUnicastUri(uri);
if (broadcaster_.count(broadcastUri) == 0) {
broadcaster_.emplace(broadcastUri, std::make_unique<CrazyflieBroadcaster>(broadcastUri));
}
crazyflies_.emplace(name, std::make_unique<CrazyflieROS>(
uri,
cf_type,
name,
this,
callback_group_cf_cmd_,
callback_group_cf_srv_,
broadcaster_.at(broadcastUri).get()));
update_name_to_id_map(name, crazyflies_[name]->id());
}
else if (constr == "none") {
// we still might want to track this object, so update our map
uint8_t id = parameter_overrides.at("robots." + name + ".id").get<uint8_t>();
update_name_to_id_map(name, id);
} else {
RCLCPP_INFO(logger_, "[all] Unknown connection type %s", constr.c_str());
}
}
}
this->declare_parameter("poses_qos_deadline", 100.0f);
double poses_qos_deadline = this->get_parameter("poses_qos_deadline").get_parameter_value().get<double>();
rclcpp::SensorDataQoS sensor_data_qos;
sensor_data_qos.keep_last(1);
sensor_data_qos.deadline(rclcpp::Duration(0/*s*/, 1e9/poses_qos_deadline /*ns*/));
sub_poses_ = this->create_subscription<NamedPoseArray>(
"poses", sensor_data_qos, std::bind(&CrazyflieServer::posesChanged, this, _1), sub_opt_mocap);
// support for all.params
// Create a parameter subscriber that can be used to monitor parameter changes
param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
cb_handle_ = param_subscriber_->add_parameter_event_callback(std::bind(&CrazyflieServer::on_parameter_event, this, _1));
// topics for "all"
subscription_cmd_full_state_ = this->create_subscription<crazyflie_interfaces::msg::FullState>("all/cmd_full_state", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieServer::cmd_full_state_changed, this, _1), sub_opt_all_cmd);
// services for "all"
service_start_trajectory_ = this->create_service<StartTrajectory>("all/start_trajectory", std::bind(&CrazyflieServer::start_trajectory, this, _1, _2), get_service_qos(), callback_group_all_srv_);
service_takeoff_ = this->create_service<Takeoff>("all/takeoff", std::bind(&CrazyflieServer::takeoff, this, _1, _2), get_service_qos(), callback_group_all_srv_);
service_land_ = this->create_service<Land>("all/land", std::bind(&CrazyflieServer::land, this, _1, _2), get_service_qos(), callback_group_all_srv_);
service_go_to_ = this->create_service<GoTo>("all/go_to", std::bind(&CrazyflieServer::go_to, this, _1, _2), get_service_qos(), callback_group_all_srv_);
service_notify_setpoints_stop_ = this->create_service<NotifySetpointsStop>("all/notify_setpoints_stop", std::bind(&CrazyflieServer::notify_setpoints_stop, this, _1, _2), get_service_qos(), callback_group_all_srv_);
service_arm_ = this->create_service<Arm>("all/arm", std::bind(&CrazyflieServer::arm, this, _1, _2), get_service_qos(), callback_group_all_srv_);
// This is the last service to announce and can be used to check if the server is fully available
service_emergency_ = this->create_service<Empty>("all/emergency", std::bind(&CrazyflieServer::emergency, this, _1, _2), get_service_qos(), callback_group_all_srv_);
}
private:
void emergency(const std::shared_ptr<Empty::Request> request,
std::shared_ptr<Empty::Response> response)
{
RCLCPP_INFO(logger_, "[all] emergency()");
for (int i = 0; i < broadcasts_num_repeats_; ++i)
{
for (auto &bc :
gitextract_puxxq204/
├── .github/
│ └── workflows/
│ ├── ci-docs2.yml
│ ├── ci-ros2-mac.yml
│ ├── ci-ros2-win.yml
│ ├── ci-ros2.yml
│ ├── systemtests.yml
│ └── systemtests_sim.yml
├── .gitignore
├── .gitmodules
├── LICENSE
├── README.md
├── crazyflie/
│ ├── CHANGELOG.rst
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── aideck_streamer.yaml
│ │ ├── config.rviz
│ │ ├── crazyflies.yaml
│ │ ├── motion_capture.yaml
│ │ ├── server.yaml
│ │ └── teleop.yaml
│ ├── launch/
│ │ ├── launch.py
│ │ ├── launch_teleop2.py
│ │ └── teleop_launch.py
│ ├── package.xml
│ ├── scripts/
│ │ ├── aideck_streamer.py
│ │ ├── cfmult.py
│ │ ├── chooser.py
│ │ ├── crazyflie_server.py
│ │ ├── flash.py
│ │ ├── gui.py
│ │ ├── simple_mapper_multiranger.py
│ │ └── vel_mux.py
│ ├── src/
│ │ ├── crazyflie_server.cpp
│ │ └── teleop.cpp
│ └── urdf/
│ ├── cf2_assembly.stl
│ ├── cf2_assembly_with_props.dae
│ └── crazyflie_description.urdf
├── crazyflie_examples/
│ ├── CHANGELOG.rst
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── nav2_params.yaml
│ │ ├── nav2_view.rviz
│ │ └── slam_params.yaml
│ ├── crazyflie_examples/
│ │ ├── __init__.py
│ │ ├── arming.py
│ │ ├── cmd_full_state.py
│ │ ├── data/
│ │ │ ├── figure8.csv
│ │ │ └── multi_trajectory/
│ │ │ ├── traj0.csv
│ │ │ └── traj1.csv
│ │ ├── figure8.py
│ │ ├── goto_unicast.py
│ │ ├── group_mask.py
│ │ ├── hello_world.py
│ │ ├── infinite_flight.py
│ │ ├── multi_trajectory.py
│ │ ├── nice_hover.py
│ │ ├── set_param.py
│ │ └── swap.py
│ ├── data/
│ │ ├── map.data
│ │ ├── map.pgm
│ │ ├── map.posegraph
│ │ └── map.yaml
│ ├── launch/
│ │ ├── keyboard_velmux_launch.py
│ │ ├── launch.py
│ │ ├── multiranger_mapping_launch.py
│ │ ├── multiranger_nav2_launch.py
│ │ └── multiranger_simple_mapper_launch.py
│ ├── package.xml
│ ├── resource/
│ │ └── crazyflie_examples
│ ├── setup.cfg
│ └── test/
│ ├── test_flake8.py
│ └── test_pep257.py
├── crazyflie_interfaces/
│ ├── CHANGELOG.rst
│ ├── CMakeLists.txt
│ ├── msg/
│ │ ├── ConnectionStatistics.msg
│ │ ├── ConnectionStatisticsArray.msg
│ │ ├── FullState.msg
│ │ ├── Hover.msg
│ │ ├── LogBlock.msg
│ │ ├── LogDataGeneric.msg
│ │ ├── Position.msg
│ │ ├── Status.msg
│ │ ├── TrajectoryPolynomialPiece.msg
│ │ └── VelocityWorld.msg
│ ├── package.xml
│ └── srv/
│ ├── AddLogging.srv
│ ├── Arm.srv
│ ├── GoTo.srv
│ ├── Land.srv
│ ├── NotifySetpointsStop.srv
│ ├── RemoveLogging.srv
│ ├── SetGroupMask.srv
│ ├── StartTrajectory.srv
│ ├── Stop.srv
│ ├── Takeoff.srv
│ ├── UpdateParams.srv
│ └── UploadTrajectory.srv
├── crazyflie_py/
│ ├── CHANGELOG.rst
│ ├── crazyflie_py/
│ │ ├── __init__.py
│ │ ├── crazyflie.py
│ │ ├── crazyswarm_py.py
│ │ ├── genericJoystick.py
│ │ ├── joystick.py
│ │ ├── keyboard.py
│ │ ├── linuxjsdev.py
│ │ ├── uav_trajectory.py
│ │ └── util.py
│ ├── package.xml
│ ├── resource/
│ │ └── crazyflie_py
│ ├── setup.cfg
│ ├── setup.py
│ └── test/
│ ├── test_flake8.py
│ └── test_pep257.py
├── crazyflie_sim/
│ ├── CHANGELOG.rst
│ ├── CMakeLists.txt
│ ├── crazyflie_sim/
│ │ ├── __init__.py
│ │ ├── backend/
│ │ │ ├── data/
│ │ │ │ ├── dynobench/
│ │ │ │ │ └── crazyflie2.yaml
│ │ │ │ ├── neuralswarm2/
│ │ │ │ │ ├── phi_G.pth
│ │ │ │ │ ├── phi_L.pth
│ │ │ │ │ ├── phi_S.pth
│ │ │ │ │ ├── rho_L.pth
│ │ │ │ │ └── rho_S.pth
│ │ │ │ └── pinocchio/
│ │ │ │ └── crazyflie2.urdf
│ │ │ ├── dynobench.py
│ │ │ ├── neuralswarm.py
│ │ │ ├── none.py
│ │ │ ├── np.py
│ │ │ └── pinocchio.py
│ │ ├── crazyflie_server.py
│ │ ├── crazyflie_sil.py
│ │ ├── sim_data_types.py
│ │ └── visualization/
│ │ ├── README.md
│ │ ├── blender.py
│ │ ├── data/
│ │ │ ├── README.md
│ │ │ └── model/
│ │ │ ├── cf.mtl
│ │ │ └── cf.obj
│ │ ├── pdf.py
│ │ ├── record_states.py
│ │ └── rviz.py
│ ├── package.xml
│ ├── resource/
│ │ └── crazyflie_sim
│ ├── setup.cfg
│ └── test/
│ ├── test_flake8.py
│ └── test_pep257.py
├── docs/
│ ├── .gitignore
│ ├── Makefile
│ ├── api.rst
│ ├── changelog.rst
│ ├── conf.py
│ ├── configuration.rst
│ ├── generate_install_deps_code.py
│ ├── generated/
│ │ └── .gitignore
│ ├── gettingstarted.rst
│ ├── glossary.rst
│ ├── hardware.rst
│ ├── howto/
│ │ ├── git_integration.rstinclude
│ │ ├── howto.rst
│ │ └── streaming_setpoint.rstinclude
│ ├── index.rst
│ ├── installation.rst
│ ├── internals.rst
│ ├── requirements.txt
│ └── tutorials/
│ ├── hover.rstinclude
│ └── tutorials.rst
├── docs2/
│ ├── .gitignore
│ ├── Makefile
│ ├── conf.py
│ ├── faq.rst
│ ├── howto.rst
│ ├── index.rst
│ ├── installation.rst
│ ├── overview.rst
│ ├── requirements.txt
│ ├── tutorials.rst
│ └── usage.rst
├── ros_ws/
│ └── src/
│ └── crazyswarm/
│ ├── launch/
│ │ ├── USC.yaml
│ │ ├── allCrazyflies.yaml
│ │ ├── figure8_smooth.csv
│ │ ├── hover_swarm.launch
│ │ ├── mocap_helper.launch
│ │ ├── object_tracker.yaml
│ │ ├── rqt.perspective
│ │ └── test.rviz
│ └── scripts/
│ ├── backgroundComputation.py
│ ├── cmdVelocityCircle.py
│ ├── collisionAvoidance.py
│ ├── collisionAvoidanceHighConflict.py
│ ├── conftest.py
│ ├── crossing2_pps/
│ │ ├── pp1.csv
│ │ └── pp2.csv
│ ├── crossing4_pps/
│ │ ├── pp1.csv
│ │ ├── pp2.csv
│ │ ├── pp3.csv
│ │ └── pp4.csv
│ ├── csv_sequence.py
│ ├── example_cmd_pos.py
│ ├── graphVisualization.py
│ ├── individual_hover.py
│ ├── led_colors.py
│ ├── pytest.ini
│ ├── sequence_trajectories/
│ │ ├── 1/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 2/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 3/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 4/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 5/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ ├── 6/
│ │ │ ├── 1.csv
│ │ │ ├── 10.csv
│ │ │ ├── 11.csv
│ │ │ ├── 12.csv
│ │ │ ├── 13.csv
│ │ │ ├── 14.csv
│ │ │ ├── 15.csv
│ │ │ ├── 16.csv
│ │ │ ├── 17.csv
│ │ │ ├── 18.csv
│ │ │ ├── 19.csv
│ │ │ ├── 2.csv
│ │ │ ├── 3.csv
│ │ │ ├── 4.csv
│ │ │ ├── 5.csv
│ │ │ ├── 6.csv
│ │ │ ├── 7.csv
│ │ │ ├── 8.csv
│ │ │ └── 9.csv
│ │ └── 7/
│ │ ├── 1.csv
│ │ ├── 10.csv
│ │ ├── 11.csv
│ │ ├── 12.csv
│ │ ├── 13.csv
│ │ ├── 14.csv
│ │ ├── 15.csv
│ │ ├── 16.csv
│ │ ├── 17.csv
│ │ ├── 18.csv
│ │ ├── 19.csv
│ │ ├── 2.csv
│ │ ├── 3.csv
│ │ ├── 4.csv
│ │ ├── 5.csv
│ │ ├── 6.csv
│ │ ├── 7.csv
│ │ ├── 8.csv
│ │ └── 9.csv
│ ├── swap6v.py
│ ├── swap6v_pps/
│ │ ├── pp1.csv
│ │ ├── pp2.csv
│ │ ├── pp3.csv
│ │ ├── pp4.csv
│ │ ├── pp5.csv
│ │ └── pp6.csv
│ ├── takeoff.csv
│ ├── test_collisionAvoidance.py
│ ├── test_highLevel.py
│ ├── test_lowLevel.py
│ ├── test_ros.py
│ ├── test_simOnly.py
│ ├── test_videoOutput.py
│ ├── test_yamlString.py
│ ├── udp_multicast.py
│ ├── waypoints.csv
│ ├── waypoints.py
│ └── waypoints_simple.py
├── rosinstall
└── systemtests/
├── SDplotting/
│ ├── cfusdlog.py
│ ├── data_helper.py
│ ├── plot.py
│ ├── save.py
│ └── settings.yaml
├── figure8_ideal_traj.csv
├── mcap_handler.py
├── multi_trajectory_ideal_traj0.csv
├── plotter_class.py
└── test_flights.py
SYMBOL INDEX (498 symbols across 80 files)
FILE: crazyflie/launch/launch.py
function parse_yaml (line 11) | def parse_yaml(context):
function generate_launch_description (line 99) | def generate_launch_description():
FILE: crazyflie/launch/launch_teleop2.py
function generate_launch_description (line 8) | def generate_launch_description():
FILE: crazyflie/launch/teleop_launch.py
function generate_launch_description (line 4) | def generate_launch_description():
FILE: crazyflie/scripts/aideck_streamer.py
class ImageStreamerNode (line 16) | class ImageStreamerNode(Node):
method __init__ (line 17) | def __init__(self):
method _construct_from_yaml (line 98) | def _construct_from_yaml(self, config):
method _rx_bytes (line 112) | def _rx_bytes(self, size):
method receive_callback (line 118) | def receive_callback(self):
method publish_callback (line 142) | def publish_callback(self):
function main (line 161) | def main(args=None):
FILE: crazyflie/scripts/cfmult.py
function main (line 33) | def main():
function _read_yaml_uris (line 93) | def _read_yaml_uris(configpath):
FILE: crazyflie/scripts/chooser.py
function selected_cfs (line 45) | def selected_cfs():
function save (line 49) | def save():
function dot (line 68) | def dot(a, b):
class CFWidget (line 84) | class CFWidget(Tkinter.Frame):
method __init__ (line 85) | def __init__(self, parent, name):
function minmax (line 110) | def minmax(a, b):
function mouseDown (line 113) | def mouseDown(event):
function mouseUp (line 118) | def mouseUp(event):
function drag (line 121) | def drag(event, select):
function clear (line 148) | def clear():
function fill (line 153) | def fill():
function mkbutton (line 158) | def mkbutton(parent, name, command):
function sysOff (line 167) | def sysOff():
function reboot (line 173) | def reboot():
function flashSTM (line 180) | def flashSTM():
function flashNRF (line 187) | def flashNRF():
function checkBattery (line 194) | def checkBattery():
function checkBatteryLoop (line 269) | def checkBatteryLoop():
FILE: crazyflie/scripts/crazyflie_server.py
class CrazyflieServer (line 73) | class CrazyflieServer(Node):
method __init__ (line 74) | def __init__(self):
method _init_topics_and_services (line 272) | def _init_topics_and_services(self):
method _init_default_logblocks (line 367) | def _init_default_logblocks(self, prefix, link_uri, list_logvar, globa...
method _param_to_dict (line 414) | def _param_to_dict(self, param_ros):
method _latency_callback (line 428) | def _latency_callback(self, latency, uri=""):
method _uplink_rate_callback (line 434) | def _uplink_rate_callback(self, uplink_rate, uri=""):
method _downlink_rate_callback (line 440) | def _downlink_rate_callback(self, downlink_rate, uri=""):
method _connected (line 446) | def _connected(self, link_uri):
method _fully_connected (line 467) | def _fully_connected(self, link_uri):
method _disconnected (line 487) | def _disconnected(self, link_uri):
method _connection_failed (line 490) | def _connection_failed(self, link_uri, msg):
method _init_logging (line 494) | def _init_logging(self):
method _init_default_logging (line 542) | def _init_default_logging(self, prefix, link_uri, callback_fnc):
method _log_scan_data_callback (line 567) | def _log_scan_data_callback(self, timestamp, data, logconf, uri):
method _log_pose_data_callback (line 603) | def _log_pose_data_callback(self, timestamp, data, logconf, uri):
method _log_odom_data_callback (line 651) | def _log_odom_data_callback(self, timestamp, data, logconf, uri):
method _log_status_data_callback (line 713) | def _log_status_data_callback(self, timestamp, data, logconf, uri):
method _log_custom_data_callback (line 739) | def _log_custom_data_callback(self, timestamp, data, logconf, uri):
method _log_error_callback (line 757) | def _log_error_callback(self, logconf, msg):
method _init_parameters (line 760) | def _init_parameters(self):
method _parameters_callback (line 849) | def _parameters_callback(self, params):
method _emergency_callback (line 893) | def _emergency_callback(self, request, response, uri="all"):
method _arm_callback (line 902) | def _arm_callback(self, request, response, uri="all"):
method _takeoff_callback (line 924) | def _takeoff_callback(self, request, response, uri="all"):
method _land_callback (line 950) | def _land_callback(self, request, response, uri="all"):
method _go_to_callback (line 974) | def _go_to_callback(self, request, response, uri="all"):
method _notify_setpoints_stop_callback (line 1018) | def _notify_setpoints_stop_callback(self, request, response, uri="all"):
method _upload_trajectory_callback (line 1030) | def _upload_trajectory_callback(self, request, response, uri="all"):
method _start_trajectory_callback (line 1085) | def _start_trajectory_callback(self, request, response, uri="all"):
method _poses_changed (line 1111) | def _poses_changed(self, msg):
method _cmd_vel_legacy_changed (line 1136) | def _cmd_vel_legacy_changed(self, msg, uri=""):
method _cmd_position_changed (line 1153) | def _cmd_position_changed(self, msg, uri=""):
method _cmd_velocity_world_changed (line 1165) | def _cmd_velocity_world_changed(self, msg, uri=""):
method _cmd_hover_changed (line 1178) | def _cmd_hover_changed(self, msg, uri=""):
method _cmd_full_state_changed (line 1190) | def _cmd_full_state_changed(self, msg, uri=""):
method _remove_logging (line 1203) | def _remove_logging(self, request, response, uri="all"):
method _add_logging (line 1238) | def _add_logging(self, request, response, uri="all"):
function main (line 1307) | def main(args=None):
FILE: crazyflie/scripts/flash.py
class Flash (line 17) | class Flash(Node):
method __init__ (line 18) | def __init__(self, uri, file_name):
function main (line 44) | def main(args=None):
FILE: crazyflie/scripts/gui.py
class NiceGuiNode (line 25) | class NiceGuiNode(Node):
method __init__ (line 27) | def __init__(self) -> None:
method on_rosout (line 106) | def on_rosout(self, msg: Log) -> None:
method on_timer (line 123) | def on_timer(self) -> None:
method on_vis_click (line 176) | def on_vis_click(self, e: events.SceneClickEventArguments):
method on_status (line 185) | def on_status(self, msg, name) -> None:
method on_tab_change (line 240) | def on_tab_change(self, arg):
function main (line 248) | def main() -> None:
function ros_main (line 253) | def ros_main() -> None:
FILE: crazyflie/scripts/simple_mapper_multiranger.py
class SimpleMapperMultiranger (line 31) | class SimpleMapperMultiranger(Node):
method __init__ (line 32) | def __init__(self):
method odom_subscribe_callback (line 66) | def odom_subscribe_callback(self, msg):
method scan_subscribe_callback (line 77) | def scan_subscribe_callback(self, msg):
method rotate_and_create_points (line 111) | def rotate_and_create_points(self):
method rot (line 140) | def rot(self, roll, pitch, yaw, origin, point):
function main (line 170) | def main(args=None):
FILE: crazyflie/scripts/vel_mux.py
class VelMux (line 19) | class VelMux(Node):
method __init__ (line 20) | def __init__(self):
method cmd_vel_callback (line 51) | def cmd_vel_callback(self, msg):
method timer_callback (line 59) | def timer_callback(self):
function main (line 86) | def main(args=None):
FILE: crazyflie/src/crazyflie_server.cpp
function get_service_qos (line 48) | inline auto get_service_qos() { return rmw_qos_profile_services_default; }
function get_service_qos (line 50) | inline auto get_service_qos() { return rclcpp::ServicesQoS(); }
class CrazyflieLogger (line 59) | class CrazyflieLogger : public Logger
method CrazyflieLogger (line 62) | CrazyflieLogger(rclcpp::Logger logger, const std::string& prefix)
method info (line 71) | virtual void info(const std::string &msg)
method warning (line 76) | virtual void warning(const std::string &msg)
method error (line 81) | virtual void error(const std::string &msg)
function extract_names (line 90) | std::set<std::string> extract_names(
class CrazyflieROS (line 108) | class CrazyflieROS
type logPose (line 111) | struct logPose {
type logScan (line 118) | struct logScan {
type logOdom (line 125) | struct logOdom {
type logStatus (line 139) | struct logStatus {
method CrazyflieROS (line 155) | CrazyflieROS(
method spin_once (line 553) | void spin_once()
method broadcastUri (line 559) | std::string broadcastUri() const
method id (line 564) | uint8_t id() const
method change_parameter (line 579) | void change_parameter(const rclcpp::Parameter& p)
method cmd_full_state_changed (line 638) | void cmd_full_state_changed(const crazyflie_interfaces::msg::FullState...
method cmd_position_changed (line 666) | void cmd_position_changed(const crazyflie_interfaces::msg::Position::S...
method cmd_velocity_world_changed (line 674) | void cmd_velocity_world_changed(const crazyflie_interfaces::msg::Veloc...
method cmd_hover_changed (line 682) | void cmd_hover_changed(const crazyflie_interfaces::msg::Hover::SharedP...
method cmd_vel_legacy_changed (line 690) | void cmd_vel_legacy_changed(const geometry_msgs::msg::Twist::SharedPtr...
method on_console (line 700) | void on_console(const char *msg)
method emergency (line 712) | void emergency(const std::shared_ptr<Empty::Request> request,
method start_trajectory (line 719) | void start_trajectory(const std::shared_ptr<StartTrajectory::Request> ...
method takeoff (line 736) | void takeoff(const std::shared_ptr<Takeoff::Request> request,
method land (line 747) | void land(const std::shared_ptr<Land::Request> request,
method go_to (line 758) | void go_to(const std::shared_ptr<GoTo::Request> request,
method upload_trajectory (line 772) | void upload_trajectory(const std::shared_ptr<UploadTrajectory::Request...
method notify_setpoints_stop (line 803) | void notify_setpoints_stop(const std::shared_ptr<NotifySetpointsStop::...
method arm (line 814) | void arm(const std::shared_ptr<Arm::Request> request,
method on_logging_pose (line 824) | void on_logging_pose(uint32_t time_in_ms, const logPose* data) {
method on_logging_scan (line 858) | void on_logging_scan(uint32_t time_in_ms, const logScan* data) {
method on_logging_odom (line 888) | void on_logging_odom(uint32_t time_in_ms, const logOdom* data) {
method on_logging_status (line 916) | void on_logging_status(uint32_t time_in_ms, const logStatus* data) {
method on_logging_custom (line 984) | void on_logging_custom(uint32_t time_in_ms, const std::vector<float>* ...
method on_link_statistics_timer (line 997) | void on_link_statistics_timer()
method on_latency (line 1033) | void on_latency(uint64_t latency_in_us)
class CrazyflieServer (line 1111) | class CrazyflieServer : public rclcpp::Node
method CrazyflieServer (line 1114) | CrazyflieServer()
method emergency (line 1257) | void emergency(const std::shared_ptr<Empty::Request> request,
method start_trajectory (line 1271) | void start_trajectory(const std::shared_ptr<StartTrajectory::Request> ...
method takeoff (line 1291) | void takeoff(const std::shared_ptr<Takeoff::Request> request,
method land (line 1307) | void land(const std::shared_ptr<Land::Request> request,
method go_to (line 1323) | void go_to(const std::shared_ptr<GoTo::Request> request,
method notify_setpoints_stop (line 1341) | void notify_setpoints_stop(const std::shared_ptr<NotifySetpointsStop::...
method arm (line 1357) | void arm(const std::shared_ptr<Arm::Request> request,
method cmd_full_state_changed (line 1372) | void cmd_full_state_changed(const crazyflie_interfaces::msg::FullState...
method posesChanged (line 1404) | void posesChanged(const NamedPoseArray::SharedPtr msg)
method on_parameter_event (line 1448) | void on_parameter_event(const rcl_interfaces::msg::ParameterEvent &event)
method on_watchdog_timer (line 1516) | void on_watchdog_timer()
method broadcast_set_param (line 1575) | void broadcast_set_param(
method update_name_to_id_map (line 1589) | void update_name_to_id_map(const std::string& name, uint8_t id)
function main (line 1650) | int main(int argc, char *argv[])
FILE: crazyflie/src/teleop.cpp
class TeleopNode (line 30) | class TeleopNode : public rclcpp::Node
method TeleopNode (line 33) | TeleopNode()
type Axis (line 117) | struct Axis
method angle_normalize (line 131) | float angle_normalize(float a){
method on_parameter_event (line 151) | void on_parameter_event(const rcl_interfaces::msg::ParameterEvent &event)
method publish (line 164) | void publish()
method joyChanged (line 210) | void joyChanged(const sensor_msgs::msg::Joy::SharedPtr msg)
method getAxis (line 247) | sensor_msgs::msg::Joy::_axes_type::value_type getAxis(const sensor_msg...
method emergency (line 269) | void emergency()
method arm (line 279) | void arm()
method takeoff (line 291) | void takeoff()
method land (line 310) | void land()
method declareAxis (line 337) | void declareAxis(const std::string& name)
method getAxis (line 344) | void getAxis(const std::string& name, Axis& axis)
method on_mode_switched (line 351) | void on_mode_switched()
function main (line 410) | int main(int argc, char *argv[])
FILE: crazyflie_examples/crazyflie_examples/arming.py
function main (line 6) | def main():
FILE: crazyflie_examples/crazyflie_examples/cmd_full_state.py
function executeTrajectory (line 10) | def executeTrajectory(timeHelper, cf, trajpath, rate=100, offset=np.zero...
function main (line 31) | def main():
FILE: crazyflie_examples/crazyflie_examples/figure8.py
function main (line 10) | def main():
FILE: crazyflie_examples/crazyflie_examples/goto_unicast.py
function main (line 7) | def main():
FILE: crazyflie_examples/crazyflie_examples/group_mask.py
function main (line 6) | def main():
FILE: crazyflie_examples/crazyflie_examples/hello_world.py
function main (line 10) | def main():
FILE: crazyflie_examples/crazyflie_examples/infinite_flight.py
function main (line 10) | def main():
FILE: crazyflie_examples/crazyflie_examples/multi_trajectory.py
function main (line 10) | def main():
FILE: crazyflie_examples/crazyflie_examples/nice_hover.py
function main (line 7) | def main():
FILE: crazyflie_examples/crazyflie_examples/set_param.py
function main (line 6) | def main():
FILE: crazyflie_examples/crazyflie_examples/swap.py
function main (line 7) | def main():
FILE: crazyflie_examples/launch/keyboard_velmux_launch.py
function generate_launch_description (line 10) | def generate_launch_description():
FILE: crazyflie_examples/launch/launch.py
function generate_launch_description (line 14) | def generate_launch_description():
FILE: crazyflie_examples/launch/multiranger_mapping_launch.py
function generate_launch_description (line 10) | def generate_launch_description():
FILE: crazyflie_examples/launch/multiranger_nav2_launch.py
function generate_launch_description (line 10) | def generate_launch_description():
FILE: crazyflie_examples/launch/multiranger_simple_mapper_launch.py
function generate_launch_description (line 10) | def generate_launch_description():
FILE: crazyflie_examples/test/test_flake8.py
function test_flake8 (line 21) | def test_flake8():
FILE: crazyflie_examples/test/test_pep257.py
function test_pep257 (line 21) | def test_pep257():
FILE: crazyflie_py/crazyflie_py/crazyflie.py
function arrayToGeometryPoint (line 31) | def arrayToGeometryPoint(a):
class TimeHelper (line 39) | class TimeHelper:
method __init__ (line 55) | def __init__(self, node):
method time (line 62) | def time(self):
method sleep (line 66) | def sleep(self, duration):
method sleepForRate (line 73) | def sleepForRate(self, rateHz):
method isShutdown (line 89) | def isShutdown(self):
class Crazyflie (line 94) | class Crazyflie:
method __init__ (line 101) | def __init__(self, node, cfname, paramTypeDict):
method setGroupMask (line 200) | def setGroupMask(self, groupMask):
method emergency (line 271) | def emergency(self):
method takeoff (line 287) | def takeoff(self, targetHeight, duration, groupMask=0):
method land (line 306) | def land(self, targetHeight, duration, groupMask=0):
method goTo (line 340) | def goTo(self, goal, yaw, duration, relative=False, groupMask=0):
method uploadTrajectory (line 383) | def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
method startTrajectory (line 417) | def startTrajectory(self, trajectoryId,
method notifySetpointsStop (line 446) | def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0):
method arm (line 479) | def arm(self, arm=True):
method getParam (line 512) | def getParam(self, name):
method setParam (line 552) | def setParam(self, name, value):
method cmdFullState (line 590) | def cmdFullState(self, pos, vel, acc, yaw, omega):
method cmdPosition (line 708) | def cmdPosition(self, pos, yaw=0.):
method status_topic_callback (line 754) | def status_topic_callback(self, msg):
method get_status (line 773) | def get_status(self):
method poseStamped_topic_callback (line 784) | def poseStamped_topic_callback(self, msg):
method get_pose (line 805) | def get_pose(self):
method get_position (line 808) | def get_position(self):
class CrazyflieServer (line 812) | class CrazyflieServer(rclpy.node.Node):
method __init__ (line 826) | def __init__(self):
method emergency (line 915) | def emergency(self):
method takeoff (line 931) | def takeoff(self, targetHeight, duration, groupMask=0):
method land (line 952) | def land(self, targetHeight, duration, groupMask=0):
method goTo (line 976) | def goTo(self, goal, yaw, duration, groupMask=0):
method startTrajectory (line 1006) | def startTrajectory(self, trajectoryId,
method arm (line 1034) | def arm(self, arm=True):
method setParam (line 1049) | def setParam(self, name, value):
method cmdFullState (line 1066) | def cmdFullState(self, pos, vel, acc, yaw, omega):
FILE: crazyflie_py/crazyflie_py/crazyswarm_py.py
class Crazyswarm (line 7) | class Crazyswarm:
method __init__ (line 9) | def __init__(self):
FILE: crazyflie_py/crazyflie_py/genericJoystick.py
class Joystick (line 24) | class Joystick:
method __init__ (line 26) | def __init__(self, timeHelper):
method checkIfButtonIsPressed (line 60) | def checkIfButtonIsPressed(self):
method waitUntilButtonPressed (line 67) | def waitUntilButtonPressed(self):
method checkIfAnyButtonIsPressed (line 82) | def checkIfAnyButtonIsPressed(self):
method waitUntilAnyButtonPressed (line 92) | def waitUntilAnyButtonPressed(self):
FILE: crazyflie_py/crazyflie_py/joystick.py
class Joystick (line 7) | class Joystick:
method __init__ (line 9) | def __init__(self):
method joyChanged (line 14) | def joyChanged(self, data):
method waitUntilButtonPressed (line 21) | def waitUntilButtonPressed(self):
FILE: crazyflie_py/crazyflie_py/keyboard.py
class KeyPoller (line 6) | class KeyPoller():
method __enter__ (line 8) | def __enter__(self):
method __exit__ (line 20) | def __exit__(self, type, value, traceback): # noqa A002
method poll (line 23) | def poll(self):
FILE: crazyflie_py/crazyflie_py/linuxjsdev.py
class JEvent (line 70) | class JEvent(object):
method __init__ (line 73) | def __init__(self, evt_type, number, value):
method __repr__ (line 78) | def __repr__(self):
class _JS (line 88) | class _JS():
method __init__ (line 90) | def __init__(self, num, name):
method open (line 101) | def open(self): # noqa: A003
method close (line 125) | def close(self):
method __initvalues (line 135) | def __initvalues(self):
method __updatestate (line 142) | def __updatestate(self, jsdata):
method __decode_event (line 149) | def __decode_event(self, jsdata):
method _read_all_events (line 161) | def _read_all_events(self):
method read (line 184) | def read(self):
class Joystick (line 194) | class Joystick():
method __init__ (line 197) | def __init__(self):
method devices (line 202) | def devices(self):
method open (line 220) | def open(self, device_id): # noqa: A003
method close (line 224) | def close(self, device_id):
method read (line 228) | def read(self, device_id):
FILE: crazyflie_py/crazyflie_py/uav_trajectory.py
function normalize (line 6) | def normalize(v):
class Polynomial (line 12) | class Polynomial:
method __init__ (line 14) | def __init__(self, p):
method eval (line 18) | def eval(self, t): # noqa: A003
method derivative (line 26) | def derivative(self):
class TrajectoryOutput (line 30) | class TrajectoryOutput:
method __init__ (line 32) | def __init__(self):
class Polynomial4D (line 41) | class Polynomial4D:
method __init__ (line 43) | def __init__(self, duration, px, py, pz, pyaw):
method derivative (line 51) | def derivative(self):
method eval (line 59) | def eval(self, t): # noqa: A003
class Trajectory (line 98) | class Trajectory:
method __init__ (line 100) | def __init__(self):
method n_pieces (line 104) | def n_pieces(self):
method loadcsv (line 107) | def loadcsv(self, filename):
method eval (line 113) | def eval(self, t): # noqa: A003
FILE: crazyflie_py/crazyflie_py/util.py
function check_ellipsoid_collisions (line 8) | def check_ellipsoid_collisions(positions, radii):
function poisson_disk_sample (line 31) | def poisson_disk_sample(n, dim, mindist):
FILE: crazyflie_py/test/test_flake8.py
function test_flake8 (line 21) | def test_flake8():
FILE: crazyflie_py/test/test_pep257.py
function test_pep257 (line 21) | def test_pep257():
FILE: crazyflie_sim/crazyflie_sim/backend/dynobench.py
class Backend (line 15) | class Backend:
method __init__ (line 18) | def __init__(self, node: Node, names: list[str], states: list[State]):
method time (line 30) | def time(self) -> float:
method step (line 33) | def step(self, states_desired: list[State], actions: list[Action]) -> ...
method shutdown (line 51) | def shutdown(self):
function rpm_to_force (line 56) | def rpm_to_force(rpm):
function sim_state2dynobench_state (line 64) | def sim_state2dynobench_state(state):
function dynobench_state2sim_state (line 74) | def dynobench_state2sim_state(state):
class Quadrotor (line 83) | class Quadrotor:
method __init__ (line 86) | def __init__(self, state):
method step (line 91) | def step(self, action, dt):
FILE: crazyflie_sim/crazyflie_sim/backend/neuralswarm.py
class phi_Net (line 24) | class phi_Net(nn.Module):
method __init__ (line 26) | def __init__(self, inputdim=6, hiddendim=40):
method forward (line 33) | def forward(self, x):
class rho_Net (line 41) | class rho_Net(nn.Module):
method __init__ (line 43) | def __init__(self, hiddendim=40):
method forward (line 50) | def forward(self, x):
class NeuralSwarm (line 58) | class NeuralSwarm:
method __init__ (line 60) | def __init__(self, model_folder):
method compute_Fa (line 73) | def compute_Fa(self, data_self, data_neighbors):
class Backend (line 103) | class Backend:
method __init__ (line 106) | def __init__(self, node: Node, names: list[str], states: list[State]):
method time (line 119) | def time(self) -> float:
method step (line 122) | def step(self, states_desired: list[State], actions: list[Action]) -> ...
method shutdown (line 150) | def shutdown(self):
FILE: crazyflie_sim/crazyflie_sim/backend/none.py
class Backend (line 10) | class Backend:
method __init__ (line 13) | def __init__(self, node: Node, names: list[str], states: list[State]):
method time (line 20) | def time(self) -> float:
method step (line 23) | def step(self, states_desired: list[State], actions: list[Action]) -> ...
method shutdown (line 35) | def shutdown(self):
FILE: crazyflie_sim/crazyflie_sim/backend/np.py
class Backend (line 12) | class Backend:
method __init__ (line 15) | def __init__(self, node: Node, names: list[str], states: list[State]):
method time (line 27) | def time(self) -> float:
method step (line 30) | def step(self, states_desired: list[State], actions: list[Action]) -> ...
method shutdown (line 48) | def shutdown(self):
class Quadrotor (line 52) | class Quadrotor:
method __init__ (line 55) | def __init__(self, state):
method step (line 84) | def step(self, action, dt, f_a=np.zeros(3)):
FILE: crazyflie_sim/crazyflie_sim/backend/pinocchio.py
class Backend (line 12) | class Backend:
method __init__ (line 15) | def __init__(self, node: Node, names: list[str], states: list[State]):
method time (line 27) | def time(self) -> float:
method step (line 30) | def step(self, states_desired: list[State], actions: list[Action]) -> ...
method shutdown (line 48) | def shutdown(self):
function rpm_to_force (line 53) | def rpm_to_force(rpm):
function pinocchio_state2sim_state (line 61) | def pinocchio_state2sim_state(q, v):
function sim_state2pinocchio_state (line 70) | def sim_state2pinocchio_state(state):
class Quadrotor (line 82) | class Quadrotor:
method __init__ (line 85) | def __init__(self, state):
method step (line 100) | def step(self, action, dt):
FILE: crazyflie_sim/crazyflie_sim/crazyflie_server.py
class CrazyflieServer (line 31) | class CrazyflieServer(Node):
method __init__ (line 33) | def __init__(self):
method on_shutdown_callback (line 215) | def on_shutdown_callback(self):
method _timer_callback (line 223) | def _timer_callback(self):
method _param_to_dict (line 240) | def _param_to_dict(self, param_ros):
method _emergency_callback (line 252) | def _emergency_callback(self, request, response, name='all'):
method _takeoff_callback (line 257) | def _takeoff_callback(self, request, response, name='all'):
method _land_callback (line 272) | def _land_callback(self, request, response, name='all'):
method _go_to_callback (line 287) | def _go_to_callback(self, request, response, name='all'):
method _notify_setpoints_stop_callback (line 316) | def _notify_setpoints_stop_callback(self, request, response, name='all'):
method _upload_trajectory_callback (line 320) | def _upload_trajectory_callback(self, request, response, name='all'):
method _start_trajectory_callback (line 343) | def _start_trajectory_callback(self, request, response, name='all'):
method _cmd_vel_legacy_changed (line 366) | def _cmd_vel_legacy_changed(self, msg, name=''):
method _cmd_hover_changed (line 374) | def _cmd_hover_changed(self, msg, name=''):
method _cmd_full_state_changed (line 382) | def _cmd_full_state_changed(self, msg, name):
function main (line 397) | def main(args=None):
FILE: crazyflie_sim/crazyflie_sim/crazyflie_sil.py
class TrajectoryPolynomialPiece (line 17) | class TrajectoryPolynomialPiece:
method __init__ (line 19) | def __init__(self, poly_x, poly_y, poly_z, poly_yaw, duration):
function copy_svec (line 27) | def copy_svec(v):
class CrazyflieSIL (line 31) | class CrazyflieSIL:
method __init__ (line 40) | def __init__(self, name, initialPosition, controller_name, time_func):
method setGroupMask (line 101) | def setGroupMask(self, groupMask):
method takeoff (line 104) | def takeoff(self, targetHeight, duration, groupMask=0):
method land (line 114) | def land(self, targetHeight, duration, groupMask=0):
method goTo (line 129) | def goTo(self, goal, yaw, duration, relative=False, groupMask=0):
method uploadTrajectory (line 152) | def uploadTrajectory(self,
method startTrajectory (line 172) | def startTrajectory(self,
method cmdFullState (line 192) | def cmdFullState(self, pos, vel, acc, yaw, omega):
method getSetpoint (line 235) | def getSetpoint(self):
method setState (line 279) | def setState(self, state: sim_data_types.State):
method executeController (line 306) | def executeController(self):
method _isGroup (line 329) | def _isGroup(self, groupMask):
method _fwcontrol_to_sim_data_types_action (line 332) | def _fwcontrol_to_sim_data_types_action(self):
method _fwsetpoint_to_sim_data_types_state (line 360) | def _fwsetpoint_to_sim_data_types_state(fwsetpoint):
FILE: crazyflie_sim/crazyflie_sim/sim_data_types.py
class State (line 4) | class State:
method __init__ (line 7) | def __init__(self, pos=np.zeros(3), vel=np.zeros(3),
method pos (line 17) | def pos(self):
method pos (line 22) | def pos(self, value):
method vel (line 26) | def vel(self):
method vel (line 31) | def vel(self, value):
method quat (line 35) | def quat(self):
method quat (line 40) | def quat(self, value):
method omega (line 44) | def omega(self):
method omega (line 49) | def omega(self, value):
method __repr__ (line 52) | def __repr__(self) -> str:
class Action (line 57) | class Action:
method __init__ (line 60) | def __init__(self, rpm):
method rpm (line 66) | def rpm(self):
method rpm (line 71) | def rpm(self, value):
method __repr__ (line 74) | def __repr__(self) -> str:
FILE: crazyflie_sim/crazyflie_sim/visualization/blender.py
function opencv2quat (line 18) | def opencv2quat(rvec):
class Visualization (line 28) | class Visualization:
method __init__ (line 30) | def __init__(self, node: Node, params: dict, names: list[str], states:...
method step (line 167) | def step(self, t, states: list[State], states_desired: list[State], ac...
method shutdown (line 222) | def shutdown(self):
FILE: crazyflie_sim/crazyflie_sim/visualization/pdf.py
class Visualization (line 14) | class Visualization:
method __init__ (line 17) | def __init__(self, node: Node, params: dict, names: list[str], states:...
method step (line 26) | def step(self, t, states: list[State], states_desired: list[State], ac...
method shutdown (line 32) | def shutdown(self):
FILE: crazyflie_sim/crazyflie_sim/visualization/record_states.py
class Visualization (line 12) | class Visualization:
method __init__ (line 15) | def __init__(self, node: Node, params: dict, names: list[str], states:...
method step (line 52) | def step(self, t, states: list[State], states_desired: list[State], ac...
method __log_csv (line 65) | def __log_csv(self, t, idx: int, P: np.ndarray, Q: np.ndarray):
method __log_np (line 70) | def __log_np(self, t, idx: int, P: np.ndarray, Q: np.ndarray):
method __shutdown_np (line 74) | def __shutdown_np(self):
method shutdown (line 84) | def shutdown(self):
FILE: crazyflie_sim/crazyflie_sim/visualization/rviz.py
class Visualization (line 12) | class Visualization:
method __init__ (line 15) | def __init__(
method step (line 28) | def step(self, t, states: list[State], states_desired: list[State], ac...
method shutdown (line 47) | def shutdown(self):
FILE: crazyflie_sim/test/test_flake8.py
function test_flake8 (line 21) | def test_flake8():
FILE: crazyflie_sim/test/test_pep257.py
function test_pep257 (line 21) | def test_pep257():
FILE: docs/generate_install_deps_code.py
function main (line 6) | def main():
FILE: ros_ws/src/crazyswarm/scripts/backgroundComputation.py
function slow (line 41) | def slow(output_queue, seconds):
FILE: ros_ws/src/crazyswarm/scripts/cmdVelocityCircle.py
function goCircle (line 11) | def goCircle(timeHelper, cf, totalTime, radius, kPosition):
FILE: ros_ws/src/crazyswarm/scripts/collisionAvoidance.py
function main (line 20) | def main():
FILE: ros_ws/src/crazyswarm/scripts/collisionAvoidanceHighConflict.py
function positionGoTo (line 23) | def positionGoTo(timeHelper, cfs, goals, kp=1.0, velMax=0.5):
function velocityGoTo (line 42) | def velocityGoTo(timeHelper, cfs, goals, kp=2.0, velMax=0.5):
function main (line 62) | def main():
FILE: ros_ws/src/crazyswarm/scripts/conftest.py
function pytest_runtest_setup (line 9) | def pytest_runtest_setup(item):
FILE: ros_ws/src/crazyswarm/scripts/csv_sequence.py
function main (line 18) | def main():
function poll_trajs (line 187) | def poll_trajs(crazyflies, timeHelper, trajs, timescale):
function poll_planners (line 205) | def poll_planners(crazyflies, timeHelper, planners, duration):
function hover (line 222) | def hover(crazyflies, timeHelper, positions, duration):
function load_all_csvs (line 239) | def load_all_csvs(path):
FILE: ros_ws/src/crazyswarm/scripts/individual_hover.py
function main (line 7) | def main():
FILE: ros_ws/src/crazyswarm/scripts/test_collisionAvoidance.py
function setUp (line 14) | def setUp(args):
function test_velocityMode_sidestepWorstCase (line 29) | def test_velocityMode_sidestepWorstCase(args=None):
function test_goToWithoutCA_CheckCollision (line 53) | def test_goToWithoutCA_CheckCollision():
function test_goToWithCA_CheckCollision (line 71) | def test_goToWithCA_CheckCollision():
function test_goToWithCA_CheckDestination (line 89) | def test_goToWithCA_CheckDestination():
function test_goToWithCA_changeEllipsoid (line 107) | def test_goToWithCA_changeEllipsoid():
function test_goToWithCA_Intersection (line 137) | def test_goToWithCA_Intersection():
function test_goToWithoutCA_Intersection (line 161) | def test_goToWithoutCA_Intersection():
function test_goToWithCA_random (line 183) | def test_goToWithCA_random():
function test_cmdPosition (line 219) | def test_cmdPosition():
function test_boundingBox (line 242) | def test_boundingBox():
function test_maxSpeed_zero (line 269) | def test_maxSpeed_zero():
function test_maxSpeed_limit (line 291) | def test_maxSpeed_limit():
FILE: ros_ws/src/crazyswarm/scripts/test_highLevel.py
function setUp (line 9) | def setUp():
function _collectRelativePositions (line 23) | def _collectRelativePositions(timeHelper, cf, duration):
function test_takeOff (line 32) | def test_takeOff():
function test_goTo_nonRelative (line 41) | def test_goTo_nonRelative():
function test_goTo_relative (line 55) | def test_goTo_relative():
function test_landing (line 67) | def test_landing():
function test_uploadTrajectory_timescale (line 79) | def test_uploadTrajectory_timescale():
function test_uploadTrajectory_fig8Bounds (line 101) | def test_uploadTrajectory_fig8Bounds():
function test_uploadTrajectory_reverse (line 121) | def test_uploadTrajectory_reverse():
function test_uploadTrajectory_broadcast (line 142) | def test_uploadTrajectory_broadcast():
function test_setGroupMask (line 161) | def test_setGroupMask():
FILE: ros_ws/src/crazyswarm/scripts/test_lowLevel.py
function setUp (line 8) | def setUp(extra_args=""):
function test_cmdFullState_zeroVel (line 19) | def test_cmdFullState_zeroVel():
function test_cmdPosition (line 29) | def test_cmdPosition():
function test_cmdVelocityWorld_checkVelocity (line 39) | def test_cmdVelocityWorld_checkVelocity():
function test_cmdVelocityWorld_checkIntegrate (line 49) | def test_cmdVelocityWorld_checkIntegrate():
function test_cmdVelocityWorld_disturbance (line 60) | def test_cmdVelocityWorld_disturbance():
function test_sleepResidual (line 79) | def test_sleepResidual():
FILE: ros_ws/src/crazyswarm/scripts/test_ros.py
function test_ros_import (line 5) | def test_ros_import():
FILE: ros_ws/src/crazyswarm/scripts/test_simOnly.py
function setUp (line 10) | def setUp():
function test_attitudeRPY (line 22) | def test_attitudeRPY():
FILE: ros_ws/src/crazyswarm/scripts/test_videoOutput.py
function videoWriterProcess (line 45) | def videoWriterProcess(path):
function test_videoOutput (line 60) | def test_videoOutput(tmp_path):
FILE: ros_ws/src/crazyswarm/scripts/test_yamlString.py
function test_yaml_string_load (line 6) | def test_yaml_string_load():
FILE: ros_ws/src/crazyswarm/scripts/udp_multicast.py
class UdpMulticastSender (line 7) | class UdpMulticastSender:
method __init__ (line 8) | def __init__(self, MCAST_GRP = '224.1.1.1', MCAST_PORT = 5007):
method send (line 14) | def send(self, msg):
class UdpMulticastReceiver (line 18) | class UdpMulticastReceiver:
method __init__ (line 19) | def __init__(self, MCAST_GRP = '224.1.1.1', MCAST_PORT = 5007):
method recv (line 27) | def recv(self, bufsize=4096):
FILE: ros_ws/src/crazyswarm/scripts/waypoints.py
class Waypoint (line 6) | class Waypoint:
method __init__ (line 7) | def __init__(self, agent, x, y, z, arrival, duration):
method __lt__ (line 15) | def __lt__(self, other):
method __repr__ (line 18) | def __repr__(self):
FILE: ros_ws/src/crazyswarm/scripts/waypoints_simple.py
function main (line 19) | def main():
FILE: systemtests/SDplotting/cfusdlog.py
function _get_name (line 35) | def _get_name(data, idx):
function decode (line 41) | def decode(filename):
FILE: systemtests/SDplotting/data_helper.py
class DataHelper (line 13) | class DataHelper:
method __init__ (line 14) | def __init__(self) -> None:
method generate_data (line 18) | def generate_data(data: dict[str, np.ndarray],
method generate_data_linspace (line 53) | def generate_data_linspace(x: np.ndarray, step: int) -> np.ndarray:
method generate_data_poly (line 57) | def generate_data_poly(x: np.ndarray, y: np.ndarray, x_fit: np.ndarray...
method generate_data_cs (line 67) | def generate_data_cs(x: np.ndarray, y: np.ndarray, x_fit: np.ndarray, ...
method generate_data_bs (line 76) | def generate_data_bs(x: np.ndarray, y: np.ndarray, x_fit: np.ndarray, ...
method generate_data_custom (line 86) | def generate_data_custom(data: dict[str, np.ndarray], target_list: lis...
FILE: systemtests/SDplotting/plot.py
function file_guard (line 20) | def file_guard(pdf_path):
function process_data (line 37) | def process_data(data, settings):
function add_data (line 106) | def add_data(data, settings):
function create_figures (line 120) | def create_figures(data_usd, settings, logfile=None, out=None, ros2_ws=N...
function plot_SD_data (line 299) | def plot_SD_data(logfile=None, output = None, experiment=None, ros2_ws =...
FILE: systemtests/SDplotting/save.py
function write_info (line 9) | def write_info(experiment=None, ros2_ws_path=None):
FILE: systemtests/mcap_handler.py
class McapHandler (line 8) | class McapHandler:
method __init__ (line 9) | def __init__(self):
method read_messages (line 12) | def read_messages(self, input_bag: str):
method write_mcap_to_csv (line 35) | def write_mcap_to_csv(self, inputbag:str, outputfile:str):
FILE: systemtests/plotter_class.py
class Plotter (line 11) | class Plotter:
method __init__ (line 13) | def __init__(self, sim_backend = False):
method file_guard (line 36) | def file_guard(self, pdf_path):
method read_csv_and_set_arrays (line 53) | def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile):
method no_match_warning (line 126) | def no_match_warning(self, unmatched_values:list):
method adjust_arrays (line 152) | def adjust_arrays(self):
method create_figures (line 185) | def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfnam...
method test_passed (line 333) | def test_passed(self) -> tuple :
FILE: systemtests/test_flights.py
function setUpModule (line 16) | def setUpModule():
function tearDownModule (line 26) | def tearDownModule():
function clean_process (line 30) | def clean_process(process:Popen) -> int :
function print_PIPE (line 49) | def print_PIPE(process : Popen, process_name, always=False):
class TestFlights (line 62) | class TestFlights(unittest.TestCase):
method __init__ (line 65) | def __init__(self, methodName: str = "runTest") -> None:
method idFolderName (line 72) | def idFolderName(self):
method setUp (line 76) | def setUp(self):
method tearDown (line 93) | def tearDown(self) -> None:
method record_start_and_clean (line 137) | def record_start_and_clean(self, testname:str, max_wait:int):
method translate_plot_and_check (line 178) | def translate_plot_and_check(self, testname:str) -> bool :
method test_figure8 (line 197) | def test_figure8(self):
Condensed preview — 360 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,850K chars).
[
{
"path": ".github/workflows/ci-docs2.yml",
"chars": 957,
"preview": "name: Docs\n\non:\n push:\n branches: [ main ]\n pull_request:\n branches: [ main ]\n\njobs:\n build:\n runs-on: ubunt"
},
{
"path": ".github/workflows/ci-ros2-mac.yml",
"chars": 1324,
"preview": "name: ROS2 (Mac)\n\n# Disable for now\non:\n push:\n branches-ignore:\n - '**'\n# on:\n# push:\n# branches: [ main"
},
{
"path": ".github/workflows/ci-ros2-win.yml",
"chars": 1345,
"preview": "name: ROS2 (Windows)\n\n# Disable for now\non:\n push:\n branches-ignore:\n - '**'\n# on:\n# push:\n# branches: [ "
},
{
"path": ".github/workflows/ci-ros2.yml",
"chars": 2250,
"preview": "name: ROS2 (Ubuntu)\n\non:\n push:\n branches: [ main ]\n pull_request:\n branches: [ main ]\n\n# Based on example provi"
},
{
"path": ".github/workflows/systemtests.yml",
"chars": 1788,
"preview": "name: System Tests\n\non:\n push:\n branches: [ \"feature-systemtests-better\" ]\n # manual trigger\n workflow_dispatch:\n\n"
},
{
"path": ".github/workflows/systemtests_sim.yml",
"chars": 2215,
"preview": "name: System Tests Simulation \n\non:\n push:\n branches: [ main ]\n pull_request:\n branches: [ main ]\n # manual tri"
},
{
"path": ".gitignore",
"chars": 47,
"preview": ".DS_Store\ncrazyswarm2_sim_webots/\n*.pyc\n*.o\n*~\n"
},
{
"path": ".gitmodules",
"chars": 138,
"preview": "[submodule \"crazyflie/deps/crazyflie_tools\"]\n\tpath = crazyflie/deps/crazyflie_tools\n\turl = https://github.com/whoenig/cr"
},
{
"path": "LICENSE",
"chars": 1075,
"preview": "The MIT License (MIT)\n\nCopyright (c) 2014 whoenig\n\nPermission is hereby granted, free of charge, to any person obtaining"
},
{
"path": "README.md",
"chars": 1389,
"preview": "* Local CI: [](https://github.co"
},
{
"path": "crazyflie/CHANGELOG.rst",
"chars": 644,
"preview": "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\nChangelog for package crazyflie\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n\n1.0.3 (2025-07-09)\n----"
},
{
"path": "crazyflie/CMakeLists.txt",
"chars": 3793,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(crazyflie)\n\n#if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES \"C"
},
{
"path": "crazyflie/config/aideck_streamer.yaml",
"chars": 762,
"preview": "image_topic: /camera/image\ncamera_info_topic: /camera/camera_info\ndeck_ip: \"192.168.4.1\"\ndeck_port: 5000\nimage_width: 32"
},
{
"path": "crazyflie/config/config.rviz",
"chars": 6009,
"preview": "Panels:\n - Class: rviz_common/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:"
},
{
"path": "crazyflie/config/crazyflies.yaml",
"chars": 3862,
"preview": "# named list of all robots\nfileversion: 3\n\nrobots:\n cf231:\n enabled: true\n uri: radio://0/80/2M/E7E7E7E7E7\n in"
},
{
"path": "crazyflie/config/motion_capture.yaml",
"chars": 1976,
"preview": "/motion_capture_tracking:\n ros__parameters:\n # one of \"optitrack\", \"optitrack_closed_source\", \"vicon\", \"qualisys\", \""
},
{
"path": "crazyflie/config/server.yaml",
"chars": 1876,
"preview": "/crazyflie_server:\n ros__parameters:\n warnings:\n frequency: 1.0 # report/run checks once per second\n motio"
},
{
"path": "crazyflie/config/teleop.yaml",
"chars": 1281,
"preview": "/teleop:\n ros__parameters:\n frequency: 100 # set to 0, to disable manual flight modes\n mode: \"high_level\" # one "
},
{
"path": "crazyflie/launch/launch.py",
"chars": 7246,
"preview": "import os\nimport yaml\nfrom ament_index_python.packages import get_package_share_directory\nfrom launch import LaunchDescr"
},
{
"path": "crazyflie/launch/launch_teleop2.py",
"chars": 3496,
"preview": "import os\nimport yaml\nfrom ament_index_python.packages import get_package_share_directory\nfrom launch import LaunchDescr"
},
{
"path": "crazyflie/launch/teleop_launch.py",
"chars": 515,
"preview": "from launch import LaunchDescription\nfrom launch_ros.actions import Node\n\ndef generate_launch_description():\n return "
},
{
"path": "crazyflie/package.xml",
"chars": 1181,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "crazyflie/scripts/aideck_streamer.py",
"chars": 6077,
"preview": "#!/usr/bin/env python3\nimport socket,os,struct, time\nimport numpy as np\nimport cv2\nimport yaml\nfrom ament_index_python.p"
},
{
"path": "crazyflie/scripts/cfmult.py",
"chars": 3350,
"preview": "#!/usr/bin/env python3\n\n\"\"\"\nHelper script to do something to multiple crazyflies on the same channel \nsimultaneously. Sp"
},
{
"path": "crazyflie/scripts/chooser.py",
"chars": 9062,
"preview": "#!/usr/bin/env python3\n\nimport argparse\ntry:\n\timport Tkinter\nexcept ImportError:\n\timport tkinter as Tkinter\nfrom ruamel."
},
{
"path": "crazyflie/scripts/crazyflie_server.py",
"chars": 55854,
"preview": "#!/usr/bin/env python3\n\n\"\"\"\nA crazyflie server for communicating with several crazyflies\n based on the official crazy"
},
{
"path": "crazyflie/scripts/flash.py",
"chars": 1827,
"preview": "#!/usr/bin/env python3\n\n\"\"\"\nA flash utility script to flash crazyflies with the latest or custom firmware\n\n 2024 - K."
},
{
"path": "crazyflie/scripts/gui.py",
"chars": 11536,
"preview": "#!/usr/bin/env python3\n\nimport threading\nimport time\nfrom pathlib import Path\nfrom functools import partial\n\nimport rclp"
},
{
"path": "crazyflie/scripts/simple_mapper_multiranger.py",
"chars": 6418,
"preview": "#!/usr/bin/env python3\n\n\"\"\" This simple mapper is loosely based on both the bitcraze cflib point cloud example \n https:/"
},
{
"path": "crazyflie/scripts/vel_mux.py",
"chars": 3887,
"preview": "#!/usr/bin/env python3\n\n\"\"\"\nA Twist message handler that get incoming twist messages from \n external packages and han"
},
{
"path": "crazyflie/src/crazyflie_server.cpp",
"chars": 67365,
"preview": "#include <memory>\n#include <vector>\n#include <regex>\n\n#include <crazyflie_cpp/Crazyflie.h>\n\n#include <rclcpp/rclcpp.hpp>"
},
{
"path": "crazyflie/src/teleop.cpp",
"chars": 14489,
"preview": "#include <memory>\n#include <vector>\n#include <chrono>\n#include <math.h>\n#include \"rclcpp/rclcpp.hpp\"\n#include \"sensor_ms"
},
{
"path": "crazyflie/urdf/cf2_assembly_with_props.dae",
"chars": 218640,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<!--\n ........... ____ _ __\n | ,-^-, | / __ )(_) /_______________ _"
},
{
"path": "crazyflie/urdf/crazyflie_description.urdf",
"chars": 292,
"preview": "<?xml version=\"1.0\"?>\n<robot name=\"$NAME\">\n <link name=\"$NAME\">\n <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\" />\n <visual>\n "
},
{
"path": "crazyflie_examples/CHANGELOG.rst",
"chars": 511,
"preview": "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\nChangelog for package crazyflie_examples\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^"
},
{
"path": "crazyflie_examples/CMakeLists.txt",
"chars": 844,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(crazyflie_examples)\n\n# find dependencies\nfind_package(ament_cmake REQUIRED)\n"
},
{
"path": "crazyflie_examples/config/nav2_params.yaml",
"chars": 6838,
"preview": "controller_server:\n ros__parameters:\n use_sim_time: True\n controller_frequency: 20.0\n min_x_velocity_threshold"
},
{
"path": "crazyflie_examples/config/nav2_view.rviz",
"chars": 18370,
"preview": "Panels:\n - Class: rviz_common/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n"
},
{
"path": "crazyflie_examples/config/slam_params.yaml",
"chars": 2477,
"preview": "slam_toolbox:\n ros__parameters:\n\n # Plugin params\n solver_plugin: solver_plugins::CeresSolver\n ceres_linear_so"
},
{
"path": "crazyflie_examples/crazyflie_examples/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "crazyflie_examples/crazyflie_examples/arming.py",
"chars": 407,
"preview": "#!/usr/bin/env python\n\nfrom crazyflie_py import Crazyswarm\n\n\ndef main():\n swarm = Crazyswarm()\n timeHelper = swarm"
},
{
"path": "crazyflie_examples/crazyflie_examples/cmd_full_state.py",
"chars": 1224,
"preview": "#!/usr/bin/env python\n\nfrom pathlib import Path\n\nfrom crazyflie_py import Crazyswarm\nfrom crazyflie_py.uav_trajectory im"
},
{
"path": "crazyflie_examples/crazyflie_examples/data/figure8.csv",
"chars": 3215,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv",
"chars": 8453,
"preview": "Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj1.csv",
"chars": 4247,
"preview": "Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "crazyflie_examples/crazyflie_examples/figure8.py",
"chars": 1237,
"preview": "#!/usr/bin/env python\n\nfrom pathlib import Path\n\nfrom crazyflie_py import Crazyswarm\nfrom crazyflie_py.uav_trajectory im"
},
{
"path": "crazyflie_examples/crazyflie_examples/goto_unicast.py",
"chars": 826,
"preview": "#!/usr/bin/env python\n\nfrom crazyflie_py import Crazyswarm\nimport numpy as np\n\n\ndef main():\n Z = 0.5\n\n swarm = Cra"
},
{
"path": "crazyflie_examples/crazyflie_examples/group_mask.py",
"chars": 707,
"preview": "#!/usr/bin/env python\n\nfrom crazyflie_py import Crazyswarm\n\n\ndef main():\n swarm = Crazyswarm()\n timeHelper = swarm"
},
{
"path": "crazyflie_examples/crazyflie_examples/hello_world.py",
"chars": 506,
"preview": "\"\"\"Takeoff-hover-land for one CF. Useful to validate hardware config.\"\"\"\n\nfrom crazyflie_py import Crazyswarm\n\n\nTAKEOFF_"
},
{
"path": "crazyflie_examples/crazyflie_examples/infinite_flight.py",
"chars": 3244,
"preview": "#!/usr/bin/env python\n\nfrom pathlib import Path\n\nfrom crazyflie_py import Crazyswarm\nfrom crazyflie_py.uav_trajectory im"
},
{
"path": "crazyflie_examples/crazyflie_examples/multi_trajectory.py",
"chars": 1295,
"preview": "#!/usr/bin/env python\n\nfrom pathlib import Path\n\nfrom crazyflie_py import Crazyswarm\nfrom crazyflie_py.uav_trajectory im"
},
{
"path": "crazyflie_examples/crazyflie_examples/nice_hover.py",
"chars": 600,
"preview": "#!/usr/bin/env python\n\nfrom crazyflie_py import Crazyswarm\nimport numpy as np\n\n\ndef main():\n Z = 1.0\n\n swarm = Cra"
},
{
"path": "crazyflie_examples/crazyflie_examples/set_param.py",
"chars": 742,
"preview": "#!/usr/bin/env python\n\nfrom crazyflie_py import Crazyswarm\n\n\ndef main():\n # In case of key errors, wait for all the c"
},
{
"path": "crazyflie_examples/crazyflie_examples/swap.py",
"chars": 1177,
"preview": "#!/usr/bin/env python\n\nfrom crazyflie_py import Crazyswarm\nimport numpy as np\n\n\ndef main():\n Id2 = 231\n Id1 = 5\n "
},
{
"path": "crazyflie_examples/data/map.yaml",
"chars": 120,
"preview": "image: map.pgm\nmode: trinary\nresolution: 0.1\norigin: [-1.99, -1.77, 0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.25"
},
{
"path": "crazyflie_examples/launch/keyboard_velmux_launch.py",
"chars": 1048,
"preview": "import os\n\nfrom ament_index_python.packages import get_package_share_directory\nfrom launch import LaunchDescription\nfrom"
},
{
"path": "crazyflie_examples/launch/launch.py",
"chars": 1337,
"preview": "import os\n\nfrom ament_index_python.packages import get_package_share_directory\n\n\nfrom launch import LaunchDescription\nfr"
},
{
"path": "crazyflie_examples/launch/multiranger_mapping_launch.py",
"chars": 1918,
"preview": "import os\n\nfrom ament_index_python.packages import get_package_share_directory\nfrom launch import LaunchDescription\nfrom"
},
{
"path": "crazyflie_examples/launch/multiranger_nav2_launch.py",
"chars": 3469,
"preview": "import os\n\nfrom ament_index_python.packages import get_package_share_directory\nfrom launch import LaunchDescription\nfrom"
},
{
"path": "crazyflie_examples/launch/multiranger_simple_mapper_launch.py",
"chars": 1488,
"preview": "import os\n\nfrom ament_index_python.packages import get_package_share_directory\nfrom launch import LaunchDescription\nfrom"
},
{
"path": "crazyflie_examples/package.xml",
"chars": 932,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "crazyflie_examples/resource/crazyflie_examples",
"chars": 0,
"preview": ""
},
{
"path": "crazyflie_examples/setup.cfg",
"chars": 621,
"preview": "[options.entry_points]\nconsole_scripts =\n hello_world = crazyflie_examples.hello_world:main\n nice_hover = crazyfli"
},
{
"path": "crazyflie_examples/test/test_flake8.py",
"chars": 884,
"preview": "# Copyright 2017 Open Source Robotics Foundation, Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\""
},
{
"path": "crazyflie_examples/test/test_pep257.py",
"chars": 803,
"preview": "# Copyright 2015 Open Source Robotics Foundation, Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\""
},
{
"path": "crazyflie_interfaces/CHANGELOG.rst",
"chars": 731,
"preview": "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\nChangelog for package crazyflie_interfaces\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^"
},
{
"path": "crazyflie_interfaces/CMakeLists.txt",
"chars": 1223,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(crazyflie_interfaces)\n\n# Default to C++14\nif(NOT CMAKE_CXX_STANDARD)\n set(C"
},
{
"path": "crazyflie_interfaces/msg/ConnectionStatistics.msg",
"chars": 112,
"preview": "string uri\nuint64 sent_count\nuint64 sent_ping_count\nuint64 receive_count\nuint64 enqueued_count\nuint64 ack_count\n"
},
{
"path": "crazyflie_interfaces/msg/ConnectionStatisticsArray.msg",
"chars": 52,
"preview": "std_msgs/Header header\nConnectionStatistics[] stats\n"
},
{
"path": "crazyflie_interfaces/msg/FullState.msg",
"chars": 99,
"preview": "std_msgs/Header header\ngeometry_msgs/Pose pose\ngeometry_msgs/Twist twist\ngeometry_msgs/Vector3 acc\n"
},
{
"path": "crazyflie_interfaces/msg/Hover.msg",
"chars": 81,
"preview": "std_msgs/Header header\nfloat32 vx\nfloat32 vy\nfloat32 yaw_rate\nfloat32 z_distance\n"
},
{
"path": "crazyflie_interfaces/msg/LogBlock.msg",
"chars": 53,
"preview": "string topic_name\nint16 frequency\nstring[] variables\n"
},
{
"path": "crazyflie_interfaces/msg/LogDataGeneric.msg",
"chars": 250,
"preview": "std_msgs/Header header # Header including the ROS 2 timestamp when the log data was received\nuint32 timestamp # "
},
{
"path": "crazyflie_interfaces/msg/Position.msg",
"chars": 65,
"preview": "std_msgs/Header header\nfloat32 x\nfloat32 y\nfloat32 z\nfloat32 yaw\n"
},
{
"path": "crazyflie_interfaces/msg/Status.msg",
"chars": 1166,
"preview": "\n# Constants\nuint16 SUPERVISOR_INFO_CAN_BE_ARMED = 1\nuint16 SUPERVISOR_INFO_IS_ARMED = 2\nuint16 SUPERVISOR_INFO_AUTO"
},
{
"path": "crazyflie_interfaces/msg/TrajectoryPolynomialPiece.msg",
"chars": 109,
"preview": "#\n\nfloat32[] poly_x\nfloat32[] poly_y\nfloat32[] poly_z\nfloat32[] poly_yaw\nbuiltin_interfaces/Duration duration"
},
{
"path": "crazyflie_interfaces/msg/VelocityWorld.msg",
"chars": 65,
"preview": "std_msgs/Header header\ngeometry_msgs/Vector3 vel\nfloat32 yaw_rate"
},
{
"path": "crazyflie_interfaces/package.xml",
"chars": 1254,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "crazyflie_interfaces/srv/AddLogging.srv",
"chars": 65,
"preview": "string topic_name\nint32 frequency\nstring[] vars\n---\nbool success\n"
},
{
"path": "crazyflie_interfaces/srv/Arm.srv",
"chars": 13,
"preview": "bool arm\n---\n"
},
{
"path": "crazyflie_interfaces/srv/GoTo.srv",
"chars": 115,
"preview": "uint8 group_mask\nbool relative\ngeometry_msgs/Point goal\nfloat32 yaw # deg\nbuiltin_interfaces/Duration duration\n---\n"
},
{
"path": "crazyflie_interfaces/srv/Land.srv",
"chars": 73,
"preview": "uint8 group_mask\nfloat32 height\nbuiltin_interfaces/Duration duration\n---\n"
},
{
"path": "crazyflie_interfaces/srv/NotifySetpointsStop.srv",
"chars": 51,
"preview": "uint8 group_mask\nuint32 remain_valid_millisecs\n---\n"
},
{
"path": "crazyflie_interfaces/srv/RemoveLogging.srv",
"chars": 35,
"preview": "string topic_name\n---\nbool success\n"
},
{
"path": "crazyflie_interfaces/srv/SetGroupMask.srv",
"chars": 21,
"preview": "uint8 group_mask\n---\n"
},
{
"path": "crazyflie_interfaces/srv/StartTrajectory.srv",
"chars": 87,
"preview": "uint8 group_mask\nuint8 trajectory_id\nfloat32 timescale\nbool reversed\nbool relative\n---\n"
},
{
"path": "crazyflie_interfaces/srv/Stop.srv",
"chars": 21,
"preview": "uint8 group_mask\n---\n"
},
{
"path": "crazyflie_interfaces/srv/Takeoff.srv",
"chars": 73,
"preview": "uint8 group_mask\nfloat32 height\nbuiltin_interfaces/Duration duration\n---\n"
},
{
"path": "crazyflie_interfaces/srv/UpdateParams.srv",
"chars": 20,
"preview": "string[] params\n---\n"
},
{
"path": "crazyflie_interfaces/srv/UploadTrajectory.srv",
"chars": 79,
"preview": "uint8 trajectory_id\nuint32 piece_offset\nTrajectoryPolynomialPiece[] pieces\n---\n"
},
{
"path": "crazyflie_py/CHANGELOG.rst",
"chars": 473,
"preview": "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\nChangelog for package crazyflie_py\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n\n1.0.3 (2025-07"
},
{
"path": "crazyflie_py/crazyflie_py/__init__.py",
"chars": 64,
"preview": "from .crazyswarm_py import Crazyswarm\n\n__all__ = ['Crazyswarm']\n"
},
{
"path": "crazyflie_py/crazyflie_py/crazyflie.py",
"chars": 47797,
"preview": "#!/usr/bin/env python\n\nfrom collections import defaultdict\n\n# import sys\n# import rospy\n# import time\n# import tf_conver"
},
{
"path": "crazyflie_py/crazyflie_py/crazyswarm_py.py",
"chars": 316,
"preview": "import rclpy\n\nfrom . import genericJoystick\nfrom .crazyflie import CrazyflieServer, TimeHelper\n\n\nclass Crazyswarm:\n\n "
},
{
"path": "crazyflie_py/crazyflie_py/genericJoystick.py",
"chars": 3184,
"preview": "import copy\n# import pyglet\n\nfrom . import keyboard\n\n# class JoyStickHandler:\n# def __init__(self):\n# self.b"
},
{
"path": "crazyflie_py/crazyflie_py/joystick.py",
"chars": 650,
"preview": "import time\n\nimport rospy\nfrom sensor_msgs.msg import Joy\n\n\nclass Joystick:\n\n def __init__(self):\n self.lastBu"
},
{
"path": "crazyflie_py/crazyflie_py/keyboard.py",
"chars": 769,
"preview": "import select\nimport sys\nimport termios\n\n\nclass KeyPoller():\n\n def __enter__(self):\n # Save the terminal setti"
},
{
"path": "crazyflie_py/crazyflie_py/linuxjsdev.py",
"chars": 7397,
"preview": "# -*- coding: utf-8 -*-\n# ||\n# +------+ / __ )(_) /_______________ _____ ___\n# | 0xBC | / __ / / __/ __"
},
{
"path": "crazyflie_py/crazyflie_py/uav_trajectory.py",
"chars": 3664,
"preview": "#!/usr/bin/env python\n\nimport numpy as np\n\n\ndef normalize(v):\n norm = np.linalg.norm(v)\n assert norm > 0\n retur"
},
{
"path": "crazyflie_py/crazyflie_py/util.py",
"chars": 1981,
"preview": "\"\"\"Useful functions for both pycrazyswarm internals and user scripts.\"\"\"\n\nimport numpy as np\nimport scipy as sp\nimport s"
},
{
"path": "crazyflie_py/package.xml",
"chars": 781,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "crazyflie_py/resource/crazyflie_py",
"chars": 0,
"preview": ""
},
{
"path": "crazyflie_py/setup.cfg",
"chars": 93,
"preview": "[develop]\nscript_dir=$base/lib/crazyflie_py\n[install]\ninstall_scripts=$base/lib/crazyflie_py\n"
},
{
"path": "crazyflie_py/setup.py",
"chars": 672,
"preview": "from setuptools import setup\n\npackage_name = 'crazyflie_py'\n\nsetup(\n name=package_name,\n version='1.0.3',\n pack"
},
{
"path": "crazyflie_py/test/test_flake8.py",
"chars": 884,
"preview": "# Copyright 2017 Open Source Robotics Foundation, Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\""
},
{
"path": "crazyflie_py/test/test_pep257.py",
"chars": 843,
"preview": "# Copyright 2015 Open Source Robotics Foundation, Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\""
},
{
"path": "crazyflie_sim/CHANGELOG.rst",
"chars": 507,
"preview": "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\nChangelog for package crazyflie_sim\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n\n1.0.3 (2025"
},
{
"path": "crazyflie_sim/CMakeLists.txt",
"chars": 853,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(crazyflie_sim)\n\n# find dependencies\nfind_package(ament_cmake REQUIRED)\nfind_"
},
{
"path": "crazyflie_sim/crazyflie_sim/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "crazyflie_sim/crazyflie_sim/backend/data/dynobench/crazyflie2.yaml",
"chars": 455,
"preview": "# distance_weights: [1. , .5, .2, .2 ]\ndistance_weights: [1., .5, .1, .05]\nmax_vel: 4\nmax_angular_vel: 8\nmax_acc: 25 # u"
},
{
"path": "crazyflie_sim/crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf",
"chars": 781,
"preview": "<?xml version=\"1.0\"?>\n<robot name=\"crazflie\">\n \n <link name=\"world\"></link>\n \n <joint name=\"freefly\" type=\"f"
},
{
"path": "crazyflie_sim/crazyflie_sim/backend/dynobench.py",
"chars": 3069,
"preview": "from pathlib import Path\n\nimport numpy as np\nfrom rclpy.node import Node\nfrom rclpy.time import Time\nimport robot_python"
},
{
"path": "crazyflie_sim/crazyflie_sim/backend/neuralswarm.py",
"chars": 5588,
"preview": "\"\"\"\nThis implementes interaction force prediction using NeuralSwarm(2).\n\nSee https://github.com/aerorobotics/neural-swar"
},
{
"path": "crazyflie_sim/crazyflie_sim/backend/none.py",
"chars": 1015,
"preview": "from __future__ import annotations\n\nfrom rclpy.node import Node\nfrom rclpy.time import Time\nfrom rosgraph_msgs.msg impor"
},
{
"path": "crazyflie_sim/crazyflie_sim/backend/np.py",
"chars": 4245,
"preview": "from __future__ import annotations\n\nimport numpy as np\nfrom rclpy.node import Node\nfrom rclpy.time import Time\nfrom rosg"
},
{
"path": "crazyflie_sim/crazyflie_sim/backend/pinocchio.py",
"chars": 3606,
"preview": "from pathlib import Path\n\nimport numpy as np\nimport pinocchio as pin\nfrom rclpy.node import Node\nfrom rclpy.time import "
},
{
"path": "crazyflie_sim/crazyflie_sim/crazyflie_server.py",
"chars": 15088,
"preview": "#!/usr/bin/env python3\n\n\"\"\"\nA crazyflie server for simulation.\n\n 2022 - Wolfgang Hönig (TU Berlin)\n 2025 - Updated"
},
{
"path": "crazyflie_sim/crazyflie_sim/crazyflie_sil.py",
"chars": 15480,
"preview": "#!/usr/bin/env python3\n\n\"\"\"\nCrazyflie Software-In-The-Loop Wrapper that uses the firmware Python bindings.\n\n 2022 - W"
},
{
"path": "crazyflie_sim/crazyflie_sim/sim_data_types.py",
"chars": 1828,
"preview": "import numpy as np\n\n\nclass State:\n \"\"\"Class that stores the state of a UAV as used in the simulator interface.\"\"\"\n\n "
},
{
"path": "crazyflie_sim/crazyflie_sim/visualization/README.md",
"chars": 3522,
"preview": "# Visualizations\n\n## Blender\n\n### Dependencies\n\n- `bpy`\n- `rowan`\n- `numpy` \n- `yaml` \n\n### Output\n- Takes pictures from"
},
{
"path": "crazyflie_sim/crazyflie_sim/visualization/blender.py",
"chars": 9590,
"preview": "import datetime\nimport os\nfrom pathlib import Path\n\nimport bpy\nimport numpy as np\nfrom rclpy.node import Node\nimport row"
},
{
"path": "crazyflie_sim/crazyflie_sim/visualization/data/README.md",
"chars": 333,
"preview": "# Acknowledgments \n\n- All background images are in the public domain (CC0 license) and were sourced from [polyhaven.com]"
},
{
"path": "crazyflie_sim/crazyflie_sim/visualization/data/model/cf.mtl",
"chars": 777,
"preview": "# Blender 3.4.1 MTL File: 'None'\n# www.blender.org\n\nnewmtl Material.001\nNs 250.000000\nKa 0.652850 0.652850 0.652850\nKd 0"
},
{
"path": "crazyflie_sim/crazyflie_sim/visualization/data/model/cf.obj",
"chars": 275663,
"preview": "# Blender 3.4.1\n# www.blender.org\nmtllib cf.mtl\ng cf_Mesh\nv 0.031894 0.032030 0.007876\nv 0.031894 0.032030 0.011571\nv 0."
},
{
"path": "crazyflie_sim/crazyflie_sim/visualization/pdf.py",
"chars": 4821,
"preview": "from __future__ import annotations\n\nimport copy\n\nfrom matplotlib.backends.backend_pdf import PdfPages\nimport matplotlib."
},
{
"path": "crazyflie_sim/crazyflie_sim/visualization/record_states.py",
"chars": 3482,
"preview": "from __future__ import annotations\n\nimport datetime\nimport os\n\nimport numpy as np\nfrom rclpy.node import Node\n\nfrom ..si"
},
{
"path": "crazyflie_sim/crazyflie_sim/visualization/rviz.py",
"chars": 1645,
"preview": "from __future__ import annotations\n\nimport math\n\nfrom geometry_msgs.msg import TransformStamped\nfrom rclpy.node import N"
},
{
"path": "crazyflie_sim/package.xml",
"chars": 939,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "crazyflie_sim/resource/crazyflie_sim",
"chars": 0,
"preview": ""
},
{
"path": "crazyflie_sim/setup.cfg",
"chars": 99,
"preview": "[options.entry_points]\nconsole_scripts =\n crazyflie_server = crazyflie_sim.crazyflie_server:main"
},
{
"path": "crazyflie_sim/test/test_flake8.py",
"chars": 884,
"preview": "# Copyright 2017 Open Source Robotics Foundation, Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\""
},
{
"path": "crazyflie_sim/test/test_pep257.py",
"chars": 803,
"preview": "# Copyright 2015 Open Source Robotics Foundation, Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\""
},
{
"path": "docs/.gitignore",
"chars": 12,
"preview": "_build\nhtml\n"
},
{
"path": "docs/Makefile",
"chars": 7625,
"preview": "# Makefile for Sphinx documentation\n#\n\n# You can set these variables from the command line.\nSPHINXOPTS = -W\nSPHINXBUI"
},
{
"path": "docs/api.rst",
"chars": 1954,
"preview": ".. _api:\n\nPython API Reference\n====================\n\nThe module ``pycrazyswarm``, contained in ``/ros_ws/src/crazyswarm/"
},
{
"path": "docs/changelog.rst",
"chars": 2756,
"preview": "Changelog\n=========\n\nOctober 4th, 2019\n-----------------\n\n1. We switched to the official firmware. Our own EKF is not us"
},
{
"path": "docs/conf.py",
"chars": 10590,
"preview": "#!/usr/bin/env python3\n# -*- coding: utf-8 -*-\n#\n# Crazyswarm documentation build configuration file, created by\n# sphin"
},
{
"path": "docs/configuration.rst",
"chars": 18613,
"preview": ".. _configuration:\n\nConfiguration\n=============\n\nAfter completing :ref:`installation`,\na significant amount of configura"
},
{
"path": "docs/generate_install_deps_code.py",
"chars": 562,
"preview": "import sys\n\nimport yaml\n\n\ndef main():\n workflow_path = sys.argv[1]\n with open(workflow_path) as f:\n workflo"
},
{
"path": "docs/generated/.gitignore",
"chars": 19,
"preview": "*.rst\n*.rstinclude\n"
},
{
"path": "docs/gettingstarted.rst",
"chars": 984,
"preview": "Getting Started\n===============\n\n#. Read the Crazyswarm paper to understand the different components of the system. A pr"
},
{
"path": "docs/glossary.rst",
"chars": 1757,
"preview": "Glossary\n========\n\n.. glossary::\n\n Piecewise Polynomial\n A piecewise function is function of the form\n\n .. math::"
},
{
"path": "docs/hardware.rst",
"chars": 5158,
"preview": "Hardware\n========\n\nIf you are planning to built a Crazyswarm yourself, below is the list of components we use.\n\nComponen"
},
{
"path": "docs/howto/git_integration.rstinclude",
"chars": 1943,
"preview": ".. _tutorial_git_integration:\n\nCrazyswarm Integration with Git\n-------------------------------\n\nIn this tutorial we disc"
},
{
"path": "docs/howto/howto.rst",
"chars": 113,
"preview": "How-To Guides\n=============\n\n.. include:: git_integration.rstinclude\n\n.. include:: streaming_setpoint.rstinclude\n"
},
{
"path": "docs/howto/streaming_setpoint.rstinclude",
"chars": 22309,
"preview": ".. _tutorial_streaming_setpoint:\n\nCreating a new streaming setpoint mode\n--------------------------------------\n\nIn this"
},
{
"path": "docs/index.rst",
"chars": 4696,
"preview": ".. _introduction:\n\nWelcome to Crazyswarm's documentation!\n======================================\n\nThe Crazyswarm platfor"
},
{
"path": "docs/installation.rst",
"chars": 4367,
"preview": ".. _installation:\n\nInstallation\n============\n\nCrazyswarm runs on **Ubuntu Linux** in one of the following configurations"
},
{
"path": "docs/internals.rst",
"chars": 3506,
"preview": "Crazyswarm Internals\n====================\n\nThis page contains information for developers interested in modifying the Cra"
},
{
"path": "docs/requirements.txt",
"chars": 93,
"preview": "numpy\npyyaml\nsphinx\nsphinx-argparse\nsphinx-rtd-theme\nsphinx-tabs\nsphinxcontrib-programoutput\n"
},
{
"path": "docs/tutorials/hover.rstinclude",
"chars": 1561,
"preview": ".. _tutorial_hover:\n\nHovering (hello, world)\n------------------------\n\nAfter completing :ref:`Installation` and :ref:`Co"
},
{
"path": "docs/tutorials/tutorials.rst",
"chars": 51,
"preview": "Tutorials\n=========\n\n.. include:: hover.rstinclude\n"
},
{
"path": "docs2/.gitignore",
"chars": 7,
"preview": "_build\n"
},
{
"path": "docs2/Makefile",
"chars": 7625,
"preview": "# Makefile for Sphinx documentation\n#\n\n# You can set these variables from the command line.\nSPHINXOPTS = -W\nSPHINXBUI"
},
{
"path": "docs2/conf.py",
"chars": 8021,
"preview": "#!/usr/bin/env python3\n# -*- coding: utf-8 -*-\n#\n# Crazyswarm documentation build configuration file, created by\n# sphin"
},
{
"path": "docs2/faq.rst",
"chars": 4631,
"preview": ".. _faq:\n\nFrequently Asked Questions\n==========================\n\nHow can I get help?\n-------------------\n\nPlease start a"
},
{
"path": "docs2/howto.rst",
"chars": 5269,
"preview": ".. _howtos:\n\nHow To\n======\n\nThis page shows short how to's for the advanced usage.\n\n\nTracking Non-Robotic Objects\n------"
},
{
"path": "docs2/index.rst",
"chars": 2238,
"preview": ".. _introduction:\n\nCrazyswarm2: A ROS 2 testbed for Aerial Robot Teams\n================================================="
},
{
"path": "docs2/installation.rst",
"chars": 8998,
"preview": ".. _installation:\n\nInstallation\n============\n\nCrazyswarm2 runs on **Ubuntu Linux** in one of the following configuration"
},
{
"path": "docs2/overview.rst",
"chars": 6178,
"preview": ".. _overview:\n\nOverview\n========\n\nThis page will explain the overview of the crazyflie ROS 2 nodes:\n\n.. image:: images/a"
},
{
"path": "docs2/requirements.txt",
"chars": 95,
"preview": "numpy\npyyaml\nsphinx\nsphinx-argparse\nsphinx-rtd-theme\nsphinx_design\nsphinxcontrib-programoutput\n"
},
{
"path": "docs2/tutorials.rst",
"chars": 18875,
"preview": ".. _tutorials:\n\nROS 2 Tutorials\n===============\n\nThis page shows tutorials which connects the Crazyflie through Crazyswa"
},
{
"path": "docs2/usage.rst",
"chars": 11418,
"preview": ".. _usage:\n\nUsage\n=====\n\n.. warning::\n Do not forget to source your ROS 2 workspace in each terminal you would like t"
},
{
"path": "ros_ws/src/crazyswarm/launch/USC.yaml",
"chars": 4216,
"preview": "crazyflies:\n - id: 1\n channel: 100\n type: \"default\"\n initialPosition: [1.5, 1.5, 0.0]\n - id: 2\n channel: 1"
},
{
"path": "ros_ws/src/crazyswarm/launch/allCrazyflies.yaml",
"chars": 178,
"preview": "crazyflies:\n - id: 1\n channel: 100\n initialPosition: [1.5, 1.5, 0.0]\n type: medium\n - id: 40\n channel: 100"
},
{
"path": "ros_ws/src/crazyswarm/launch/figure8_smooth.csv",
"chars": 3215,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/launch/hover_swarm.launch",
"chars": 2570,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <arg name=\"joy_dev\" default=\"/dev/input/js0\" />\n\n <rosparam command=\"load\" file=\"$(fin"
},
{
"path": "ros_ws/src/crazyswarm/launch/mocap_helper.launch",
"chars": 297,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <node pkg=\"crazyswarm\" type=\"mocap_helper\" name=\"mocap_helper\" output=\"screen\" >\n <"
},
{
"path": "ros_ws/src/crazyswarm/launch/object_tracker.yaml",
"chars": 2915,
"preview": "hostName: vicon\nnumMarkerConfigurations: 2\nmarkerConfigurations:\n \"0\":\n numPoints: 4\n offset: [0.0, -0.01, -0.04]"
},
{
"path": "ros_ws/src/crazyswarm/launch/rqt.perspective",
"chars": 16328,
"preview": "{\n \"keys\": {}, \n \"groups\": {\n \"pluginmanager\": {\n \"keys\": {\n \"running-plugins\": {\n \"type\": \"re"
},
{
"path": "ros_ws/src/crazyswarm/launch/test.rviz",
"chars": 5340,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 89\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "ros_ws/src/crazyswarm/scripts/backgroundComputation.py",
"chars": 2346,
"preview": "#!/usr/bin/env python\n\n\"\"\"Demonstrate running a slow computation without blocking the main thread.\n\nComputations that ta"
},
{
"path": "ros_ws/src/crazyswarm/scripts/cmdVelocityCircle.py",
"chars": 1151,
"preview": "#!/usr/bin/env python\n\nimport numpy as np\nfrom pycrazyswarm import *\n\n\nZ = 1.0\nsleepRate = 30\n\n\ndef goCircle(timeHelper,"
},
{
"path": "ros_ws/src/crazyswarm/scripts/collisionAvoidance.py",
"chars": 4493,
"preview": "\"\"\"Demonstration of Buffered Voronoi Cell collision avoidance algorithm.\"\"\"\n\nfrom __future__ import print_function\nimpor"
},
{
"path": "ros_ws/src/crazyswarm/scripts/collisionAvoidanceHighConflict.py",
"chars": 3971,
"preview": "\"\"\"Worst-case test of Buffered Voronoi Cell collision avoidance algorithm.\n\nComputes the center of all initial positions"
},
{
"path": "ros_ws/src/crazyswarm/scripts/conftest.py",
"chars": 276,
"preview": "import subprocess\n\nimport pytest\n\n\nHAVE_ROS = subprocess.call(\"type roslaunch\", shell=True) == 0\n\n\ndef pytest_runtest_se"
},
{
"path": "ros_ws/src/crazyswarm/scripts/crossing2_pps/pp1.csv",
"chars": 3552,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/crossing2_pps/pp2.csv",
"chars": 3536,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/crossing4_pps/pp1.csv",
"chars": 3863,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/crossing4_pps/pp2.csv",
"chars": 3856,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/crossing4_pps/pp3.csv",
"chars": 3852,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/crossing4_pps/pp4.csv",
"chars": 3867,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/csv_sequence.py",
"chars": 7346,
"preview": "#!/usr/bin/env python\n\nfrom __future__ import print_function\n\nimport argparse\nimport os\nimport os.path\n\nimport numpy as "
},
{
"path": "ros_ws/src/crazyswarm/scripts/example_cmd_pos.py",
"chars": 572,
"preview": "from pycrazyswarm import *\nimport numpy as np\n\nswarm = Crazyswarm()\ntimeHelper = swarm.timeHelper\nallcfs = swarm.allcfs\n"
},
{
"path": "ros_ws/src/crazyswarm/scripts/graphVisualization.py",
"chars": 1721,
"preview": "#!/usr/bin/env python\n\n\"\"\"Demonstrates the graph visualization feature of the 3D visualizer.\"\"\"\n\nimport numpy as np\n\nfro"
},
{
"path": "ros_ws/src/crazyswarm/scripts/individual_hover.py",
"chars": 425,
"preview": "#!/usr/bin/env python\n\nfrom __future__ import print_function\n\nfrom pycrazyswarm import *\n\ndef main():\n swarm = Crazys"
},
{
"path": "ros_ws/src/crazyswarm/scripts/led_colors.py",
"chars": 793,
"preview": "#!/usr/bin/env python\n\nimport numpy as np\n\nfrom pycrazyswarm import *\nimport uav_trajectory\n\nif __name__ == \"__main__\":\n"
},
{
"path": "ros_ws/src/crazyswarm/scripts/pytest.ini",
"chars": 75,
"preview": "[pytest]\nmarkers =\n ros: tests that can only run when ROS is installed.\n"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/1.csv",
"chars": 3554,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/10.csv",
"chars": 2948,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/11.csv",
"chars": 3872,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/12.csv",
"chars": 3250,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/13.csv",
"chars": 3253,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/14.csv",
"chars": 3254,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/15.csv",
"chars": 3248,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/16.csv",
"chars": 3244,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/17.csv",
"chars": 2620,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/18.csv",
"chars": 2316,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/19.csv",
"chars": 6059,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
},
{
"path": "ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/2.csv",
"chars": 5102,
"preview": "duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw"
}
]
// ... and 160 more files (download for full content)
About this extraction
This page contains the full source code of the IMRCLab/crazyswarm2 GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 360 files (1.7 MB), approximately 772.4k tokens, and a symbol index with 498 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.