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