diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index ad8e299b4..b52bb6253 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -785,7 +785,15 @@ def _go_to_callback(self, request, response, uri="all"): return response def _notify_setpoints_stop_callback(self, request, response, uri="all"): - self.get_logger().info("Notify setpoint stop not yet implemented") + + self.get_logger().info(f"{uri}: Received notify setpoint stop") + + if uri == "all": + for link_uri in self.uris: + self.swarm._cfs[link_uri].cf.commander.send_notify_setpoint_stop() + else: + self.swarm._cfs[uri].cf.commander.send_notify_setpoint_stop() + return response def _upload_trajectory_callback(self, request, response, uri="all"): diff --git a/crazyflie/scripts/vel_mux.py b/crazyflie/scripts/vel_mux.py index f1155ba1c..aab381cbb 100755 --- a/crazyflie/scripts/vel_mux.py +++ b/crazyflie/scripts/vel_mux.py @@ -12,7 +12,7 @@ from rclpy.node import Node from geometry_msgs.msg import Twist -from crazyflie_interfaces.srv import Takeoff, Land +from crazyflie_interfaces.srv import Takeoff, Land, NotifySetpointsStop from crazyflie_interfaces.msg import Hover import time @@ -39,6 +39,7 @@ def __init__(self): self.takeoff_client = self.create_client(Takeoff, robot_prefix + '/takeoff') self.publisher_hover = self.create_publisher(Hover, robot_prefix + '/cmd_hover', 10) self.land_client = self.create_client(Land, robot_prefix + '/land') + self.notify_client = self.create_client(NotifySetpointsStop, robot_prefix + '/notify_setpoints_stop') self.cf_has_taken_off = False self.takeoff_client.wait_for_service() @@ -72,6 +73,8 @@ def timer_callback(self): msg.z_distance = self.hover_height self.publisher_hover.publish(msg) else: + req = NotifySetpointsStop.Request() + self.notify_client.call_async(req) req = Land.Request() req.height = 0.1 req.duration = rclpy.duration.Duration(seconds=2.0).to_msg() @@ -80,7 +83,6 @@ def timer_callback(self): self.cf_has_taken_off = False self.received_first_cmd_vel = False - def main(args=None): rclpy.init(args=args) diff --git a/crazyflie_examples/launch/keyboard_velmux_launch.py b/crazyflie_examples/launch/keyboard_velmux_launch.py index 25d318a0b..fa06c87ba 100644 --- a/crazyflie_examples/launch/keyboard_velmux_launch.py +++ b/crazyflie_examples/launch/keyboard_velmux_launch.py @@ -31,8 +31,8 @@ def generate_launch_description(): executable='vel_mux.py', name='vel_mux', output='screen', - parameters=[{'hover_height': 0.3}, - {'incoming_twist_topic': '/cmd_vel'}, - {'robot_prefix': '/cf1'}] + parameters=[{"hover_height": 0.3}, + {"incoming_twist_topic": "/cmd_vel"}, + {"robot_prefix": "/cf231"}] ), ]) diff --git a/docs2/tutorials.rst b/docs2/tutorials.rst index bb550ce1c..c007409bd 100644 --- a/docs2/tutorials.rst +++ b/docs2/tutorials.rst @@ -17,7 +17,7 @@ Teleoperation keyboard We have an example of the telop_twist_keyboard package working together with the crazyflie First, make sure that the crazyflies.yaml has the right URI and if you are using the `Flow deck `_ or `any other position system available `_ to the crazyflie. -set the controller to 1 (PID) +set the controller to 1 (PID). And if you have not already, install the teleop package for the keyboard. (replace DISTRO with humble or iron): @@ -25,7 +25,9 @@ And if you have not already, install the teleop package for the keyboard. (repl sudo apt-get install ros-DISTRO-teleop-twist-keyboard -Then, run the following launch file to start up the crazyflie server (CFlib): +Then, first checkout keyboard_velmux_launch.py and make sure that the 'robot_prefix' of vel_mux matches your crazyflie ID in crazyfies.yaml ('cf231'). + +Then run the following launch file to start up the crazyflie server (CFlib): .. code-block:: bash