cflib Add notify setpoints stop

crazyflie/scripts/crazyflie_server.py

@@ -785,7 +785,15 @@ def _go_to_callback(self, request, response, uri="all"):
         return response
 
     def _notify_setpoints_stop_callback(self, request, response, uri="all"):
-        self.get_logger().info("Notify setpoint stop not yet implemented")
+
+        self.get_logger().info(f"{uri}: Received notify setpoint stop")
+
+        if uri == "all":
+            for link_uri in self.uris:
+                self.swarm._cfs[link_uri].cf.commander.send_notify_setpoint_stop()              
+        else:
+            self.swarm._cfs[uri].cf.commander.send_notify_setpoint_stop()
+
         return response
 
     def _upload_trajectory_callback(self, request, response, uri="all"):

crazyflie/scripts/vel_mux.py

@@ -12,7 +12,7 @@
 from rclpy.node import Node
 
 from geometry_msgs.msg import Twist
-from crazyflie_interfaces.srv import Takeoff, Land
+from crazyflie_interfaces.srv import Takeoff, Land, NotifySetpointsStop
 from crazyflie_interfaces.msg import Hover
 import time
 
@@ -39,6 +39,7 @@ def __init__(self):
         self.takeoff_client = self.create_client(Takeoff, robot_prefix + '/takeoff')
         self.publisher_hover = self.create_publisher(Hover, robot_prefix + '/cmd_hover', 10)
         self.land_client = self.create_client(Land, robot_prefix + '/land')
+        self.notify_client = self.create_client(NotifySetpointsStop, robot_prefix + '/notify_setpoints_stop')
         self.cf_has_taken_off = False
 
         self.takeoff_client.wait_for_service()
@@ -72,6 +73,8 @@ def timer_callback(self):
                 msg.z_distance = self.hover_height
                 self.publisher_hover.publish(msg)
             else:
+                req = NotifySetpointsStop.Request()
+                self.notify_client.call_async(req)
                 req = Land.Request()
                 req.height = 0.1
                 req.duration = rclpy.duration.Duration(seconds=2.0).to_msg()
@@ -80,7 +83,6 @@ def timer_callback(self):
                 self.cf_has_taken_off = False
                 self.received_first_cmd_vel = False
 
-
 def main(args=None):
     rclpy.init(args=args)
 

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

docs2/tutorials.rst

@@ -17,15 +17,17 @@ Teleoperation keyboard
 We have an example of the telop_twist_keyboard package working together with the crazyflie
 
 First, make sure that the crazyflies.yaml has the right URI and if you are using the `Flow deck <https://www.bitcraze.io/products/flow-deck-v2/>`_ or `any other position system available <https://www.bitcraze.io/documentation/system/positioning//>`_ to the crazyflie.  
-set the controller to 1 (PID)
+set the controller to 1 (PID). 
 
 And if you  have not already, install the teleop package for the keyboard. (replace DISTRO with humble or galactic):
 
 .. code-block:: bash
 
     sudo apt-get install ros-DISTRO-teleop-twist-keyboard
 
-Then, run the following launch file to start up the crazyflie server (CFlib):
+Then, first checkout keyboard_velmux_launch.py and make sure that the 'robot_prefix' of vel_mux matches your crazyflie ID in crazyfies.yaml ('cf231').
+
+Then run the following launch file to start up the crazyflie server (CFlib):
 
 .. code-block:: bash