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. diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index 1b3479b5a..670c6dbe7 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/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"): 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) diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index 9594e2ee4..a76fdfc80 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; @@ -147,6 +148,18 @@ class CrazyflieROS 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(); + 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(); @@ -684,6 +697,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_; @@ -720,7 +752,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 @@ -772,6 +810,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 = @@ -841,16 +892,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]; } diff --git a/crazyflie_examples/launch/keyboard_velmux_launch.py b/crazyflie_examples/launch/keyboard_velmux_launch.py index 25d318a0b..fa06c87ba 100644 --- a/crazyflie_examples/launch/keyboard_velmux_launch.py +++ b/crazyflie_examples/launch/keyboard_velmux_launch.py @@ -31,8 +31,8 @@ def generate_launch_description(): executable='vel_mux.py', name='vel_mux', output='screen', - parameters=[{'hover_height': 0.3}, - {'incoming_twist_topic': '/cmd_vel'}, - {'robot_prefix': '/cf1'}] + parameters=[{"hover_height": 0.3}, + {"incoming_twist_topic": "/cmd_vel"}, + {"robot_prefix": "/cf231"}] ), ]) 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() ), 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() diff --git a/docs2/howto.rst b/docs2/howto.rst index 7c214d872..7c0ed6bdf 100644 --- a/docs2/howto.rst +++ b/docs2/howto.rst @@ -99,6 +99,39 @@ 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: + +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 :ref:`Collision Avoidance`) or preplan trajectories using https://github.com/IMRCLab/db-CBS or https://github.com/mjdebord/smoothener/tree/cylinders. diff --git a/docs2/installation.rst b/docs2/installation.rst index 9cc72c428..325bb73f0 100644 --- a/docs2/installation.rst +++ b/docs2/installation.rst @@ -56,7 +56,21 @@ 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 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 `_. + + 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!), diff --git a/docs2/tutorials.rst b/docs2/tutorials.rst index bb550ce1c..c007409bd 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 iron): @@ -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 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 - -