Merge branch 'main' into issue348

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

crazyflie/CMakeLists.txt

@@ -85,6 +85,7 @@ ament_target_dependencies(crazyflie_server
 # Install C++ executables
 install(TARGETS
  # crazyflie_tools
+ comCheck
  scan
  listParams
  listLogVariables

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

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"):

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)
 

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<float>();
+ max_latency_ = node->get_parameter("warnings.communication.max_unicast_latency").get_parameter_value().get<float>();
+
+ 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<double> 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<std::chrono::steady_clock> 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<int>();
  mocap_enabled_ = false;
 
+ // 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);
+
  // load crazyflies from params
  auto node_parameters_iface = this->get_node_parameters_interface();
  const std::map<std::string, rclcpp::ParameterValue> &parameter_overrides =
@@ -841,16 +892,6 @@ class CrazyflieServer : public rclcpp::Node
  param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(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<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];
  }
 
 

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"}]
  ),
  ])

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()
  ),

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()

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. 

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 <https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/installation/usb_permissions/>`_.
+
+ 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 <https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/building/building_flashing/>`_. For Crazyradio 2, follow `these instuctions to build the firmware <https://www.bitcraze.io/documentation/repository/crazyradio-firmware/master/building/building_flashing/>`_ and `these instuctions to flash it <https://www.bitcraze.io/documentation/repository/crazyradio2-firmware/main/building-and-flashing/flash//>`_.
+
+6. Update the crazyflies
+
+ If this is the first time handling crazyflies it is always good to start with `Bitcraze's getting started guide <https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/>`_.
+
+ You can update each crazyflie firmware to the latest release via `these instructions of the Bitcraze Crazyflie client <https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-upgrade>`_ .
+
+ 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 <https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-configuration>`_ .
+
+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!), 
 

docs2/tutorials.rst

@@ -17,15 +17,17 @@ 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 <https://www.bitcraze.io/products/flow-deck-v2/>`_ or `any other position system available <https://www.bitcraze.io/documentation/system/positioning//>`_ 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):
 
 .. code-block:: bash
 
  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