From 34b0fc8b7c1c9c60a2db960357f86b63ebdf319a Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 18 Oct 2023 14:42:10 +0200 Subject: [PATCH 01/13] add notify setpoint --- crazyflie/scripts/crazyflie_server.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index ad8e299b4..b52bb6253 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -785,7 +785,15 @@ def _go_to_callback(self, request, response, uri="all"): return response def _notify_setpoints_stop_callback(self, request, response, uri="all"): - self.get_logger().info("Notify setpoint stop not yet implemented") + + self.get_logger().info(f"{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"): From 4b5cae6137e19827b7e9940edfe1570706f3fba2 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 18 Oct 2023 14:42:27 +0200 Subject: [PATCH 02/13] fix vel_mux issue --- crazyflie/scripts/vel_mux.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py index f1155ba1c..aab381cbb 100755 --- a/crazyflie/scripts/vel_mux.py +++ b/crazyflie/scripts/vel_mux.py @@ -12,7 +12,7 @@ from rclpy.node import Node from geometry_msgs.msg import Twist -from crazyflie_interfaces.srv import Takeoff, Land +from crazyflie_interfaces.srv import Takeoff, Land, NotifySetpointsStop from crazyflie_interfaces.msg import Hover import time @@ -39,6 +39,7 @@ def __init__(self): 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() @@ -72,6 +73,8 @@ def timer_callback(self): 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() @@ -80,7 +83,6 @@ def timer_callback(self): self.cf_has_taken_off = False self.received_first_cmd_vel = False - def main(args=None): rclpy.init(args=args) From 12e602b9917f889851a19a15fee250ee225d9588 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 18 Oct 2023 14:48:37 +0200 Subject: [PATCH 03/13] fix explaination example. Fixes #307 --- crazyflie_examples/launch/keyboard_velmux_launch.py | 2 +- docs2/tutorials.rst | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/crazyflie_examples/launch/keyboard_velmux_launch.py b/crazyflie_examples/launch/keyboard_velmux_launch.py index db7badac4..a6ad3f674 100644 --- a/crazyflie_examples/launch/keyboard_velmux_launch.py +++ b/crazyflie_examples/launch/keyboard_velmux_launch.py @@ -31,6 +31,6 @@ def generate_launch_description(): output='screen', parameters=[{"hover_height": 0.3}, {"incoming_twist_topic": "/cmd_vel"}, - {"robot_prefix": "/cf1"}] + {"robot_prefix": "/cf231"}] ), ]) diff --git a/docs2/tutorials.rst b/docs2/tutorials.rst index 58070bc67..a13e9a876 100644 --- a/docs2/tutorials.rst +++ b/docs2/tutorials.rst @@ -17,7 +17,7 @@ Teleoperation keyboard We have an example of the telop_twist_keyboard package working together with the crazyflie First, make sure that the crazyflies.yaml has the right URI and if you are using the `Flow deck `_ or `any other position system available `_ to the crazyflie. -set the controller to 1 (PID) +set the controller to 1 (PID). And if you have not already, install the teleop package for the keyboard. (replace DISTRO with humble or galactic): @@ -25,7 +25,9 @@ And if you have not already, install the teleop package for the keyboard. (repl sudo apt-get install ros-DISTRO-teleop-twist-keyboard -Then, run the following launch file to start up the crazyflie server (CFlib): +Then, first checkout keyboard_velmux_launch.py and make sure that the 'robot_prefix' of vel_mux matches your crazyflie ID in crazyfies.yaml ('cf231'). + +Then run the following launch file to start up the crazyflie server (CFlib): .. code-block:: bash From b22d9881b300858b981adfbdfd1ecedf36c43c84 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 7 Nov 2023 11:26:27 +0100 Subject: [PATCH 04/13] server (cpp): continous unicast latency monitoring --- crazyflie/CMakeLists.txt | 1 + crazyflie/config/server.yaml | 2 + crazyflie/deps/crazyflie_tools | 2 +- crazyflie/src/crazyflie_server.cpp | 63 ++++++++++++++++++++++++------ 4 files changed, 56 insertions(+), 12 deletions(-) diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index ca4dc3764..8553b314d 100644 --- a/crazyflie/CMakeLists.txt +++ b/crazyflie/CMakeLists.txt @@ -85,6 +85,7 @@ ament_target_dependencies(crazyflie_server # Install C++ executables install(TARGETS # crazyflie_tools + comCheck scan listParams listLogVariables diff --git a/crazyflie/config/server.yaml b/crazyflie/config/server.yaml index d1ca14a8e..e0d8d6af6 100644 --- a/crazyflie/config/server.yaml +++ b/crazyflie/config/server.yaml @@ -4,6 +4,8 @@ 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 firmware_params: query_all_values_on_connect: False # simulation related diff --git a/crazyflie/deps/crazyflie_tools b/crazyflie/deps/crazyflie_tools index f26165753..65a8fcdf4 160000 --- a/crazyflie/deps/crazyflie_tools +++ b/crazyflie/deps/crazyflie_tools @@ -1 +1 @@ -Subproject commit f26165753fb8cb527ca4f78f81b2db0a7c4c251f +Subproject commit 65a8fcdf48762994dce277db506793cdcc9c7135 diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index d0910f3b5..8c7d9c3c8 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -119,6 +119,7 @@ class CrazyflieROS , name_(name) , node_(node) , tf_broadcaster_(node) + , last_on_latency_(std::chrono::steady_clock::now()) { auto sub_opt_cf_cmd = rclcpp::SubscriptionOptions(); sub_opt_cf_cmd.callback_group = callback_group_cf_cmd; @@ -143,6 +144,18 @@ class CrazyflieROS // spinning timer spin_timer_ = node->create_wall_timer(std::chrono::milliseconds(100), std::bind(&CrazyflieROS::spin_once, this), callback_group_cf_srv); + // link statistics + warning_freq_ = node->get_parameter("warnings.frequency").get_parameter_value().get(); + max_latency_ = node->get_parameter("warnings.communication.max_unicast_latency").get_parameter_value().get(); + + 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(); @@ -679,6 +692,25 @@ class CrazyflieROS (*pub)->publish(msg); } + void on_link_statistics_timer() + { + cf_.triggerLatencyMeasurement(); + + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - last_on_latency_; + if (elapsed.count() > 1.0 / warning_freq_) { + RCLCPP_WARN(logger_, "last latency update: %f s", elapsed.count()); + } + } + + void on_latency(uint64_t latency_in_us) + { + if (latency_in_us / 1000.0 > max_latency_) { + RCLCPP_WARN(logger_, "Latency: %f ms", latency_in_us / 1000.0); + } + last_on_latency_ = std::chrono::steady_clock::now(); + } + private: rclcpp::Logger logger_; CrazyflieLogger cf_logger_; @@ -715,7 +747,13 @@ class CrazyflieROS // multithreading rclcpp::CallbackGroup::SharedPtr callback_group_cf_; rclcpp::TimerBase::SharedPtr spin_timer_; - + + // link statistics + rclcpp::TimerBase::SharedPtr link_statistics_timer_; + std::chrono::time_point last_on_latency_; + float warning_freq_; + float max_latency_; + }; class CrazyflieServer : public rclcpp::Node @@ -767,6 +805,19 @@ class CrazyflieServer : public rclcpp::Node broadcasts_delay_between_repeats_ms_ = this->get_parameter("all.broadcasts.delay_between_repeats_ms").get_parameter_value().get(); mocap_enabled_ = false; + // Warnings + this->declare_parameter("warnings.frequency", 1.0); + float freq = this->get_parameter("warnings.frequency").get_parameter_value().get(); + 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({80.0, 120.0})); + auto rate_range = this->get_parameter("warnings.motion_capture.warning_if_rate_outside").get_parameter_value().get>(); + mocap_min_rate_ = rate_range[0]; + mocap_max_rate_ = rate_range[1]; + + this->declare_parameter("warnings.communication.max_unicast_latency", 10.0); + // load crazyflies from params auto node_parameters_iface = this->get_node_parameters_interface(); const std::map ¶meter_overrides = @@ -836,16 +887,6 @@ class CrazyflieServer : public rclcpp::Node param_subscriber_ = std::make_shared(this); cb_handle_ = param_subscriber_->add_parameter_event_callback(std::bind(&CrazyflieServer::on_parameter_event, this, _1)); - // Warnings - this->declare_parameter("warnings.frequency", 1.0); - float freq = this->get_parameter("warnings.frequency").get_parameter_value().get(); - 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({80.0, 120.0})); - auto rate_range = this->get_parameter("warnings.motion_capture.warning_if_rate_outside").get_parameter_value().get>(); - mocap_min_rate_ = rate_range[0]; - mocap_max_rate_ = rate_range[1]; } From ae9815d46dc7dc7523ab0c33dd827c5b08aa8800 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 Nov 2023 10:43:02 +0100 Subject: [PATCH 05/13] sim: fix cmd_full_state example --- crazyflie_sim/crazyflie_sim/crazyflie_sil.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crazyflie_sim/crazyflie_sim/crazyflie_sil.py b/crazyflie_sim/crazyflie_sim/crazyflie_sil.py index cf904d746..dc3018ad0 100644 --- a/crazyflie_sim/crazyflie_sim/crazyflie_sil.py +++ b/crazyflie_sim/crazyflie_sim/crazyflie_sil.py @@ -296,7 +296,7 @@ def executeController(self): if self.controller is None: return None - if self.mode != CrazyflieSIL.MODE_HIGH_POLY: + if self.mode == CrazyflieSIL.MODE_IDLE: return sim_data_types.Action([0, 0, 0, 0]) time_in_seconds = self.time_func() From a8a1625dfc28e4445e6afad036819ee172bbc980 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 Nov 2023 11:06:24 +0100 Subject: [PATCH 06/13] Add docs on collision avoidance and traj --- docs2/howto.rst | 37 ++++++++++++++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/docs2/howto.rst b/docs2/howto.rst index 7c214d872..274f52374 100644 --- a/docs2/howto.rst +++ b/docs2/howto.rst @@ -99,6 +99,37 @@ This requires some updated pip packages for testing, see https://docs.ros.org/en Then execute: -``` -colcon test --event-handlers=console_cohesion+ --return-code-on-test-failure --packages-select crazyflie_py -``` +.. code-block:: bash + + colcon test --event-handlers=console_cohesion+ --return-code-on-test-failure --packages-select crazyflie_py + +Collision Avoidance +------------------- + +The official firmware has support for collision avoidance using the Buffered Voronoi Cell algorithm. +It requires the use of a motion capture system (so that the positions of other drones are known) and can be enabled +in the `crazyflies.yaml`: + +.. code-block:: yaml + + all: + firmware_params: + colAv: + enable: 1 + +or inside a Python script via: + +.. code-block:: python + + swarm = Crazyswarm() + allcfs = swarm.allcfs + allcfs.setParam("colAv.enable", 1) + +Note that the algorithm might require tuning of its hyperparameters. Documention can be found at https://github.com/bitcraze/crazyflie-firmware/blob/dbb9df1137f11d4e7e3771c56d25a7137b5b69cc/src/modules/src/collision_avoidance.c#L348-L428. + +Generate Trajectories +--------------------- + +Crazyswarm2 supports polynomial trajectories (8th order). These can be generated from waypoints, waypoint/time pairs, or optimization. Useful tools are available at https://github.com/whoenig/uav_trajectories, including scripts to visualize the resulting trajectories. + +For the multi-robot case, there is no easy to-use library, yet. One can use collision avoidance (see HowTo) or preplan trajectories using https://github.com/IMRCLab/db-CBS or https://github.com/mjdebord/smoothener/tree/cylinders. From e8808b989ef9053d7e998574310492c3fdaa550f Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Thu, 16 Nov 2023 13:27:13 +0100 Subject: [PATCH 07/13] add usb permission and update instructions Should fix #50 --- docs2/installation.rst | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/docs2/installation.rst b/docs2/installation.rst index 9cc72c428..404f9f20a 100644 --- a/docs2/installation.rst +++ b/docs2/installation.rst @@ -56,7 +56,11 @@ First Installation .. note:: symlink-install allows you to edit Python and config files without running `colcon build` every time. -5. Set up software-in-the-loop simulation (optional) +5. Set up USB permissions for crazyradio + + For the crazyradio, you need to setup usb rules in order to communicate with the Crazyflie. Find the instructions for that here `in Bitcraze's USB permission guide for Linux `_. + +6. Set up software-in-the-loop simulation (optional) This currently requires cloning the Crazyflie firmware and building the Python bindings manually. In a separate folder (not part of your ROS 2 workspace!), From 890e7ceb8e1b2c779ec78c64a3c1a3497eb04deb Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Thu, 16 Nov 2023 13:41:15 +0100 Subject: [PATCH 08/13] Update installation.rst --- docs2/installation.rst | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/docs2/installation.rst b/docs2/installation.rst index 404f9f20a..9ed2813d2 100644 --- a/docs2/installation.rst +++ b/docs2/installation.rst @@ -56,11 +56,22 @@ First Installation .. note:: symlink-install allows you to edit Python and config files without running `colcon build` every time. -5. Set up USB permissions for crazyradio +5. Set up Crazyradio For the crazyradio, you need to setup usb rules in order to communicate with the Crazyflie. Find the instructions for that here `in Bitcraze's USB permission guide for Linux `_. - -6. Set up software-in-the-loop simulation (optional) + + You will also need to update the crazyradio firmware to the latest development branch to be able to use all features. For Crazyradio PA (1), `follow these instructions `_. For Crazyradio 2, follow `these instuctions to build the firmware `_ and `these instuctions to flash it `_. + +6. Update the crazyflies + + If this is the first time handling crazyflies it is always good to start with `Bitcraze's getting started guide `_. + + You can update each crazyflie firmware to the latest release via `these instructions of the Bitcraze Crazyflie client `_ . + + While you are at it, make sure that each crazyflie have an unique radio address which you can change in `the client via these instructions `_ . + +7. Set up software-in-the-loop simulation (optional) This currently requires cloning the Crazyflie firmware and building the Python bindings manually. In a separate folder (not part of your ROS 2 workspace!), From 316eb23632586d13d8b735d0c5b27a0505bb13b4 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Nov 2023 14:32:35 +0100 Subject: [PATCH 09/13] fix uppercase error and lost file --- crazyflie_examples/launch/multiranger_nav2_launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/crazyflie_examples/launch/multiranger_nav2_launch.py b/crazyflie_examples/launch/multiranger_nav2_launch.py index 9e1421325..33d834242 100644 --- a/crazyflie_examples/launch/multiranger_nav2_launch.py +++ b/crazyflie_examples/launch/multiranger_nav2_launch.py @@ -67,11 +67,11 @@ def generate_launch_description(): os.path.join(bringup_launch_dir, 'bringup_launch.py')), launch_arguments={ 'slam': 'False', - 'use_sim_time': 'false', + 'use_sim_time': 'False', 'map': cf_examples_dir + '/data/' + map_name + '.yaml', - 'params_file': os.path.join(cf_examples_dir, 'nav2_params.yaml'), - 'autostart': 'true', - 'use_composition': 'true', + 'params_file': os.path.join(cf_examples_dir, 'config/nav2_params.yaml'), + 'autostart': 'True', + 'use_composition': 'True', 'transform_publish_period': '0.02' }.items() ), From ae7db66b37e73f27364d49bc0b7ff8de6ca45212 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Thu, 16 Nov 2023 16:51:50 +0100 Subject: [PATCH 10/13] Delete pc_permissions.sh --- pc_permissions.sh | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100755 pc_permissions.sh diff --git a/pc_permissions.sh b/pc_permissions.sh deleted file mode 100755 index 1b225474d..000000000 --- a/pc_permissions.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/bin/bash - -# Sets PC permissions - -sudo groupadd plugdev -sudo usermod -a -G plugdev $USER - -echo -e "# Crazyradio (normal operation) \nSUBSYSTEM==\"usb\", ATTRS{idVendor}==\"1915\", ATTRS{idProduct}==\"7777\", MODE=\"0664\", GROUP=\"plugdev\" \n# Bootloader \nSUBSYSTEM==\"usb\", ATTRS{idVendor}==\"1915\", ATTRS{idProduct}==\"0101\", MODE=\"0664\", GROUP=\"plugdev\"" | sudo tee /etc/udev/rules.d/99-crazyradio.rules - - -echo -e "SUBSYSTEM==\"usb\", ATTRS{idVendor}==\"0483\", ATTRS{idProduct}==\"5740\", MODE=\"0664\", GROUP=\"plugdev\"" | sudo tee /etc/udev/rules.d/99-crazyflie.rules - -sudo udevadm control --reload-rules && udevadm trigger - - From 7e3328cc9ed77e383ff0d7bc70ad51ab2cfa8642 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 Nov 2023 16:58:09 +0100 Subject: [PATCH 11/13] add internal link --- docs2/howto.rst | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docs2/howto.rst b/docs2/howto.rst index 274f52374..7c0ed6bdf 100644 --- a/docs2/howto.rst +++ b/docs2/howto.rst @@ -103,6 +103,8 @@ Then execute: colcon test --event-handlers=console_cohesion+ --return-code-on-test-failure --packages-select crazyflie_py +.. _Collision Avoidance: + Collision Avoidance ------------------- @@ -132,4 +134,4 @@ Generate Trajectories Crazyswarm2 supports polynomial trajectories (8th order). These can be generated from waypoints, waypoint/time pairs, or optimization. Useful tools are available at https://github.com/whoenig/uav_trajectories, including scripts to visualize the resulting trajectories. -For the multi-robot case, there is no easy to-use library, yet. One can use collision avoidance (see HowTo) or preplan trajectories using https://github.com/IMRCLab/db-CBS or https://github.com/mjdebord/smoothener/tree/cylinders. +For the multi-robot case, there is no easy to-use library, yet. One can use collision avoidance (see :ref:`Collision Avoidance`) or preplan trajectories using https://github.com/IMRCLab/db-CBS or https://github.com/mjdebord/smoothener/tree/cylinders. From fed4474b12271857f6069c7cc2e6a57ebed24aab Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 Nov 2023 17:16:49 +0100 Subject: [PATCH 12/13] fix doc build --- docs2/installation.rst | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/docs2/installation.rst b/docs2/installation.rst index 9ed2813d2..325bb73f0 100644 --- a/docs2/installation.rst +++ b/docs2/installation.rst @@ -64,8 +64,7 @@ First Installation 6. Update the crazyflies - If this is the first time handling crazyflies it is always good to start with `Bitcraze's getting started guide `_. + If this is the first time handling crazyflies it is always good to start with `Bitcraze's getting started guide `_. You can update each crazyflie firmware to the latest release via `these instructions of the Bitcraze Crazyflie client `_ . From 94c61d4ecee4c5601f19f2da8fcb4c3c17a8e4ff Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 Nov 2023 17:22:28 +0100 Subject: [PATCH 13/13] Run docs CI on PRs; deploy only in main --- .github/workflows/ci-docs2.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci-docs2.yml b/.github/workflows/ci-docs2.yml index 3884a8f19..37521f765 100644 --- a/.github/workflows/ci-docs2.yml +++ b/.github/workflows/ci-docs2.yml @@ -1,8 +1,10 @@ -name: Docs deploy +name: Docs on: push: branches: [ main ] + pull_request: + branches: [ main ] jobs: build: @@ -33,6 +35,7 @@ jobs: 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.