From 87d439580bad0d1b4b0d38b827f6a7fab254d2ea Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 29 Sep 2022 11:04:47 +0200 Subject: [PATCH 01/10] add a velocity multiplexer for the crazyflie for external cmd_vel --- crazyflie/scripts/vel_mux.py | 39 ++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 crazyflie/scripts/vel_mux.py diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py new file mode 100644 index 000000000..746beb056 --- /dev/null +++ b/crazyflie/scripts/vel_mux.py @@ -0,0 +1,39 @@ +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import Twist + +import time + +TIME_STOP_SEND_CMD = 0.1 + +class VelMux(Node): + def __init__(self): + super().__init__('vel_mux') + self.subscription_hp = self.create_subscription( + Twist, + '/cmd_vel', + self.cmd_vel_callback, + 10) + self.msg_cmd_vel = Twist + self.received_first_cmd_vel = False + + def cmd_vel_callback(self, msg): + self.msg = msg + if self.received_first_cmd_vel is False: + self.received_first_cmd_vel = True + +def main(args=None): + rclpy.init(args=args) + + vel_mux = VelMux() + + rclpy.spin(vel_mux) + + vel_mux.destroy_node() + rclpy.shutdown() + + + +if __name__ == '__main__': + main() \ No newline at end of file From 6a65cbe436d69c7208acaad870ad48cb1dbdf168 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 29 Sep 2022 11:33:52 +0200 Subject: [PATCH 02/10] handle proper takeoff, hover and land --- crazyflie/scripts/vel_mux.py | 48 ++++++++++++++++++++++++++++++------ 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py index 746beb056..7ef8713b3 100644 --- a/crazyflie/scripts/vel_mux.py +++ b/crazyflie/scripts/vel_mux.py @@ -2,27 +2,63 @@ from rclpy.node import Node from geometry_msgs.msg import Twist - +from crazyflie_interfaces.srv import Takeoff, Land +from crazyflie_interfaces.msg import Hover import time -TIME_STOP_SEND_CMD = 0.1 +HEIGHT = 0.8 class VelMux(Node): def __init__(self): super().__init__('vel_mux') - self.subscription_hp = self.create_subscription( + self.subscription = self.create_subscription( Twist, '/cmd_vel', self.cmd_vel_callback, 10) - self.msg_cmd_vel = Twist + self.msg_cmd_vel = Twist() self.received_first_cmd_vel = False + prefix = '/cf2' + timer_period = 0.1 + self.timer = self.create_timer(timer_period, self.timer_callback) + self.take_off_client = self.create_client(Takeoff, prefix + '/take_off') + self.publisher_hover = self.create_publisher(Hover, prefix + '/cmd_vel_hover/', 10) + self.land_client = self.create_client(Takeoff, prefix + '/land') + def cmd_vel_callback(self, msg): - self.msg = msg + self.msg_cmd_vel = msg if self.received_first_cmd_vel is False: self.received_first_cmd_vel = True + def timer_callback(self): + if self.received_first_cmd_vel and self.cf_has_taken_off is False: + print('take off!') + req = Takeoff.request() + req.height = HEIGHT + req.duration = 2.0 + self.future = self.take_off_client.call_async(req) + rclpy.spin_until_future_complete(self, self.future) + self.cf_has_taken_off = True + + if self.received_first_cmd_vel and self.cf_has_taken_off: + if self.msg_cmd_vel.linear.z >= 0: + print('send velocity commands') + msg = Hover() + msg.vx = self.msg_cmd_vel.linear.x + msg.vy = self.msg_cmd_vel.linear.y + msg.yaw_rate = self.msg_cmd_vel.angular.z + msg.z_distance = HEIGHT + self.publisher_hover.publish(msg) + else: + req = Land.request() + req.height = 0.1 + req.duration = 2.0 + self.future = self.land_client.call_async(req) + rclpy.spin_until_future_complete(self, self.future) + self.cf_has_taken_off = False + + def main(args=None): rclpy.init(args=args) @@ -33,7 +69,5 @@ def main(args=None): vel_mux.destroy_node() rclpy.shutdown() - - if __name__ == '__main__': main() \ No newline at end of file From 2dfac1ef92b9987e0e9f5626aa72417043c29b73 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 30 Sep 2022 12:02:45 +0200 Subject: [PATCH 03/10] add vel_mux to build --- crazyflie/CMakeLists.txt | 1 + crazyflie/package.xml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/crazyflie/CMakeLists.txt b/crazyflie/CMakeLists.txt index 482f7f968..ac50ac91b 100644 --- a/crazyflie/CMakeLists.txt +++ b/crazyflie/CMakeLists.txt @@ -102,6 +102,7 @@ install(TARGETS install(PROGRAMS scripts/crazyflie_server.py scripts/chooser.py + scripts/vel_mux.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/crazyflie/package.xml b/crazyflie/package.xml index 3afce30a5..ead8dba88 100644 --- a/crazyflie/package.xml +++ b/crazyflie/package.xml @@ -17,6 +17,8 @@ geometry_msgs motion_capture_tracking_interfaces + tf_transformations + ament_lint_auto ament_lint_common From 589ed107d04ecb6cef0b2b4c696bb49f0dfccf43 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 30 Sep 2022 13:32:59 +0200 Subject: [PATCH 04/10] got vel mux working with cflib server --- crazyflie/scripts/crazyflie_server.py | 20 ++++++++++++++++++-- crazyflie/scripts/vel_mux.py | 27 ++++++++++++++++----------- 2 files changed, 34 insertions(+), 13 deletions(-) mode change 100644 => 100755 crazyflie/scripts/vel_mux.py diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 6cc837c42..d29190075 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -21,6 +21,7 @@ from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType +from crazyflie_interfaces.msg import Hover from std_srvs.srv import Empty from geometry_msgs.msg import Twist @@ -33,7 +34,7 @@ from tf2_ros import TransformBroadcaster from functools import partial -from math import radians, pi, cos, sin +from math import degrees, radians, pi, cos, sin FIX_HEIGHT_CMD_VEL_2D = False @@ -250,6 +251,10 @@ def __init__(self): Twist, name + "/cmd_vel_2d", partial(self._cmd_vel_2d_changed, uri=uri), 10 ) + self.create_subscription( + Hover, name + + "/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10 + ) if FIX_HEIGHT_CMD_VEL_2D: timer_period = 0.1 self.create_timer(timer_period, self.send_hover_command) @@ -803,6 +808,18 @@ def _cmd_vel_2d_changed(self, msg, uri=""): self.swarm._cfs[uri].cf.commander.send_hover_setpoint( vx, vy, yawrate, z) + def _cmd_hover_changed(self, msg, uri=""): + """ + Topic update callback to control the 2d velocity and thrust + of the crazyflie with teleop + """ + vx = msg.vx + vy = msg.vy + z = msg.z_distance + yawrate = -1.0*degrees(msg.yaw_rate) + self.swarm._cfs[uri].cf.commander.send_hover_setpoint(vx, vy, yawrate, z) + self.get_logger().info(f"{uri}: Received hover topic {vx} {vy} {yawrate} {z}") + def _remove_logging(self, request, response, uri="all"): """ Service callback to remove logging blocks of the crazyflie @@ -847,7 +864,6 @@ def _add_logging(self, request, response, uri="all"): topic_name = request.topic_name frequency = request.frequency variables = request.vars - print(self.cf_dict[uri] + "/logs/pose/frequency/", frequency) if topic_name in self.default_log_type.keys(): try: self.declare_parameter( diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py old mode 100644 new mode 100755 index 7ef8713b3..eea336cd4 --- a/crazyflie/scripts/vel_mux.py +++ b/crazyflie/scripts/vel_mux.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import rclpy from rclpy.node import Node @@ -6,7 +8,7 @@ from crazyflie_interfaces.msg import Hover import time -HEIGHT = 0.8 +HEIGHT = 0.3 class VelMux(Node): def __init__(self): @@ -18,12 +20,13 @@ def __init__(self): 10) self.msg_cmd_vel = Twist() self.received_first_cmd_vel = False - prefix = '/cf2' + prefix = '/cf1' timer_period = 0.1 self.timer = self.create_timer(timer_period, self.timer_callback) - self.take_off_client = self.create_client(Takeoff, prefix + '/take_off') - self.publisher_hover = self.create_publisher(Hover, prefix + '/cmd_vel_hover/', 10) - self.land_client = self.create_client(Takeoff, prefix + '/land') + self.take_off_client = self.create_client(Takeoff, prefix + '/takeoff') + self.publisher_hover = self.create_publisher(Hover, prefix + '/cmd_hover', 10) + self.land_client = self.create_client(Land, prefix + '/land') + self.cf_has_taken_off = False def cmd_vel_callback(self, msg): @@ -34,12 +37,14 @@ def cmd_vel_callback(self, msg): def timer_callback(self): if self.received_first_cmd_vel and self.cf_has_taken_off is False: print('take off!') - req = Takeoff.request() + req = Takeoff.Request() req.height = HEIGHT - req.duration = 2.0 + req.duration = rclpy.duration.Duration(seconds=2.0).to_msg() self.future = self.take_off_client.call_async(req) - rclpy.spin_until_future_complete(self, self.future) self.cf_has_taken_off = True + time.sleep(2.0) + + print(self.msg_cmd_vel) if self.received_first_cmd_vel and self.cf_has_taken_off: if self.msg_cmd_vel.linear.z >= 0: @@ -51,12 +56,12 @@ def timer_callback(self): msg.z_distance = HEIGHT self.publisher_hover.publish(msg) else: - req = Land.request() + req = Land.Request() req.height = 0.1 - req.duration = 2.0 + req.duration = rclpy.duration.Duration(seconds=2.0).to_msg() self.future = self.land_client.call_async(req) - rclpy.spin_until_future_complete(self, self.future) self.cf_has_taken_off = False + self.received_first_cmd_vel = False def main(args=None): From 8575274eb4add2f7ae802551d4762ec24838ff2d Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 30 Sep 2022 15:30:47 +0200 Subject: [PATCH 05/10] have an example launch file of vel_mux --- crazyflie/scripts/vel_mux.py | 45 ++++++++++++------- .../launch/keyboard_velmux_launch.py | 36 +++++++++++++++ crazyflie_examples/setup.py | 3 ++ 3 files changed, 68 insertions(+), 16 deletions(-) create mode 100644 crazyflie_examples/launch/keyboard_velmux_launch.py diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py index eea336cd4..c0463180a 100755 --- a/crazyflie/scripts/vel_mux.py +++ b/crazyflie/scripts/vel_mux.py @@ -13,42 +13,52 @@ 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('incoming_twist_topic', '/cmd_vel') + + self.hover_height = self.get_parameter('hover_height').value + robot_prefix = self.get_parameter('robot_prefix').value + incoming_twist_topic = self.get_parameter('incoming_twist_topic').value + + self.get_logger().info(f"Velocity Multiplexer set for {robot_prefix}"+ + f" with height {self.hover_height} m using the {incoming_twist_topic} topic") + + print("PARAMETERS", self.hover_height, robot_prefix, incoming_twist_topic) + self.subscription = self.create_subscription( Twist, - '/cmd_vel', + incoming_twist_topic, self.cmd_vel_callback, 10) self.msg_cmd_vel = Twist() self.received_first_cmd_vel = False - prefix = '/cf1' timer_period = 0.1 self.timer = self.create_timer(timer_period, self.timer_callback) - self.take_off_client = self.create_client(Takeoff, prefix + '/takeoff') - self.publisher_hover = self.create_publisher(Hover, prefix + '/cmd_hover', 10) - self.land_client = self.create_client(Land, prefix + '/land') + self.take_off_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.cf_has_taken_off = False - def cmd_vel_callback(self, msg): self.msg_cmd_vel = msg - if self.received_first_cmd_vel is False: + # This is to handle the zero twist messages from teleop twist keyboard closing + # or else the crazyflie would constantly take off again. + msg_is_zero = msg.linear.x == 0.0 and msg.linear.y == 0.0 and msg.angular.z == 0.0 and msg.linear.z == 0.0 + if msg_is_zero is False and self.received_first_cmd_vel is False and msg.linear.z >= 0.0: self.received_first_cmd_vel = True + print('takeoff') def timer_callback(self): if self.received_first_cmd_vel and self.cf_has_taken_off is False: - print('take off!') req = Takeoff.Request() - req.height = HEIGHT + req.height = self.hover_height req.duration = rclpy.duration.Duration(seconds=2.0).to_msg() - self.future = self.take_off_client.call_async(req) + self.take_off_client.call_async(req) self.cf_has_taken_off = True - time.sleep(2.0) - - print(self.msg_cmd_vel) - + time.sleep(2.0) if self.received_first_cmd_vel and self.cf_has_taken_off: if self.msg_cmd_vel.linear.z >= 0: - print('send velocity commands') msg = Hover() msg.vx = self.msg_cmd_vel.linear.x msg.vy = self.msg_cmd_vel.linear.y @@ -56,14 +66,17 @@ def timer_callback(self): msg.z_distance = HEIGHT self.publisher_hover.publish(msg) else: + print('land') req = Land.Request() req.height = 0.1 req.duration = rclpy.duration.Duration(seconds=2.0).to_msg() - self.future = self.land_client.call_async(req) + self.land_client.call_async(req) + time.sleep(2.0) self.cf_has_taken_off = False self.received_first_cmd_vel = False + def main(args=None): rclpy.init(args=args) diff --git a/crazyflie_examples/launch/keyboard_velmux_launch.py b/crazyflie_examples/launch/keyboard_velmux_launch.py new file mode 100644 index 000000000..db7badac4 --- /dev/null +++ b/crazyflie_examples/launch/keyboard_velmux_launch.py @@ -0,0 +1,36 @@ +import os +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + # load crazyflies + crazyflies_yaml = os.path.join( + get_package_share_directory('crazyflie'), + 'config', + 'crazyflies.yaml') + + with open(crazyflies_yaml, 'r') as ymlfile: + crazyflies = yaml.safe_load(ymlfile) + + server_params = crazyflies + + 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"}] + ), + ]) diff --git a/crazyflie_examples/setup.py b/crazyflie_examples/setup.py index 8cbd7a8d2..514befc1b 100644 --- a/crazyflie_examples/setup.py +++ b/crazyflie_examples/setup.py @@ -1,4 +1,6 @@ from setuptools import setup +import os +from glob import glob package_name = 'crazyflie_examples' @@ -11,6 +13,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*launch.py')) ], install_requires=['setuptools'], zip_safe=True, From 8a4e4a43825d845f7fe24f4334d25ec41d1012fb Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 30 Sep 2022 15:35:58 +0200 Subject: [PATCH 06/10] update documentation --- docs2/usage.rst | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/docs2/usage.rst b/docs2/usage.rst index f971c443d..5476281c4 100644 --- a/docs2/usage.rst +++ b/docs2/usage.rst @@ -48,6 +48,7 @@ Enabling Logblocks ~~~~~~~~~~~~~~~~~~ In one terminal run + .. code-block:: bash ros2 launch crazyflie launch.py backend:=cflib @@ -69,8 +70,8 @@ To close the logblocks again, run: ros2 service call /cf2/remove_logging crazyflie_interfaces/srv/RemoveLogging "{topic_name: 'pose'}" -Teleoperation -~~~~~~~~~~~~~ +Teleoperation controller +~~~~~~~~~~~~~~~~~~~~~~~~ We currently assume an XBox controller (the button mapping can be changed in teleop.yaml). It is possible to fly in different modes, including attitude-control and position-control (in which case any localization system can assist.) @@ -79,6 +80,27 @@ We currently assume an XBox controller (the button mapping can be changed in tel ros2 launch crazyflie launch.py +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 flowdeck, +set the controller to 1 (PID) + +Then, run the following launch file to start up the crazyflie server (CFlib): + +.. code-block:: bash + + ros2 launch crazyflie_examples keyboard_velmux_launch.py + +in another terminal run: + +.. code-block:: bash + + ros2 run teleop_twist_keyboard telop_twist_keyboard + +Use 't' to take off, and 'b' to land. For the rest, use the instructions of the telop package. + Python scripts ~~~~~~~~~~~~~~ From 850095da9e4d259edec75419ed079aa1731b3db7 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 30 Sep 2022 15:48:04 +0200 Subject: [PATCH 07/10] remove vel_2d functionality --- crazyflie/scripts/crazyflie_server.py | 69 +-------------------------- 1 file changed, 2 insertions(+), 67 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index d29190075..43388a291 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -36,8 +36,6 @@ from functools import partial from math import degrees, radians, pi, cos, sin -FIX_HEIGHT_CMD_VEL_2D = False - cf_log_to_ros_topic = { "uint8_t": UInt8, "uint16_t": UInt16, @@ -111,13 +109,6 @@ def __init__(self): # Initialize logging, services and parameters for each crazyflie for link_uri in self.uris: - - # Only used for the pos hold mode - self.swarm._cfs[link_uri].cmd_vel_2d = {} - self.swarm._cfs[link_uri].cmd_vel_2d['hold_pos'] = True - self.swarm._cfs[link_uri].cmd_vel_2d['current_height'] = 0.0 - self.swarm._cfs[link_uri].cmd_vel_2d['timestamp'] = 0.0 - self.swarm._cfs[link_uri].cmd_vel_2d['msg'] = Twist # Connect callbacks for different connection states of the crazyflie self.swarm._cfs[link_uri].cf.fully_connected.add_callback( @@ -247,43 +238,10 @@ def __init__(self): Twist, name + "/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed, uri=uri), 10 ) - self.create_subscription( - Twist, name + - "/cmd_vel_2d", partial(self._cmd_vel_2d_changed, uri=uri), 10 - ) self.create_subscription( Hover, name + "/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10 ) - if FIX_HEIGHT_CMD_VEL_2D: - timer_period = 0.1 - self.create_timer(timer_period, self.send_hover_command) - - def send_hover_command(self): - for uri in self.cf_dict: - height = self.swarm._cfs[uri].cmd_vel_2d['current_height'] - if height<0.2: - return - timestamp = self.swarm._cfs[uri].cmd_vel_2d['timestamp'] - - if time.time() < timestamp + 0.5: - self.get_logger().info(f'{uri}: Received 2d twist message') - - msg = self.swarm._cfs[uri].cmd_vel_2d['msg'] - - vx = msg.linear.y - vy = msg.linear.x - yawrate = msg.angular.z - self.swarm._cfs[uri].cf.commander.send_hover_setpoint( - vx, vy, yawrate, height) - self.swarm._cfs[uri].cmd_vel_2d['hold_pos'] = False - else: - if self.swarm._cfs[uri].cmd_vel_2d['hold_pos'] == False: - self.get_logger().info(f'{uri}: Sent Position hold') - - self.swarm._cfs[uri].cf.high_level_commander.go_to(0,0,0,0,0,relative=1) - self.swarm._cfs[uri].cmd_vel_2d['hold_pos'] = True - def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_enabled, topic_type): """ @@ -687,14 +645,10 @@ def _takeoff_callback(self, request, response, uri="all"): self.swarm._cfs[link_uri].cf.high_level_commander.takeoff( request.height, duration ) - self.swarm._cfs[link_uri].cmd_vel_2d['current_height'] = request.height - else: self.swarm._cfs[uri].cf.high_level_commander.takeoff( request.height, duration ) - self.swarm._cfs[uri].cmd_vel_2d['current_height'] = request.height - return response @@ -715,13 +669,10 @@ def _land_callback(self, request, response, uri="all"): self.swarm._cfs[link_uri].cf.high_level_commander.land( request.height, duration, group_mask=request.group_mask ) - self.swarm._cfs[link_uri].cmd_vel_2d['current_height'] = request.height else: self.swarm._cfs[uri].cf.high_level_commander.land( request.height, duration, group_mask=request.group_mask ) - self.swarm._cfs[uri].cmd_vel_2d['current_height'] = request.height - return response @@ -792,26 +743,10 @@ def _cmd_vel_legacy_changed(self, msg, uri=""): self.swarm._cfs[uri].cf.commander.send_setpoint( roll, pitch, yawrate, thrust) - def _cmd_vel_2d_changed(self, msg, uri=""): - """ - Topic update callback to control the 2d velocity and thrust - of the crazyflie with teleop - """ - self.swarm._cfs[uri].cmd_vel_2d['timestamp'] = time.time() - self.swarm._cfs[uri].cmd_vel_2d['msg'] = msg - - if FIX_HEIGHT_CMD_VEL_2D is False: - vx = msg.linear.y - vy = msg.linear.x - z = msg.linear.z - yawrate = msg.angular.z - self.swarm._cfs[uri].cf.commander.send_hover_setpoint( - vx, vy, yawrate, z) - def _cmd_hover_changed(self, msg, uri=""): """ - Topic update callback to control the 2d velocity and thrust - of the crazyflie with teleop + Topic update callback to control the hover command + of the crazyflie from the velocity multiplexer (vel_mux) """ vx = msg.vx vy = msg.vy From d12ce4b1eb2b1cfe15141681f125c2e2101d428c Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 30 Sep 2022 15:49:40 +0200 Subject: [PATCH 08/10] explanation in the vel_mux script --- crazyflie/scripts/vel_mux.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py index c0463180a..a11e361d7 100755 --- a/crazyflie/scripts/vel_mux.py +++ b/crazyflie/scripts/vel_mux.py @@ -1,5 +1,13 @@ #!/usr/bin/env python3 +""" +A Twist message handler that get incoming twist messages from + external packages and handles proper takeoff, landing and + hover commands of connected crazyflie in the crazyflie_server + node + + 2022 - K. N. McGuire (Bitcraze AB) +""" import rclpy from rclpy.node import Node @@ -88,4 +96,4 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() From 61f1158176ae6c407438c85cfd34c735a8c1194b Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 30 Sep 2022 15:54:14 +0200 Subject: [PATCH 09/10] Add wait for service --- crazyflie/scripts/vel_mux.py | 22 +++++++++------------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py index a11e361d7..f1155ba1c 100755 --- a/crazyflie/scripts/vel_mux.py +++ b/crazyflie/scripts/vel_mux.py @@ -16,8 +16,6 @@ from crazyflie_interfaces.msg import Hover import time -HEIGHT = 0.3 - class VelMux(Node): def __init__(self): super().__init__('vel_mux') @@ -29,11 +27,6 @@ def __init__(self): robot_prefix = self.get_parameter('robot_prefix').value incoming_twist_topic = self.get_parameter('incoming_twist_topic').value - self.get_logger().info(f"Velocity Multiplexer set for {robot_prefix}"+ - f" with height {self.hover_height} m using the {incoming_twist_topic} topic") - - print("PARAMETERS", self.hover_height, robot_prefix, incoming_twist_topic) - self.subscription = self.create_subscription( Twist, incoming_twist_topic, @@ -43,11 +36,17 @@ def __init__(self): self.received_first_cmd_vel = False timer_period = 0.1 self.timer = self.create_timer(timer_period, self.timer_callback) - self.take_off_client = self.create_client(Takeoff, robot_prefix + '/takeoff') + 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.cf_has_taken_off = False + self.takeoff_client.wait_for_service() + self.land_client.wait_for_service() + + self.get_logger().info(f"Velocity Multiplexer set for {robot_prefix}"+ + f" with height {self.hover_height} m using the {incoming_twist_topic} topic") + def cmd_vel_callback(self, msg): self.msg_cmd_vel = msg # This is to handle the zero twist messages from teleop twist keyboard closing @@ -55,14 +54,13 @@ def cmd_vel_callback(self, msg): msg_is_zero = msg.linear.x == 0.0 and msg.linear.y == 0.0 and msg.angular.z == 0.0 and msg.linear.z == 0.0 if msg_is_zero is False and self.received_first_cmd_vel is False and msg.linear.z >= 0.0: self.received_first_cmd_vel = True - print('takeoff') def timer_callback(self): if self.received_first_cmd_vel and self.cf_has_taken_off is False: req = Takeoff.Request() req.height = self.hover_height req.duration = rclpy.duration.Duration(seconds=2.0).to_msg() - self.take_off_client.call_async(req) + self.takeoff_client.call_async(req) self.cf_has_taken_off = True time.sleep(2.0) if self.received_first_cmd_vel and self.cf_has_taken_off: @@ -71,10 +69,9 @@ def timer_callback(self): msg.vx = self.msg_cmd_vel.linear.x msg.vy = self.msg_cmd_vel.linear.y msg.yaw_rate = self.msg_cmd_vel.angular.z - msg.z_distance = HEIGHT + msg.z_distance = self.hover_height self.publisher_hover.publish(msg) else: - print('land') req = Land.Request() req.height = 0.1 req.duration = rclpy.duration.Duration(seconds=2.0).to_msg() @@ -84,7 +81,6 @@ def timer_callback(self): self.received_first_cmd_vel = False - def main(args=None): rclpy.init(args=args) From 4ae321f4ef45f38cfabd40be08275bb59b1dfc4c Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 3 Oct 2022 10:59:20 +0200 Subject: [PATCH 10/10] add sudo apt update to docs ci --- .github/workflows/ci-docs2.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci-docs2.yml b/.github/workflows/ci-docs2.yml index 4c057a00f..e20f539b1 100644 --- a/.github/workflows/ci-docs2.yml +++ b/.github/workflows/ci-docs2.yml @@ -16,6 +16,7 @@ jobs: - name: Install dependencies run: | + sudo apt update sudo apt install texlive-latex-extra dvipng sudo pip3 install -U setuptools sudo pip3 install -U -r docs2/requirements.txt