From 34b0fc8b7c1c9c60a2db960357f86b63ebdf319a Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 18 Oct 2023 14:42:10 +0200 Subject: [PATCH 01/38] 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/38] 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/38] 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 4bad810674d8c9f3a2dcb8562ea06c24dedb4237 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Wed, 1 Nov 2023 14:10:57 +0100 Subject: [PATCH 04/38] cpp backend: process all incoming radio messages Process all radio messages every millisecond. The behavior before was to process one message every 100 ms, which caused long delays for incoming messages (such as log topics). Fixes #300 --- crazyflie/deps/crazyflie_tools | 2 +- crazyflie/src/crazyflie_server.cpp | 9 +++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/crazyflie/deps/crazyflie_tools b/crazyflie/deps/crazyflie_tools index f26165753..2e0ae6da8 160000 --- a/crazyflie/deps/crazyflie_tools +++ b/crazyflie/deps/crazyflie_tools @@ -1 +1 @@ -Subproject commit f26165753fb8cb527ca4f78f81b2db0a7c4c251f +Subproject commit 2e0ae6da886aff19bf93116c487e7c737fa3e417 diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index d0910f3b5..9594e2ee4 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -141,7 +141,11 @@ class CrazyflieROS subscription_cmd_position_ = node->create_subscription(name + "/cmd_position", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieROS::cmd_position_changed, this, _1), sub_opt_cf_cmd); // spinning timer - spin_timer_ = node->create_wall_timer(std::chrono::milliseconds(100), std::bind(&CrazyflieROS::spin_once, this), callback_group_cf_srv); + // 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); auto start = std::chrono::system_clock::now(); @@ -367,7 +371,8 @@ class CrazyflieROS void spin_once() { - cf_.spin_once(); + // process all packets from the receive queue + cf_.processAllPackets(); } std::string broadcastUri() const From b22d9881b300858b981adfbdfd1ecedf36c43c84 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 7 Nov 2023 11:26:27 +0100 Subject: [PATCH 05/38] 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 8319945e73f6fb6d82baa6eebb5e9b25dd42c0f7 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 Nov 2023 09:54:39 +0100 Subject: [PATCH 06/38] add support for downloadUSDLogfile --- crazyflie/CMakeLists.txt | 1 + crazyflie/deps/crazyflie_tools | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index ca4dc3764..1b3479b5a 100644 --- a/crazyflie/CMakeLists.txt +++ b/crazyflie/CMakeLists.txt @@ -95,6 +95,7 @@ install(TARGETS console log setParam + downloadUSDLogfile # teleop crazyflie_server diff --git a/crazyflie/deps/crazyflie_tools b/crazyflie/deps/crazyflie_tools index 2e0ae6da8..c7f3b6c89 160000 --- a/crazyflie/deps/crazyflie_tools +++ b/crazyflie/deps/crazyflie_tools @@ -1 +1 @@ -Subproject commit 2e0ae6da886aff19bf93116c487e7c737fa3e417 +Subproject commit c7f3b6c895be149073758825383bdfd9d31f6679 From ae9815d46dc7dc7523ab0c33dd827c5b08aa8800 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 Nov 2023 10:43:02 +0100 Subject: [PATCH 07/38] 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 08/38] 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 09/38] 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 10/38] 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 11/38] 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 c111710eccbdbaf44f93e45edfae8535e40c0e31 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Nov 2023 15:26:19 +0100 Subject: [PATCH 12/38] add simple mapper --- crazyflie_examples/CMakeLists.txt | 6 + .../launch/multiranger_simple_mapper.py | 46 +++++ crazyflie_examples/scripts/simple_mapper.py | 165 ++++++++++++++++++ 3 files changed, 217 insertions(+) create mode 100644 crazyflie_examples/launch/multiranger_simple_mapper.py create mode 100644 crazyflie_examples/scripts/simple_mapper.py diff --git a/crazyflie_examples/CMakeLists.txt b/crazyflie_examples/CMakeLists.txt index 0c70c94d2..68ed05566 100644 --- a/crazyflie_examples/CMakeLists.txt +++ b/crazyflie_examples/CMakeLists.txt @@ -32,4 +32,10 @@ if(BUILD_TESTING) endforeach() endif() +# Install Python executables +install(PROGRAMS + scripts/simple_mapper.py + DESTINATION lib/${PROJECT_NAME} +) + ament_package() diff --git a/crazyflie_examples/launch/multiranger_simple_mapper.py b/crazyflie_examples/launch/multiranger_simple_mapper.py new file mode 100644 index 000000000..1b7b56d3a --- /dev/null +++ b/crazyflie_examples/launch/multiranger_simple_mapper.py @@ -0,0 +1,46 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +import yaml + + +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 + + cf_examples_dir = get_package_share_directory('crazyflie_examples') + bringup_dir = get_package_share_directory('nav2_bringup') + bringup_launch_dir = os.path.join(bringup_dir, 'launch') + map_name = 'map' + + return LaunchDescription([ + Node( + package='crazyflie', + executable='crazyflie_server.py', + name='crazyflie_server', + output='screen', + parameters=[{'world_tf_name': 'map'}, + server_params], + ), + Node( + package='crazyflie', + executable='vel_mux.py', + name='vel_mux', + output='screen', + parameters=[{'hover_height': 0.3}, + {'incoming_twist_topic': '/cmd_vel'}, + {'robot_prefix': '/cf231'}] + ) + ]) diff --git a/crazyflie_examples/scripts/simple_mapper.py b/crazyflie_examples/scripts/simple_mapper.py new file mode 100644 index 000000000..dc5f9791b --- /dev/null +++ b/crazyflie_examples/scripts/simple_mapper.py @@ -0,0 +1,165 @@ +""" 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 SimpleMapper(Node): + def __init__(self): + super().__init__('simple_mapper') + self.odom_subscriber = self.create_subscription(Odometry, '/odom', self.odom_subcribe_callback, 10) + self.ranges_subscriber = self.create_subscription(LaserScan, '/scan', self.scan_subsribe_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 = '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, '/map', + qos_profile=QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST,)) + + def odom_subcribe_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_subsribe_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_left = self.ranges[1] + r_front = self.ranges[2] + r_right = 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 = SimpleMapper() + rclpy.spin(simple_mapper) + rclpy.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file From ae7db66b37e73f27364d49bc0b7ff8de6ca45212 Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Thu, 16 Nov 2023 16:51:50 +0100 Subject: [PATCH 13/38] 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 14/38] 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 15/38] 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 16/38] 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. From 0ef00381c7a434a3f5d9a038c0c2faccdaa5e4b1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 16 Nov 2023 17:38:08 +0100 Subject: [PATCH 17/38] fix flake8 errors --- crazyflie_examples/launch/keyboard_velmux_launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/crazyflie_examples/launch/keyboard_velmux_launch.py b/crazyflie_examples/launch/keyboard_velmux_launch.py index fa06c87ba..8fee2f797 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": "/cf231"}] + parameters=[{'hover_height': 0.3}, + {'incoming_twist_topic': '/cmd_vel'}, + {'robot_prefix': '/cf231'}] ), ]) From 4d616a4897cf218ef42b2e6b18fcdab9c94779ce Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 17 Nov 2023 14:25:20 +0100 Subject: [PATCH 18/38] update submodule --- crazyflie/deps/crazyflie_tools | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crazyflie/deps/crazyflie_tools b/crazyflie/deps/crazyflie_tools index c7f3b6c89..1efd3788f 160000 --- a/crazyflie/deps/crazyflie_tools +++ b/crazyflie/deps/crazyflie_tools @@ -1 +1 @@ -Subproject commit c7f3b6c895be149073758825383bdfd9d31f6679 +Subproject commit 1efd3788f5bf59e0fa3da2e4d2bc5cd7bee5b0fc From 487f7e7eeb03d3a9208177afde75b36f16a31502 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Nov 2023 13:23:43 +0100 Subject: [PATCH 19/38] add trajectory cflib --- crazyflie/scripts/crazyflie_server.py | 70 ++++++++++++++++++++++++++- 1 file changed, 68 insertions(+), 2 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index b52bb6253..a47c9c896 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -20,6 +20,8 @@ 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 @@ -797,11 +799,75 @@ def _notify_setpoints_stop_callback(self, request, response, uri="all"): return response def _upload_trajectory_callback(self, request, response, uri="all"): - self.get_logger().info("Notify trajectory not yet implemented") + + id = request.trajectory_id + offset = request.piece_offset + size = request.pieces.size() + total_duration = 0 + self.get_logger().info("upload_trajectory(id=%d,offset=%d, size=%d)"% ( + id, + offset, + size, + )) + + trajectory = [] + for i in range(size): + piece = request.pieces[i] + px = piece.poly_x + py = piece.poly_y + pz = piece.poly_z + pyaw = piece.poly_yaw + duration = request.duration.sec + 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)[id] + trajectory_mem.trajectory = trajectory + upload_result = trajectory_mem.write_data_sync() + if not upload_result: + self.get_logger().info(f"{link_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)[id] + trajectory_mem.trajectory = trajectory + upload_result = trajectory_mem.write_data_sync() + if not upload_result: + self.get_logger().info(f"{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"): - self.get_logger().info("Start trajectory not yet implemented") + + id = request.trajectory_id + ts = request.timescale + rel = request.relative + rev = request.reversed + gm = request.group_mask + + self.get_logger().info("start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)"% ( + id, + ts, + rel, + rev, + gm + )) + if uri == "all": + for link_uri in self.uris: + self.swarm._cfs[link_uri].cf.commander.start_trajectory(id, ts, rel, rev, gm) + else: + self.swarm._cfs[uri].cf.commander.start_trajectory(id, ts, rel, rev, gm) + return response def _poses_changed(self, msg): From 0a37e57049f1cd2cedd5089868108915bf664167 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Nov 2023 13:55:41 +0100 Subject: [PATCH 20/38] fixed error messaging --- crazyflie/scripts/crazyflie_server.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index a47c9c896..138dc6bf8 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -802,22 +802,22 @@ def _upload_trajectory_callback(self, request, response, uri="all"): id = request.trajectory_id offset = request.piece_offset - size = request.pieces.size() + lenght = len(request.pieces) total_duration = 0 - self.get_logger().info("upload_trajectory(id=%d,offset=%d, size=%d)"% ( + self.get_logger().info("upload_trajectory(id=%d,offset=%d, lenght=%d)"% ( id, offset, - size, + lenght, )) trajectory = [] - for i in range(size): + for i in range(lenght): piece = request.pieces[i] - px = piece.poly_x - py = piece.poly_y - pz = piece.poly_z - pyaw = piece.poly_yaw - duration = request.duration.sec + 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 = piece.duration.sec trajectory.append(Poly4D(duration, px, py, pz, pyaw )) total_duration = total_duration + duration @@ -864,9 +864,9 @@ def _start_trajectory_callback(self, request, response, uri="all"): )) if uri == "all": for link_uri in self.uris: - self.swarm._cfs[link_uri].cf.commander.start_trajectory(id, ts, rel, rev, gm) + self.swarm._cfs[link_uri].cf.high_level_commander.start_trajectory(id, ts, rel, rev, gm) else: - self.swarm._cfs[uri].cf.commander.start_trajectory(id, ts, rel, rev, gm) + self.swarm._cfs[uri].cf.high_level_commander.start_trajectory(id, ts, rel, rev, gm) return response From fb11fa03fc79e766e8eacdfc2c3bcca97f8040bc Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Nov 2023 14:00:06 +0100 Subject: [PATCH 21/38] add launch file --- .../multiranger_simple_mapper_launch.py | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 crazyflie_examples/launch/multiranger_simple_mapper_launch.py diff --git a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py new file mode 100644 index 000000000..f046d8996 --- /dev/null +++ b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py @@ -0,0 +1,44 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import yaml + + +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 + + return LaunchDescription([ + Node( + package='crazyflie', + executable='crazyflie_server.py', + name='crazyflie_server', + output='screen', + parameters=[server_params], + ), + Node( + package='crazyflie', + executable='vel_mux.py', + name='vel_mux', + output='screen', + parameters=[{'hover_height': 0.3}, + {'incoming_twist_topic': '/cmd_vel'}, + {'robot_prefix': '/cf1'}] + ), + #Node( + # package='crazyflie_examples', + # executable='simple_mapper.py', + # name='simple mapper', + # output='screen' + #), + ]) From 8468108f4b75bc29ce47b6634362239f296b026a Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Nov 2023 14:39:32 +0100 Subject: [PATCH 22/38] moved simple mapper to crazyflie scripts --- crazyflie/CMakeLists.txt | 1 + .../scripts/simple_mapper.py | 9 +++- crazyflie_examples/CMakeLists.txt | 6 --- .../launch/multiranger_simple_mapper.py | 46 ------------------- .../multiranger_simple_mapper_launch.py | 12 ++--- 5 files changed, 14 insertions(+), 60 deletions(-) rename {crazyflie_examples => crazyflie}/scripts/simple_mapper.py (95%) mode change 100644 => 100755 delete mode 100644 crazyflie_examples/launch/multiranger_simple_mapper.py diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index ca4dc3764..828cd9a44 100644 --- a/crazyflie/CMakeLists.txt +++ b/crazyflie/CMakeLists.txt @@ -106,6 +106,7 @@ install(PROGRAMS scripts/chooser.py scripts/vel_mux.py scripts/cfmult.py + scripts/simple_mapper.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/crazyflie_examples/scripts/simple_mapper.py b/crazyflie/scripts/simple_mapper.py old mode 100644 new mode 100755 similarity index 95% rename from crazyflie_examples/scripts/simple_mapper.py rename to crazyflie/scripts/simple_mapper.py index dc5f9791b..967bd3d93 --- a/crazyflie_examples/scripts/simple_mapper.py +++ b/crazyflie/scripts/simple_mapper.py @@ -1,3 +1,5 @@ +#!/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: @@ -28,8 +30,8 @@ class SimpleMapper(Node): def __init__(self): super().__init__('simple_mapper') - self.odom_subscriber = self.create_subscription(Odometry, '/odom', self.odom_subcribe_callback, 10) - self.ranges_subscriber = self.create_subscription(LaserScan, '/scan', self.scan_subsribe_callback, 10) + self.odom_subscriber = self.create_subscription(Odometry, '/cf231/odom', self.odom_subcribe_callback, 10) + self.ranges_subscriber = self.create_subscription(LaserScan, '/cf231/scan', self.scan_subsribe_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] @@ -51,6 +53,9 @@ def __init__(self): self.map_publisher = self.create_publisher(OccupancyGrid, '/map', qos_profile=QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST,)) + self.get_logger().info(f"Simple mapper set for crazyflie"+ + f" using the odom and scan topic") + def odom_subcribe_callback(self, msg): self.position[0] = msg.pose.pose.position.x self.position[1] = msg.pose.pose.position.y diff --git a/crazyflie_examples/CMakeLists.txt b/crazyflie_examples/CMakeLists.txt index 68ed05566..0c70c94d2 100644 --- a/crazyflie_examples/CMakeLists.txt +++ b/crazyflie_examples/CMakeLists.txt @@ -32,10 +32,4 @@ if(BUILD_TESTING) endforeach() endif() -# Install Python executables -install(PROGRAMS - scripts/simple_mapper.py - DESTINATION lib/${PROJECT_NAME} -) - ament_package() diff --git a/crazyflie_examples/launch/multiranger_simple_mapper.py b/crazyflie_examples/launch/multiranger_simple_mapper.py deleted file mode 100644 index 1b7b56d3a..000000000 --- a/crazyflie_examples/launch/multiranger_simple_mapper.py +++ /dev/null @@ -1,46 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node -import yaml - - -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 - - cf_examples_dir = get_package_share_directory('crazyflie_examples') - bringup_dir = get_package_share_directory('nav2_bringup') - bringup_launch_dir = os.path.join(bringup_dir, 'launch') - map_name = 'map' - - return LaunchDescription([ - Node( - package='crazyflie', - executable='crazyflie_server.py', - name='crazyflie_server', - output='screen', - parameters=[{'world_tf_name': 'map'}, - server_params], - ), - Node( - package='crazyflie', - executable='vel_mux.py', - name='vel_mux', - output='screen', - parameters=[{'hover_height': 0.3}, - {'incoming_twist_topic': '/cmd_vel'}, - {'robot_prefix': '/cf231'}] - ) - ]) diff --git a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py index f046d8996..97fc12ba7 100644 --- a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py +++ b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py @@ -35,10 +35,10 @@ def generate_launch_description(): {'incoming_twist_topic': '/cmd_vel'}, {'robot_prefix': '/cf1'}] ), - #Node( - # package='crazyflie_examples', - # executable='simple_mapper.py', - # name='simple mapper', - # output='screen' - #), + Node( + package='crazyflie', + executable='simple_mapper.py', + name='simple_mapper', + output='screen' + ), ]) From 1c81737f10c4c8c65d88a2253dfc12acf138ca22 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Nov 2023 14:49:33 +0100 Subject: [PATCH 23/38] add robot prefix parameters --- crazyflie/scripts/simple_mapper.py | 11 +++++++---- crazyflie/scripts/vel_mux.py | 2 +- crazyflie_examples/launch/keyboard_velmux_launch.py | 4 +++- .../launch/multiranger_simple_mapper_launch.py | 8 ++++++-- 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/crazyflie/scripts/simple_mapper.py b/crazyflie/scripts/simple_mapper.py index 967bd3d93..db7723850 100755 --- a/crazyflie/scripts/simple_mapper.py +++ b/crazyflie/scripts/simple_mapper.py @@ -30,8 +30,11 @@ class SimpleMapper(Node): def __init__(self): super().__init__('simple_mapper') - self.odom_subscriber = self.create_subscription(Odometry, '/cf231/odom', self.odom_subcribe_callback, 10) - self.ranges_subscriber = self.create_subscription(LaserScan, '/cf231/scan', self.scan_subsribe_callback, 10) + 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_subcribe_callback, 10) + self.ranges_subscriber = self.create_subscription(LaserScan, robot_prefix + '/scan', self.scan_subsribe_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] @@ -50,10 +53,10 @@ def __init__(self): 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, '/map', + 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"+ + self.get_logger().info(f"Simple mapper set for crazyflie "+ robot_prefix + f" using the odom and scan topic") def odom_subcribe_callback(self, msg): diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py index f1155ba1c..a9809f6c1 100755 --- a/crazyflie/scripts/vel_mux.py +++ b/crazyflie/scripts/vel_mux.py @@ -20,7 +20,7 @@ class VelMux(Node): def __init__(self): super().__init__('vel_mux') self.declare_parameter('hover_height', 0.5) - self.declare_parameter('robot_prefix', '/cf1') + self.declare_parameter('robot_prefix', '/cf231') self.declare_parameter('incoming_twist_topic', '/cmd_vel') self.hover_height = self.get_parameter('hover_height').value diff --git a/crazyflie_examples/launch/keyboard_velmux_launch.py b/crazyflie_examples/launch/keyboard_velmux_launch.py index 25d318a0b..241348aaa 100644 --- a/crazyflie_examples/launch/keyboard_velmux_launch.py +++ b/crazyflie_examples/launch/keyboard_velmux_launch.py @@ -18,6 +18,8 @@ def generate_launch_description(): server_params = crazyflies + crazyflie_name = '/cf231' + return LaunchDescription([ Node( package='crazyflie', @@ -33,6 +35,6 @@ def generate_launch_description(): output='screen', parameters=[{'hover_height': 0.3}, {'incoming_twist_topic': '/cmd_vel'}, - {'robot_prefix': '/cf1'}] + {'robot_prefix': crazyflie_name}] ), ]) diff --git a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py index 97fc12ba7..0f850410b 100644 --- a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py +++ b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py @@ -18,6 +18,8 @@ def generate_launch_description(): server_params = crazyflies + crazyflie_name = '/cf231' + return LaunchDescription([ Node( package='crazyflie', @@ -33,12 +35,14 @@ def generate_launch_description(): output='screen', parameters=[{'hover_height': 0.3}, {'incoming_twist_topic': '/cmd_vel'}, - {'robot_prefix': '/cf1'}] + {'robot_prefix': crazyflie_name}] ), Node( package='crazyflie', executable='simple_mapper.py', name='simple_mapper', - output='screen' + output='screen', + parameters=[ + {'robot_prefix': crazyflie_name}] ), ]) From ed35c8c0fcf9524bcf9a45af325ee5f92d78a548 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 20 Nov 2023 14:52:13 +0100 Subject: [PATCH 24/38] revert velmux changes, will do later --- crazyflie/scripts/vel_mux.py | 2 +- crazyflie_examples/launch/keyboard_velmux_launch.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py index a9809f6c1..b89706ac5 100755 --- a/crazyflie/scripts/vel_mux.py +++ b/crazyflie/scripts/vel_mux.py @@ -20,7 +20,7 @@ class VelMux(Node): def __init__(self): super().__init__('vel_mux') self.declare_parameter('hover_height', 0.5) - self.declare_parameter('robot_prefix', '/cf231') + self.declare_parameter('robot_prefix', '/cf') self.declare_parameter('incoming_twist_topic', '/cmd_vel') self.hover_height = self.get_parameter('hover_height').value diff --git a/crazyflie_examples/launch/keyboard_velmux_launch.py b/crazyflie_examples/launch/keyboard_velmux_launch.py index 241348aaa..06b395ad4 100644 --- a/crazyflie_examples/launch/keyboard_velmux_launch.py +++ b/crazyflie_examples/launch/keyboard_velmux_launch.py @@ -35,6 +35,6 @@ def generate_launch_description(): output='screen', parameters=[{'hover_height': 0.3}, {'incoming_twist_topic': '/cmd_vel'}, - {'robot_prefix': crazyflie_name}] + {'robot_prefix': '/cf1'}] ), ]) From 7b1f1a3143498e5c55bfad0c318d02c9edcb82a0 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 21 Nov 2023 13:42:24 +0100 Subject: [PATCH 25/38] fixed duration issue --- crazyflie/scripts/crazyflie_server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 138dc6bf8..7f569e0bd 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -817,7 +817,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"): py = Poly4D.Poly(piece.poly_y) pz = Poly4D.Poly(piece.poly_z) pyaw = Poly4D.Poly(piece.poly_yaw) - duration = piece.duration.sec + duration = float(piece.duration.sec) + float(piece.duration.nanosec)/1e9 trajectory.append(Poly4D(duration, px, py, pz, pyaw )) total_duration = total_duration + duration From ca6152a8d71f78c228f56e7aba1dd39d8c41b079 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 21 Nov 2023 13:43:53 +0100 Subject: [PATCH 26/38] update overview --- docs2/overview.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs2/overview.rst b/docs2/overview.rst index 7383949ee..655afd9c6 100644 --- a/docs2/overview.rst +++ b/docs2/overview.rst @@ -87,7 +87,7 @@ Support functionality with backends +---------------------+---------+-----------+---------+ | - go_to | Yes | Yes | Yes | +---------------------+---------+-----------+---------+ -| - upload/start traj | Yes | No | Yes | +| - upload/start traj | Yes | Yes | Yes | +---------------------+---------+-----------+---------+ | Positioning System | +---------------------+---------+-----------+---------+ From 36cb38dc1f171a50fdb634121f6f9ea7a4f46201 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 21 Nov 2023 13:55:14 +0100 Subject: [PATCH 27/38] autopep8 format --- crazyflie/scripts/crazyflie_server.py | 186 +++++++++++++++----------- 1 file changed, 107 insertions(+), 79 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 7f569e0bd..448405256 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -54,6 +54,7 @@ "double": ParameterType.PARAMETER_DOUBLE, } + class CrazyflieServer(Node): def __init__(self): super().__init__( @@ -69,21 +70,21 @@ def __init__(self): self.cf_dict = {} self.uri_dict = {} self.type_dict = {} - + # Assign default topic types, variables and callbacks self.default_log_type = {"pose": PoseStamped, - "scan": LaserScan, - "odom": Odometry} + "scan": LaserScan, + "odom": Odometry} 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']} + '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']} self.default_log_fnc = {"pose": self._log_pose_data_callback, - "scan": self._log_scan_data_callback, - "odom": self._log_odom_data_callback} + "scan": self._log_scan_data_callback, + "odom": self._log_odom_data_callback} self.world_tf_name = "world" try: @@ -100,7 +101,8 @@ def __init__(self): 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") + connection = self._ros_parameters['robot_types'][type_cf].get( + "connection", "crazyflie") if connection == "crazyflie": uri = robot_data[crazyflie]["uri"] self.uris.append(uri) @@ -153,7 +155,8 @@ def __init__(self): 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) + self._init_default_logblocks( + prefix, link_uri, list_logvar, logging_enabled, topic_type) # Check for any custom_log topics custom_logging_enabled = False @@ -207,13 +210,14 @@ def __init__(self): 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() - + # Create services for the entire swarm and each individual crazyflie self.create_service(Empty, "all/emergency", self._emergency_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) + self.create_service( + StartTrajectory, "all/start_trajectory", self._start_trajectory_callback) for uri in self.cf_dict: name = self.cf_dict[uri] @@ -232,28 +236,35 @@ def __init__(self): 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) + 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) + 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) + 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 + "/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed, + uri=uri), 10 ) self.create_subscription( Hover, name + "/cmd_hover", partial(self._cmd_hover_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)) + 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", + NamedPoseArray, "/poses", self._poses_changed, qos_profile ) @@ -263,7 +274,7 @@ def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_ """ cf_name = self.cf_dict[link_uri] cf_type = self.type_dict[link_uri] - + logging_enabled = False logging_freq = 10 try: @@ -293,8 +304,10 @@ def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_ 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 + + "_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( @@ -356,7 +369,7 @@ def _init_logging(self): 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"]: @@ -408,11 +421,11 @@ def _init_default_logging(self, prefix, link_uri, callback_fnc): f"{link_uri} setup logging for {prefix} at freq {frequency}") except KeyError as e: self.get_logger().info(f'{link_uri}: Could not start log configuration,' - '{} not found in TOC'.format(str(e))) + '{} not found in TOC'.format(str(e))) except AttributeError: self.get_logger().info( f'{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, @@ -431,7 +444,7 @@ def _log_scan_data_callback(self, timestamp, data, logconf, uri): if right_range > max_range: right_range = float("inf") if back_range > max_range: - back_range = float("inf") + back_range = float("inf") self.ranges = [back_range, right_range, front_range, left_range] msg = LaserScan() @@ -440,8 +453,8 @@ def _log_scan_data_callback(self, timestamp, data, logconf, uri): 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_min = -0.5 * 2 * pi + msg.angle_max = 0.25 * 2 * pi msg.angle_increment = 1.0 * pi/2 self.swarm._cfs[uri].logging["scan_publisher"].publish(msg) @@ -572,10 +585,11 @@ def _init_parameters(self): for param in sorted(p_toc[group].keys()): name = group + "." + param - # Check the parameter type + # Check the parameter type elem = p_toc[group][param] type_cf_param = elem.ctype - parameter_descriptor = ParameterDescriptor(type=cf_log_to_ros_param[type_cf_param]) + parameter_descriptor = ParameterDescriptor( + type=cf_log_to_ros_param[type_cf_param]) # Check ros parameters if an parameter should be set # Parameter sets for individual robots has priority, @@ -586,11 +600,13 @@ def _init_parameters(self): except KeyError: pass try: - set_param_value = self._ros_parameters["robot_types"][self.cf_dict[link_uri]]["firmware_params"][group][param] + set_param_value = self._ros_parameters["robot_types"][self.cf_dict[link_uri] + ]["firmware_params"][group][param] except KeyError: pass try: - set_param_value = self._ros_parameters["robots"][self.cf_dict[link_uri]]["firmware_params"][group][param] + set_param_value = self._ros_parameters["robots"][self.cf_dict[link_uri] + ]["firmware_params"][group][param] except KeyError: pass @@ -637,7 +653,7 @@ def _init_parameters(self): descriptor=parameter_descriptor, ) - # Now all parameters are set + # Now all parameters are set set_param_all = True self.get_logger().info("All Crazyflies parameters are initialized") @@ -673,8 +689,8 @@ def _parameters_callback(self, params): try: for link_uri in self.uris: cf = self.swarm._cfs[link_uri].cf.param.set_value( - name_param, param.value - ) + name_param, param.value + ) self.get_logger().info( f" {link_uri}: {name_param} is set to {param.value}" ) @@ -682,9 +698,9 @@ def _parameters_callback(self, params): except Exception as e: self.get_logger().info(str(e)) return SetParametersResult(successful=False) - + return SetParametersResult(successful=False) - + def _emergency_callback(self, request, response, uri="all"): if uri == "all": for link_uri in self.uris: @@ -792,23 +808,23 @@ def _notify_setpoints_stop_callback(self, request, response, uri="all"): if uri == "all": for link_uri in self.uris: - self.swarm._cfs[link_uri].cf.commander.send_notify_setpoint_stop() + 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("upload_trajectory(id=%d,offset=%d, lenght=%d)"% ( - id, - offset, - lenght, - )) + self.get_logger().info("upload_trajectory(id=%d,offset=%d, lenght=%d)" % ( + id, + offset, + lenght, + )) trajectory = [] for i in range(lenght): @@ -817,59 +833,66 @@ def _upload_trajectory_callback(self, request, response, uri="all"): 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 )) + 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)[id] + trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems( + MemoryElement.TYPE_TRAJ)[id] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: self.get_logger().info(f"{link_uri}: Upload failed") upload_success_all = False - else: - self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory(id, offset, len(trajectory)) + 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)[id] + trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems( + MemoryElement.TYPE_TRAJ)[id] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: self.get_logger().info(f"{uri}: Upload failed") response.success = False return response - self.swarm._cfs[uri].cf.high_level_commander.define_trajectory(id, offset, len(trajectory)) + 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("start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)"% ( - id, - ts, - rel, - rev, - gm - )) + self.get_logger().info("start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" % ( + 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) + 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) + 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 @@ -887,7 +910,7 @@ def _poses_changed(self, msg): 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}") + # 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) @@ -895,7 +918,6 @@ def _poses_changed(self, msg): 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 @@ -917,8 +939,10 @@ def _cmd_hover_changed(self, msg, uri=""): 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) - self.get_logger().info(f"{uri}: Received hover topic {vx} {vy} {yawrate} {z}") + self.swarm._cfs[uri].cf.commander.send_hover_setpoint( + vx, vy, yawrate, z) + self.get_logger().info( + f"{uri}: Received hover topic {vx} {vy} {yawrate} {z}") def _remove_logging(self, request, response, uri="all"): """ @@ -968,8 +992,10 @@ def _add_logging(self, request, response, uri="all"): 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.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"{uri}: Add {topic_name} logging") except rclpy.exceptions.ParameterAlreadyDeclaredException: self.get_logger().info( @@ -988,7 +1014,7 @@ def _add_logging(self, request, response, uri="all"): 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( @@ -1006,8 +1032,10 @@ def _add_logging(self, request, response, uri="all"): self.get_logger().info( f"{uri}: Failed to add {topic_name} logging") self.get_logger().info(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.") + 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: @@ -1018,7 +1046,7 @@ def _add_logging(self, request, response, uri="all"): response.success = True return response - + def main(args=None): From ff7ac0fadd3a4a2b42aeacdeb0969576e155d288 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 21 Nov 2023 13:57:30 +0100 Subject: [PATCH 28/38] fix doc --- docs2/overview.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs2/overview.rst b/docs2/overview.rst index 655afd9c6..7cb09506e 100644 --- a/docs2/overview.rst +++ b/docs2/overview.rst @@ -87,7 +87,7 @@ Support functionality with backends +---------------------+---------+-----------+---------+ | - go_to | Yes | Yes | Yes | +---------------------+---------+-----------+---------+ -| - upload/start traj | Yes | Yes | Yes | +| - upload/start traj | Yes | Yes | Yes | +---------------------+---------+-----------+---------+ | Positioning System | +---------------------+---------+-----------+---------+ From 098691d38e452e3808024937ab016ddf35ca3b4b Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Nov 2023 14:49:15 +0100 Subject: [PATCH 29/38] fix mapping issue wrong order ranges --- crazyflie/scripts/simple_mapper.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crazyflie/scripts/simple_mapper.py b/crazyflie/scripts/simple_mapper.py index db7723850..f0f316e78 100755 --- a/crazyflie/scripts/simple_mapper.py +++ b/crazyflie/scripts/simple_mapper.py @@ -109,9 +109,9 @@ def rotate_and_create_points(self): pitch = self.angles[1] yaw = self.angles[2] r_back = self.ranges[0] - r_left = self.ranges[1] + r_right = self.ranges[1] r_front = self.ranges[2] - r_right = self.ranges[3] + 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]] From 02b70523ad3890ab7ebd50a45da1fb5edb63696e Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Nov 2023 14:52:21 +0100 Subject: [PATCH 30/38] change file name simple mapper --- crazyflie/CMakeLists.txt | 2 +- .../scripts/{simple_mapper.py => simple_mapper_multiranger.py} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename crazyflie/scripts/{simple_mapper.py => simple_mapper_multiranger.py} (100%) diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index 828cd9a44..e6bf91d0a 100644 --- a/crazyflie/CMakeLists.txt +++ b/crazyflie/CMakeLists.txt @@ -106,7 +106,7 @@ install(PROGRAMS scripts/chooser.py scripts/vel_mux.py scripts/cfmult.py - scripts/simple_mapper.py + scripts/simple_mapper_multiranger.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/crazyflie/scripts/simple_mapper.py b/crazyflie/scripts/simple_mapper_multiranger.py similarity index 100% rename from crazyflie/scripts/simple_mapper.py rename to crazyflie/scripts/simple_mapper_multiranger.py From 2386488754be1c82f97bd9971171b65b273adae3 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Nov 2023 14:54:09 +0100 Subject: [PATCH 31/38] simple mappr function name change --- crazyflie/scripts/simple_mapper_multiranger.py | 8 ++++---- .../launch/multiranger_simple_mapper_launch.py | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/crazyflie/scripts/simple_mapper_multiranger.py b/crazyflie/scripts/simple_mapper_multiranger.py index f0f316e78..879f551ec 100755 --- a/crazyflie/scripts/simple_mapper_multiranger.py +++ b/crazyflie/scripts/simple_mapper_multiranger.py @@ -27,9 +27,9 @@ GLOBAL_SIZE_Y = 20.0 MAP_RES = 0.1 -class SimpleMapper(Node): +class SimpleMapperMultiranger(Node): def __init__(self): - super().__init__('simple_mapper') + super().__init__('simple_mapper_multiranger') self.declare_parameter('robot_prefix', '/cf231') robot_prefix = self.get_parameter('robot_prefix').value @@ -163,8 +163,8 @@ def rot(self, roll, pitch, yaw, origin, point): def main(args=None): rclpy.init(args=args) - simple_mapper = SimpleMapper() - rclpy.spin(simple_mapper) + simple_mapper_multiranger = SimpleMapperMultiranger() + rclpy.spin(simple_mapper_multiranger) rclpy.destroy_node() rclpy.shutdown() diff --git a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py index 0f850410b..d8e4b4952 100644 --- a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py +++ b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py @@ -39,8 +39,8 @@ def generate_launch_description(): ), Node( package='crazyflie', - executable='simple_mapper.py', - name='simple_mapper', + executable='simple_mapper_multiranger.py', + name='simple_mapper_multiranger', output='screen', parameters=[ {'robot_prefix': crazyflie_name}] From 93668a3b53b2e597dacbf655bea924698f21bd87 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Nov 2023 14:55:28 +0100 Subject: [PATCH 32/38] fix callback names typo --- crazyflie/scripts/simple_mapper_multiranger.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/crazyflie/scripts/simple_mapper_multiranger.py b/crazyflie/scripts/simple_mapper_multiranger.py index 879f551ec..afd4ef851 100755 --- a/crazyflie/scripts/simple_mapper_multiranger.py +++ b/crazyflie/scripts/simple_mapper_multiranger.py @@ -33,8 +33,8 @@ def __init__(self): 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_subcribe_callback, 10) - self.ranges_subscriber = self.create_subscription(LaserScan, robot_prefix + '/scan', self.scan_subsribe_callback, 10) + 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] @@ -59,7 +59,7 @@ def __init__(self): self.get_logger().info(f"Simple mapper set for crazyflie "+ robot_prefix + f" using the odom and scan topic") - def odom_subcribe_callback(self, msg): + 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 @@ -70,7 +70,7 @@ def odom_subcribe_callback(self, msg): self.angles[2] = euler[2] self.position_update = True - def scan_subsribe_callback(self, msg): + def scan_subscribe_callback(self, msg): self.ranges = msg.ranges self.range_max = msg.range_max data = self.rotate_and_create_points() From b466f821667e401e6e2fef9e85e02f4895bd3bbb Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Nov 2023 14:55:54 +0100 Subject: [PATCH 33/38] autopep8 --- .../scripts/simple_mapper_multiranger.py | 81 ++++++++++--------- 1 file changed, 44 insertions(+), 37 deletions(-) diff --git a/crazyflie/scripts/simple_mapper_multiranger.py b/crazyflie/scripts/simple_mapper_multiranger.py index afd4ef851..096b6126a 100755 --- a/crazyflie/scripts/simple_mapper_multiranger.py +++ b/crazyflie/scripts/simple_mapper_multiranger.py @@ -27,16 +27,19 @@ 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] + 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 @@ -52,13 +55,14 @@ def __init__(self): self.position_update = False - self.map = [-1] * int(GLOBAL_SIZE_X / MAP_RES) * int(GLOBAL_SIZE_Y / MAP_RES) + 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,)) + 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 + + 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 @@ -81,15 +85,17 @@ def scan_subscribe_callback(self, msg): 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 ) + 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 ) + 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 + 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() @@ -132,33 +138,34 @@ def rotate_and_create_points(self): return data def rot(self, roll, pitch, yaw, origin, point): - cosr = math.cos((roll)) - cosp = math.cos((pitch)) - cosy = math.cos((yaw)) + cosr = math.cos((roll)) + cosp = math.cos((pitch)) + cosy = math.cos((yaw)) + + sinr = math.sin((roll)) + sinp = math.sin((pitch)) + siny = math.sin((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]]) - roty = np.array([[cosy, -siny, 0], - [siny, cosy, 0], - [0, 0, 1]]) + rotp = np.array([[cosp, 0, sinp], + [0, 1, 0], + [-sinp, 0, cosp]]) - rotp = np.array([[cosp, 0, sinp], - [0, 1, 0], - [-sinp, 0, cosp]]) + rotr = np.array([[1, 0, 0], + [0, cosr, -sinr], + [0, sinr, cosr]]) - rotr = np.array([[1, 0, 0], - [0, cosr, -sinr], - [0, sinr, cosr]]) + rotFirst = np.dot(rotr, rotp) - rotFirst = np.dot(rotr, rotp) + rot = np.array(np.dot(rotFirst, roty)) - rot = np.array(np.dot(rotFirst, roty)) + tmp = np.subtract(point, origin) + tmp2 = np.dot(rot, tmp) + return np.add(tmp2, origin) - tmp = np.subtract(point, origin) - tmp2 = np.dot(rot, tmp) - return np.add(tmp2, origin) def main(args=None): @@ -170,4 +177,4 @@ def main(args=None): if __name__ == '__main__': - main() \ No newline at end of file + main() From 7019b14203a6bbb1012c53d0c1716da90d1dc3da Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Nov 2023 14:56:20 +0100 Subject: [PATCH 34/38] revert change velmux --- crazyflie_examples/launch/keyboard_velmux_launch.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/crazyflie_examples/launch/keyboard_velmux_launch.py b/crazyflie_examples/launch/keyboard_velmux_launch.py index 06b395ad4..25d318a0b 100644 --- a/crazyflie_examples/launch/keyboard_velmux_launch.py +++ b/crazyflie_examples/launch/keyboard_velmux_launch.py @@ -18,8 +18,6 @@ def generate_launch_description(): server_params = crazyflies - crazyflie_name = '/cf231' - return LaunchDescription([ Node( package='crazyflie', From b20230ab4b69a6948ab63ee497d861affd350c89 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Nov 2023 15:05:47 +0100 Subject: [PATCH 35/38] add doc about the example --- docs2/tutorials.rst | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/docs2/tutorials.rst b/docs2/tutorials.rst index bb550ce1c..f56d62f75 100644 --- a/docs2/tutorials.rst +++ b/docs2/tutorials.rst @@ -87,6 +87,44 @@ Here you can see an example of 5 crazyflies with the Pose default topic enabled, +Mapping with simple mapper +-------------------------- + +If you have a crazyflie with a multiranger and flowdeck, you can try out some simple mapping. + +Make sure that the scan and odometry logging is enabled in crazyflies.yaml: + +.. code-block:: bash + + firmware_logging: + enabled: true + default_topics: + odom: + frequency: 10 # Hz + scan: + frequency: 10 # Hz + +and make sure that the pid controller and kalman filter is enabled: + +.. code-block:: bash + + firmware_params: + stabilizer: + estimator: 2 # 1: complementary, 2: kalman + controller: 1 # 1: PID, 2: mellinger + +If you are using a different name for your crazyflie, make sure to change the following in the example launch file (multiranger_simple_mapper_launch.py): + +.. code-block:: bash + crazyflie_name = '/cf231' + +Then start the simple mapper example launch file: + +.. code-block:: bash + ros2 launch crazyflie_examples multiranger_simple_mapper_launch.py + +And watch the mapping happening in rviz2 while controlling the crazyflie with the teleop node (see the sections above). + Mapping with the SLAM toolbox ----------------------------- From 99ccd67c875ece9fe4945dbacb89c196b2740bc5 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 23 Nov 2023 15:08:33 +0100 Subject: [PATCH 36/38] fix doc issues --- docs2/tutorials.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs2/tutorials.rst b/docs2/tutorials.rst index f56d62f75..84281cc81 100644 --- a/docs2/tutorials.rst +++ b/docs2/tutorials.rst @@ -116,11 +116,13 @@ and make sure that the pid controller and kalman filter is enabled: If you are using a different name for your crazyflie, make sure to change the following in the example launch file (multiranger_simple_mapper_launch.py): .. code-block:: bash + crazyflie_name = '/cf231' Then start the simple mapper example launch file: .. code-block:: bash + ros2 launch crazyflie_examples multiranger_simple_mapper_launch.py And watch the mapping happening in rviz2 while controlling the crazyflie with the teleop node (see the sections above). From 9050b3790a13f51e4d44f37d804c012355659b9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Wolfgang=20H=C3=B6nig?= Date: Fri, 1 Dec 2023 06:27:33 +0100 Subject: [PATCH 37/38] add system tests Add infrastructure for continuous integration tests that run on physical hardware (to be manually triggered via github actions; requires a custom github runner with access to the robot hardware). This initial change runs two of the example scripts and uploads a resulting PDF with the trajectory tracking results. Co-authored-by: JulienT hevenoz --- .github/workflows/systemtests.yml | 68 ++++++ systemtests/mcap_handler.py | 64 ++++++ systemtests/plotter_class.py | 343 ++++++++++++++++++++++++++++++ systemtests/run_test.py | 119 +++++++++++ 4 files changed, 594 insertions(+) create mode 100644 .github/workflows/systemtests.yml create mode 100644 systemtests/mcap_handler.py create mode 100644 systemtests/plotter_class.py create mode 100755 systemtests/run_test.py diff --git a/.github/workflows/systemtests.yml b/.github/workflows/systemtests.yml new file mode 100644 index 000000000..21588070b --- /dev/null +++ b/.github/workflows/systemtests.yml @@ -0,0 +1,68 @@ +name: System Tests + +on: + push: + branches: [ "feature_systemtests" ] + # manual trigger + workflow_dispatch: + +jobs: + build: + runs-on: self-hosted + 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 + + - name: Flight test + id: step5 + run: | + cd ros2_ws + source /opt/ros/humble/setup.bash + . install/local_setup.bash + export ROS_LOCALHOST_ONLY=1 + python3 src/crazyswarm2/systemtests/run_test.py + + - name: Upload files + id: step6 + uses: actions/upload-artifact@v3 + with: + name: pdf_bagfiles_and_logs + path: | + ros2_ws/bagfiles + ros2_ws/results + ~/.ros/log + + + + + + # - 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 \ No newline at end of file diff --git a/systemtests/mcap_handler.py b/systemtests/mcap_handler.py new file mode 100644 index 000000000..de15911c1 --- /dev/null +++ b/systemtests/mcap_handler.py @@ -0,0 +1,64 @@ +from pathlib import Path +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message +from std_msgs.msg import String +import rosbag2_py +import csv + +class McapHandler: + def __init__(self): + pass + + def read_messages(self, input_bag: str): + reader = rosbag2_py.SequentialReader() + reader.open( + rosbag2_py.StorageOptions(uri=input_bag, storage_id="mcap"), + rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ), + ) + topic_types = reader.get_all_topics_and_types() + + def typename(topic_name): + for topic_type in topic_types: + if topic_type.name == topic_name: + return topic_type.type + raise ValueError(f"topic {topic_name} not in bag") + + while reader.has_next(): + topic, data, timestamp = reader.read_next() + msg_type = get_message(typename(topic)) + msg = deserialize_message(data, msg_type) + yield topic, msg, timestamp + del reader + + def write_mcap_to_csv(self, inputbag:str, outputfile:str): + '''A method which translates an .mcap rosbag file format to a .csv file. + Only written to translate the /tf topic but could easily be extended to other topics''' + + try: + print("Translating .mcap to .csv") + f = open(outputfile, 'w+') + writer = csv.writer(f) + for topic, msg, timestamp in self.read_messages(inputbag): + writer.writerow([timestamp, msg.transforms[0].transform.translation.x, msg.transforms[0].transform.translation.y, msg.transforms[0].transform.translation.z]) + f.close() + except FileNotFoundError: + print(f"McapHandler : file {outputfile} not found") + exit(1) + + + + +if __name__ == "__main__": + + #command line utility + + from argparse import ArgumentParser, Namespace + parser = ArgumentParser(description="Translates the /tf topic of an .mcap rosbag file format to a .csv file") + parser.add_argument("inputbag", type=str, help="The .mcap rosbag file to be translated") + parser.add_argument("outputfile", type=str, help="Output csv file that has to be created/overwritten") + args:Namespace = parser.parse_args() + + translator = McapHandler() + translator.write_mcap_to_csv(args.inputbag,args.outputfile) diff --git a/systemtests/plotter_class.py b/systemtests/plotter_class.py new file mode 100644 index 000000000..beca43e33 --- /dev/null +++ b/systemtests/plotter_class.py @@ -0,0 +1,343 @@ +import matplotlib.pyplot as plt +import os +import sys +from matplotlib.backends.backend_pdf import PdfPages +import numpy as np +from crazyflie_py.uav_trajectory import Trajectory + + +class Plotter: + + def __init__(self): + self.params = {'experiment':'1','trajectory':'figure8.csv','motors':'standard motors(CF)', 'propellers':'standard propellers(CF)'} + self.bag_times = np.empty([0]) + self.bag_x = np.empty([0]) + self.bag_y = np.empty([0]) + self.bag_z = np.empty([0]) + self.ideal_traj_x = np.empty([0]) + self.ideal_traj_y = np.empty([0]) + self.ideal_traj_z = np.empty([0]) + self.euclidian_dist = np.empty([0]) + self.deviation = [] #list of all indexes where euclidian_distance(ideal - recorded) > EPSILON + + self.EPSILON = 0.05 # euclidian distance in [m] between ideal and recorded trajectory under which the drone has to stay to pass the test + self.DELAY_CONST_FIG8 = 4.75 #this is the delay constant which I found by adding up all the time.sleep() etc in the figure8.py file. This should be implemented better later + self.ALTITUDE_CONST_FIG8 = 1.0 #this is the altitude given for the takeoff in figure8.py. I should find a better solution than a symbolic constant ? + self.ALTITUDE_CONST_MULTITRAJ = 1 #takeoff altitude for traj0 in multi_trajectory.py + + def file_guard(self, pdf_path): + msg = None + if os.path.exists(pdf_path): + msg = input("file already exists, overwrite? [y/n]: ") + if msg == "n": + print("exiting...") + sys.exit(0) + elif msg == "y": + print("overwriting...") + os.remove(pdf_path) + else: + print("invalid msg...") + self.file_guard(pdf_path) + return + + + + def read_csv_and_set_arrays(self, ideal_csvfile, rosbag_csvfile): + '''Method that reads the csv data of the ideal test trajectory and of the actual recorded trajectory and initializes the attribute arrays''' + + #check which test we are plotting : figure8 or multi_trajectory + if("fig8" in rosbag_csvfile): + fig8 = True + print("Plotting fig8 test data") + else: + fig8 = False + print("Plotting multi_trajectory test data") + + #get ideal trajectory data + self.ideal_traj_csv = Trajectory() + try: + path_to_ideal_csv = os.path.join(os.path.dirname(os.path.abspath(__file__)),ideal_csvfile) + self.ideal_traj_csv.loadcsv(path_to_ideal_csv) + except FileNotFoundError: + print("Plotter : File not found " + path_to_ideal_csv) + exit(1) + + + #get rosbag data + rosbag_data = np.loadtxt(rosbag_csvfile, delimiter=",") + + self.bag_times = np.array(rosbag_data[:,0]) + self.bag_x = np.array(rosbag_data[:,1]) + self.bag_y = np.array(rosbag_data[:,2]) + self.bag_z = np.array(rosbag_data[:,3]) + + self.adjust_arrays() + bag_arrays_size = len(self.bag_times) + print("number of datapoints in self.bag_*: ",bag_arrays_size) + + #####calculate ideal trajectory points corresponding to the times of recorded points + + self.ideal_traj_x = np.empty([bag_arrays_size]) + self.ideal_traj_y = np.empty([bag_arrays_size]) + self.ideal_traj_z = np.empty([bag_arrays_size]) + self.euclidian_dist = np.empty([bag_arrays_size]) + + + no_match_in_idealcsv=[] + + for i in range(bag_arrays_size): + try: + pos = self.ideal_traj_csv.eval(self.bag_times[i] - self.DELAY_CONST_FIG8).pos + except AssertionError: + no_match_in_idealcsv.append(i) + pos = [0,0,0] #for all recorded datapoints who cannot be matched to a corresponding ideal position we assume the drone is on its ground start position (ie those datapoints are before takeoff or after landing) + + + self.ideal_traj_x[i], self.ideal_traj_y[i]= pos[0], pos[1] + if(fig8) : #special case : in fig8 test no altitude is given in the trajectory polynomials but is stays constant after takeoff in figure8.py + self.ideal_traj_z[i] = self.ALTITUDE_CONST_FIG8 + else : #for multi_trajectory test the altitude given in the trajectory polynomials is added to the takeoff altitude + self.ideal_traj_z[i] = self.ALTITUDE_CONST_MULTITRAJ + pos[2] + + self.euclidian_dist[i] = np.linalg.norm([self.ideal_traj_x[i]-self.bag_x[i], + self.ideal_traj_y[i]-self.bag_y[i], self.ideal_traj_z[i]-self.bag_z[i]]) + if self.euclidian_dist[i] > self.EPSILON: + self.deviation.append(i) + + self.no_match_warning(no_match_in_idealcsv) + + + def no_match_warning(self, unmatched_values:list): + ''' A method which prints a warning saying how many (if any) recorded datapoints could not be matched to an ideal datapoint''' + + no_match_arr = np.array(unmatched_values) + + if no_match_arr.size == 0: + return + + split_index_arr = [] + + for i in range(no_match_arr.size - 1): #find indexes which are not consecutive + if(no_match_arr[i+1] != no_match_arr[i]+1): + split_index_arr.append(i+1) + + banana_split = np.split(no_match_arr, split_index_arr) #split array into sub-array of consecutive indexes -> each sub-array is a timerange for which ideal positions weren't able to calculated + print(f"{len(no_match_arr)} recorded positions weren't able to be matched with a specified ideal position and were given the default (0,0,0) ideal position instead.") + print("Probable reason : their timestamp is before the start of the ideal trajectory or after its end.") + if len(banana_split)==2: + timerange1_start = self.bag_times[banana_split[0][0]] + timerange1_end= self.bag_times[banana_split[0][1]] + timerange2_start = self.bag_times[banana_split[1][0]] + timerange2_end = self.bag_times[banana_split[1][1]] + print(f"These datapoints correspond to the time ranges [{timerange1_start} , {timerange1_end}] and [{timerange2_start} , {timerange2_end}]") + + + + def adjust_arrays(self): + ''' Method that trims the self.bag_* attributes to get rid of the datapoints where the drone is immobile on the ground and makes self.bag_times start at 0 [s]''' + + print(f"rosbag initial length {(self.bag_times[-1]-self.bag_times[0]) * 10**-9}s") + + #find the takeoff time and landing times + ground_level = self.bag_z[0] + airborne = False + takeoff_index = None + landing_index = None + i=0 + for z_coord in self.bag_z: + if not(airborne) and z_coord > ground_level + ground_level*(0.1): #when altitude of the drone is 10% higher than the ground level, it started takeoff + takeoff_index = i + takeoff_time = self.bag_times[takeoff_index] + airborne = True + print(f"takeof time is {(takeoff_time-self.bag_times[0]) * 10**-9}") + if airborne and z_coord < ground_level + ground_level*(0.1): #find when it lands again + landing_index = i + landing_time = self.bag_times[landing_index] + print(f"landing time is {(landing_time-self.bag_times[0]) * 10**-9}") + break + i+=1 + + assert (takeoff_index != None) and (landing_index != None), "Plotter : couldn't find drone takeoff or landing" + + + ####get rid of datapoints before takeoff and after landing in bag_times, bag_x, bag_y, bag_y + + assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "Plotter : self.bag_* aren't the same size before trimming" + index_arr = np.arange(len(self.bag_times)) + slicing_arr = np.delete(index_arr, index_arr[takeoff_index:landing_index+1]) #in our slicing array we take out all the indexes of when the drone is in flight so that it only contains the indexes of when the drone is on the ground + + #delete the datapoints where drone is on the ground + self.bag_times = np.delete(self.bag_times, slicing_arr) + self.bag_x = np.delete(self.bag_x, slicing_arr) + self.bag_y = np.delete(self.bag_y, slicing_arr) + self.bag_z = np.delete(self.bag_z, slicing_arr) + + assert len(self.bag_times) == len(self.bag_x) == len(self.bag_y) == len(self.bag_z), "Plotter : self.bag_* aren't the same size after trimming" + + #rewrite bag_times to start at 0 and be written in [s] instead of [ns] + bag_start_time = self.bag_times[0] + self.bag_times = (self.bag_times-bag_start_time) * (10**-9) + assert self.bag_times[0] == 0 + print(f"trimmed bag_times starts: {self.bag_times[0]}s and ends: {self.bag_times[-1]}, size: {len(self.bag_times)}") + + + + def create_figures(self, ideal_csvfile:str, rosbag_csvfile:str, pdfname:str): + '''Method that creates the pdf with the plots''' + + self.read_csv_and_set_arrays(ideal_csvfile,rosbag_csvfile) + + passed="failed" + if self.test_passed(): + passed = "passed" + + #create PDF to save figures + if(pdfname[-4:] != ".pdf"): + pdfname= pdfname + '.pdf' + + #check if user wants to overwrite the report file + self.file_guard(pdfname) + pdf_pages = PdfPages(pdfname) + + #create title page + if "figure8" in ideal_csvfile: + name = "Figure8" + elif "multi_trajectory" in ideal_csvfile: + name = "Multi_trajectory" + else: + name = "Unnamed test" + print("Plotter : test name not defined") + + text = f'{name} trajectory test' + title_text_settings = f'Settings:\n' + title_text_parameters = f'Parameters:\n' + for key, value in self.params.items(): + title_text_parameters += f" {key}: {value}\n" + title_text_results = f'Results: test {passed}\n' + f'max error : ' + + title_text = text + "\n" + title_text_settings + "\n" + title_text_parameters + "\n" + title_text_results + fig = plt.figure(figsize=(5,8)) + fig.text(0.1, 0.1, title_text, size=11) + + + pdf_pages.savefig(fig) + + + #create plots + fig, ax = plt.subplots() + ax.plot(self.bag_times, self.ideal_traj_x, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10) + ax.plot(self.bag_times, self.bag_x, label='Recorded trajectory') + ax.set_xlabel('time [s]') + ax.set_ylabel('x position [m]') + ax.set_title("Trajectory x") + + ax.grid(which='major', color='#DDDDDD', linewidth=0.8) + ax.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5) + ax.minorticks_on() + fig.tight_layout(pad = 4) + fig.legend() + + pdf_pages.savefig(fig) + + fig2, ax2 = plt.subplots() + ax2.plot(self.bag_times, self.ideal_traj_y, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10) + ax2.plot(self.bag_times, self.bag_y, label='Recorded trajectory') + ax2.set_xlabel('time [s]') + ax2.set_ylabel('y position [m]') + ax2.set_title("Trajectory y") + ax2.grid(which='major', color='#DDDDDD', linewidth=0.8) + ax2.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5) + ax2.minorticks_on() + fig2.tight_layout(pad = 4) + fig2.legend() + pdf_pages.savefig(fig2) + + + fig3, ax3 = plt.subplots() + ax3.plot(self.bag_times, self.ideal_traj_z, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10) + ax3.plot(self.bag_times, self.bag_z, label='Recorded trajectory') + ax3.set_xlabel('time [s]') + ax3.set_ylabel('z position [m]') + ax3.set_title("Trajectory z") + ax3.grid(which='major', color='#DDDDDD', linewidth=0.8) + ax3.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5) + ax3.minorticks_on() + fig3.tight_layout(pad = 4) + fig3.legend() + pdf_pages.savefig(fig3) + + fig4, ax4 = plt.subplots() + ax4.plot(self.bag_times, self.euclidian_dist) + ax4.set_xlabel('time [s]') + ax4.set_ylabel('Euclidean distance [m]') + ax4.set_title('Deviation between ideal and recorded trajectories') + fig4.tight_layout(pad=4) + ax4.grid(which='major', color='#DDDDDD', linewidth=0.8) + ax4.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5) + ax4.minorticks_on() + pdf_pages.savefig(fig4) + + + fig5,ax5 = plt.subplots() + ax5.plot(self.ideal_traj_x, self.ideal_traj_y, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10) + ax5.plot(self.bag_x, self.bag_y, label='Recorded trajectory') + ax5.set_xlabel('x [m]') + ax5.set_ylabel('y [m]') + ax5.set_title('2D visualization') + fig5.tight_layout(pad=4) + ax5.grid(which='major', color='#DDDDDD', linewidth=0.8) + ax5.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5) + ax5.minorticks_on() + pdf_pages.savefig(fig5) + + + fig6 = plt.figure() + ax6 = fig6.add_subplot(projection="3d") + ax6.plot3D(self.ideal_traj_x,self.ideal_traj_y,self.ALTITUDE_CONST_FIG8, label='Ideal trajectory', linestyle="--", linewidth=1, zorder=10) + ax6.plot3D(self.bag_x,self.bag_y,self.bag_z, label='Recorded trajectory', linewidth=1) + ax6.grid(True) + ax6.set_title('3D visualization') + ax6.set_xlabel('x [m]') + ax6.set_ylabel('y [m]') + ax6.set_zlabel('z [m]') + plt.close(fig6) + plt.tight_layout(pad=4) + pdf_pages.savefig(fig6) + + + + pdf_pages.close() + + print("Results saved in " + pdfname) + + def test_passed(self) -> bool : + '''Returns True if the deviation between recorded and ideal trajectories of the drone always stayed lower + than EPSILON. Returns False otherwise ''' + + nb_dev_points = len(self.deviation) + + if nb_dev_points == 0: + print("Test passed") + return True + else: + print(f"The deviation between ideal and recorded trajectories is greater than {self.EPSILON}m for {nb_dev_points} " + f"datapoints, which corresponds to a duration of {nb_dev_points*0.01}s") + return False + +if __name__=="__main__": + + #command line utility + + from argparse import ArgumentParser, Namespace + parser = ArgumentParser(description="Creates a pdf plotting the recorded trajectory of a drone against its desired trajectory") + parser.add_argument("desired_trajectory", type=str, help=".csv file containing (time,x,y,z) of the ideal/desired drone trajectory") + parser.add_argument("recorded_trajectory", type=str, help=".csv file containing (time,x,y,z) of the recorded drone trajectory") + parser.add_argument("pdf", type=str, help="name of the pdf file you want to create/overwrite") + parser.add_argument("--open", help="Open the pdf directly after it is created", action="store_true") + args : Namespace = parser.parse_args() + + plotter = Plotter() + plotter.create_figures(args.desired_trajectory, args.recorded_trajectory, args.pdf) + if args.open: + import subprocess + subprocess.call(["xdg-open", args.pdf]) diff --git a/systemtests/run_test.py b/systemtests/run_test.py new file mode 100755 index 000000000..73cfa1406 --- /dev/null +++ b/systemtests/run_test.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 +#!/usr/bin/env python3 +from subprocess import Popen, PIPE +import time +import os +import signal +from mcap_handler import McapHandler +from datetime import datetime +from plotter_class import Plotter +from pathlib import Path +import shutil +import atexit + + +class Waiter: + '''A helper class which sleeps the amount of seconds it is instanciated with. If the user interrupts with ^C during the waiting time (zB if the drone crashes), it will stop sleeping and continue + to the end of the program where all child processes are terminated correctly. All subsequent Waiter() calls will be ignored''' + wait=True + + def __init__(self, seconds): + if seconds > 0 : + self.sleep(seconds) + + def sleep(self, seconds): + start=time.time() + while Waiter.wait and time.time()<(start+seconds): + try: + time.sleep(1) + except KeyboardInterrupt: + Waiter.wait = False + +def record_start_and_terminate(testname:str, testduration:int, bagfolder:str): + '''Helper function that starts recording the /tf topic in a rosbag, starts the test, waits, closes the rosbag and terminate all processes + NB the testname must be the name of the crayzflie_examples executable (ie the CLI grammar "ros2 run crazyflie_examples testname" must be valid)''' + index = bagfolder.find("bag_") + bagname = bagfolder[index:] + + + src = "source " + bagfolder[:index-9] + "install/setup.bash" # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" + + command = f"{src} && ros2 bag record -s mcap -o {testname}_{bagname} /tf" + record_bag = Popen(command, shell=True, cwd=bagfolder, stderr=PIPE, stdout=True, + start_new_session=True, text=True, executable="/bin/bash") + + command = f"{src} && ros2 run crazyflie_examples {testname}" + start_flight_test = Popen(command, shell=True, stderr=True, stdout=True, + start_new_session=True, text=True, executable="/bin/bash") + + + Waiter(testduration) #wait x seconds for the crazyflie to fly the test + + os.killpg(os.getpgid(start_flight_test.pid), signal.SIGTERM) #kill flight test and all of its child processes + os.killpg(os.getpgid(record_bag.pid), signal.SIGTERM) #kill rosbag + + #if something went wrong with the bash command lines in Popen, print the error + if record_bag.stderr != None: + print(testname," record_bag stderr: ", record_bag.stderr.readlines()) + if start_flight_test.stderr != None: + print(testname," start_flight flight stderr: ", start_flight_test.stderr.readlines()) + + + +def translate_and_plot(testname:str, bagfolder:str): + '''Helper function that translates rosbag .mcap format to .csv, then uses that csv to plot a pdf ''' + index = bagfolder.find("bag_") + bagname = bagfolder[index:] + # NB : the mcap filename is almost the same as the folder name but has _0 at the end + inputbag = str(bagfolder) + f"/{testname}_{bagname}/{testname}_{bagname}_0.mcap" + output_csv = str(bagfolder) + f"/{testname}_{bagname}/{testname}_{bagname}_0.csv" + writer = McapHandler() + writer.write_mcap_to_csv(inputbag, output_csv) #translate bag from mcap to csv + output_pdf = str(path.parents[3].joinpath(f"results/Results_{testname}_"+ now +".pdf")) + rosbag_csv = output_csv + if "figure8" in testname: + test_file = "../crazyflie_examples/crazyflie_examples/data/figure8.csv" + elif "multi_trajectory" in testname: + test_file = "../crazyflie_examples/crazyflie_examples/data/multi_trajectory/traj0.csv" + else: + print("run_test.py : test file not defined") + plotter = Plotter() + plotter.create_figures(test_file, rosbag_csv, output_pdf) #plot the data + + +if __name__ == "__main__": + + path = Path(__file__) #Path(__file__) should be "/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/src/crazyswarm2/systemtests/newsub.py" ; path.parents[0]=.../systemstests + + #delete results, logs and bags of previous experiments + shutil.rmtree(path.parents[3].joinpath("bagfiles")) + shutil.rmtree(path.parents[3].joinpath("results")) + shutil.rmtree(Path.home() / ".ros/log") + + + #create the folder where we will record the different bags and the folder where the results pdf will be saved + now = datetime.now().strftime('%d_%m_%Y-%H_%M_%S') + bagfolder = str((path.parents[3].joinpath(f"bagfiles/bag_" + now))) # /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/bagfiles/bag_d_m_Y-H_M_S + os.makedirs(bagfolder) + os.makedirs(path.parents[3].joinpath("results")) # /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/results + + + src = "source " + str(path.parents[3].joinpath("install/setup.bash")) # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" + command = f"{src} && ros2 launch crazyflie launch.py" + launch_crazyswarm = Popen(command, shell=True, stderr=True, stdout=True, text=True, + start_new_session=True, executable="/bin/bash") + + time.sleep(1) + print("f8") + record_start_and_terminate("figure8", 20, bagfolder) + print("multi") + record_start_and_terminate("multi_trajectory", 80, bagfolder) + + os.killpg(os.getpgid(launch_crazyswarm.pid), signal.SIGTERM) #kill crazyswarm and all of its child processes + + + #test done, now we create the results pdf + translate_and_plot("figure8", bagfolder) + translate_and_plot("multi_trajectory", bagfolder) + + exit(0) From 5bd08d8194ac97062a11645e79c3eeff3621c53e Mon Sep 17 00:00:00 2001 From: julienthevenoz <125280576+julienthevenoz@users.noreply.github.com> Date: Thu, 7 Dec 2023 21:47:58 +0100 Subject: [PATCH 38/38] Improve systemtests (#378) * clean processes automatically at exit; replaced custom Waiter class with Popen.wait() function * cleanup --- .github/workflows/systemtests.yml | 2 +- systemtests/run_test.py | 105 +++++++++++++++++------------- 2 files changed, 61 insertions(+), 46 deletions(-) diff --git a/.github/workflows/systemtests.yml b/.github/workflows/systemtests.yml index 21588070b..da29294a4 100644 --- a/.github/workflows/systemtests.yml +++ b/.github/workflows/systemtests.yml @@ -2,7 +2,7 @@ name: System Tests on: push: - branches: [ "feature_systemtests" ] + branches: [ "feature-systemtests-better" ] # manual trigger workflow_dispatch: diff --git a/systemtests/run_test.py b/systemtests/run_test.py index 73cfa1406..d5f80f554 100755 --- a/systemtests/run_test.py +++ b/systemtests/run_test.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 #!/usr/bin/env python3 -from subprocess import Popen, PIPE +from subprocess import Popen, PIPE, TimeoutExpired import time import os import signal @@ -12,45 +12,58 @@ import atexit -class Waiter: - '''A helper class which sleeps the amount of seconds it is instanciated with. If the user interrupts with ^C during the waiting time (zB if the drone crashes), it will stop sleeping and continue - to the end of the program where all child processes are terminated correctly. All subsequent Waiter() calls will be ignored''' - wait=True +def clean_process(process:Popen) -> int : + '''Kills process and its children on exit if they aren't already terminated (called with atexit). Returns 0 on success, 1 on failure''' + + if process.poll() == None: + group_id = os.getpgid(process.pid) + print(f"cleaning process {group_id}") + os.killpg(group_id, signal.SIGTERM) + time.sleep(0.01) #necessary delay before first poll + i=0 + while i < 10 and process.poll() == None: #in case the process termination is lazy and takes some time, we wait up to 0.5 sec per process + if process.poll() != None: + return 0 #if we have a returncode-> it terminated + time.sleep(0.05) #if not wait a bit longer + if(i == 9): + print(f"Process group {process} with groupID {group_id} didn't terminate correctly") + return 1 #after 0.5s we stop waiting and consider it did not terminate correctly + return 0 + else: + return 0 #process already terminated - def __init__(self, seconds): - if seconds > 0 : - self.sleep(seconds) - - def sleep(self, seconds): - start=time.time() - while Waiter.wait and time.time()<(start+seconds): - try: - time.sleep(1) - except KeyboardInterrupt: - Waiter.wait = False - -def record_start_and_terminate(testname:str, testduration:int, bagfolder:str): - '''Helper function that starts recording the /tf topic in a rosbag, starts the test, waits, closes the rosbag and terminate all processes + +def record_start_and_clean(testname:str, max_wait:int, bagfolder:str): + '''Starts recording the /tf topic in a rosbag, starts the test, waits, closes the rosbag and terminate all processes. max_wait is the max amount of time you want to wait + before forcefully terminating the test flight script (in case it never finishes correctly). NB the testname must be the name of the crayzflie_examples executable (ie the CLI grammar "ros2 run crazyflie_examples testname" must be valid)''' + index = bagfolder.find("bag_") bagname = bagfolder[index:] - - src = "source " + bagfolder[:index-9] + "install/setup.bash" # -> "source /home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/install/setup.bash" - - command = f"{src} && ros2 bag record -s mcap -o {testname}_{bagname} /tf" - record_bag = Popen(command, shell=True, cwd=bagfolder, stderr=PIPE, stdout=True, - start_new_session=True, text=True, executable="/bin/bash") - command = f"{src} && ros2 run crazyflie_examples {testname}" - start_flight_test = Popen(command, shell=True, stderr=True, stdout=True, - start_new_session=True, text=True, executable="/bin/bash") - + try: + command = f"{src} && ros2 bag record -s mcap -o {testname}_{bagname} /tf" + record_bag = Popen(command, shell=True, cwd=bagfolder, stderr=PIPE, stdout=True, + start_new_session=True, text=True, executable="/bin/bash") + atexit.register(clean_process, record_bag) + + command = f"{src} && ros2 run crazyflie_examples {testname}" + start_flight_test = Popen(command, shell=True, stderr=True, stdout=True, + start_new_session=True, text=True, executable="/bin/bash") + atexit.register(clean_process, start_flight_test) - Waiter(testduration) #wait x seconds for the crazyflie to fly the test + start_flight_test.wait(timeout=max_wait) #raise Timeoutexpired after max_wait seconds if start_flight_test didn't finish by itself + clean_process(start_flight_test) + clean_process(record_bag) - os.killpg(os.getpgid(start_flight_test.pid), signal.SIGTERM) #kill flight test and all of its child processes - os.killpg(os.getpgid(record_bag.pid), signal.SIGTERM) #kill rosbag + except TimeoutExpired: #if max_wait is exceeded + clean_process(start_flight_test) + clean_process(record_bag) + + except KeyboardInterrupt: #if drone crashes, user can ^C to skip the waiting + clean_process(start_flight_test) + clean_process(record_bag) #if something went wrong with the bash command lines in Popen, print the error if record_bag.stderr != None: @@ -59,9 +72,9 @@ def record_start_and_terminate(testname:str, testduration:int, bagfolder:str): print(testname," start_flight flight stderr: ", start_flight_test.stderr.readlines()) - def translate_and_plot(testname:str, bagfolder:str): - '''Helper function that translates rosbag .mcap format to .csv, then uses that csv to plot a pdf ''' + '''Translates rosbag .mcap format to .csv, then uses that csv to plot a pdf ''' + index = bagfolder.find("bag_") bagname = bagfolder[index:] # NB : the mcap filename is almost the same as the folder name but has _0 at the end @@ -81,15 +94,18 @@ def translate_and_plot(testname:str, bagfolder:str): plotter.create_figures(test_file, rosbag_csv, output_pdf) #plot the data + if __name__ == "__main__": - path = Path(__file__) #Path(__file__) should be "/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/src/crazyswarm2/systemtests/newsub.py" ; path.parents[0]=.../systemstests + path = Path(__file__) #Path(__file__) in this case "/home/github/actions-runner/_work/crazyswarm2/crazyswarm2/ros2_ws/src/crazyswarm2/systemtests/newsub.py" ; path.parents[0]=.../systemstests - #delete results, logs and bags of previous experiments - shutil.rmtree(path.parents[3].joinpath("bagfiles")) - shutil.rmtree(path.parents[3].joinpath("results")) - shutil.rmtree(Path.home() / ".ros/log") - + #delete results, logs and bags of previous experiments if they exist + if(Path(path.parents[3].joinpath("bagfiles")).exists()): + shutil.rmtree(path.parents[3].joinpath("bagfiles")) + if(Path(path.parents[3].joinpath("results")).exists()): + shutil.rmtree(path.parents[3].joinpath("results")) + if(Path(Path.home() / ".ros/log").exists()): + shutil.rmtree(Path.home() / ".ros/log") #create the folder where we will record the different bags and the folder where the results pdf will be saved now = datetime.now().strftime('%d_%m_%Y-%H_%M_%S') @@ -102,14 +118,13 @@ def translate_and_plot(testname:str, bagfolder:str): command = f"{src} && ros2 launch crazyflie launch.py" launch_crazyswarm = Popen(command, shell=True, stderr=True, stdout=True, text=True, start_new_session=True, executable="/bin/bash") + atexit.register(clean_process, launch_crazyswarm) #atexit helps us to make sure processes are cleaned even if script exits unexpectedly time.sleep(1) - print("f8") - record_start_and_terminate("figure8", 20, bagfolder) - print("multi") - record_start_and_terminate("multi_trajectory", 80, bagfolder) + record_start_and_clean("figure8", 20, bagfolder) + record_start_and_clean("multi_trajectory", 80, bagfolder) - os.killpg(os.getpgid(launch_crazyswarm.pid), signal.SIGTERM) #kill crazyswarm and all of its child processes + clean_process(launch_crazyswarm) #kill crazyswarm and all of its child processes #test done, now we create the results pdf