From 17f187467a7ddb134c6c28074740cccc35003fc0 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Thu, 14 Dec 2023 14:14:29 +0100
Subject: [PATCH] crazyflie_py: add support for setting groupMask

See https://github.com/IMRCLab/crazyswarm2/discussions/203

Only works with recent firmware versions.
---
 .../crazyflie_examples/group_mask.py          | 27 ++++++++++++
 crazyflie_examples/setup.cfg                  |  1 +
 crazyflie_py/crazyflie_py/crazyflie.py        | 42 +++++++++----------
 3 files changed, 49 insertions(+), 21 deletions(-)
 create mode 100644 crazyflie_examples/crazyflie_examples/group_mask.py

diff --git a/crazyflie_examples/crazyflie_examples/group_mask.py b/crazyflie_examples/crazyflie_examples/group_mask.py
new file mode 100644
index 000000000..72ee8b2a9
--- /dev/null
+++ b/crazyflie_examples/crazyflie_examples/group_mask.py
@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+
+from crazyflie_py import Crazyswarm
+
+
+def main():
+    swarm = Crazyswarm()
+    timeHelper = swarm.timeHelper
+    allcfs = swarm.allcfs
+
+    # set group mask to enable group 1 and 4 (one by one)
+    for cf in allcfs.crazyflies:
+        cf.setGroupMask(0b00001001)
+
+    print('Takeoff with a different mask (2) -> nothing should happen')
+    allcfs.takeoff(targetHeight=0.5, duration=3.0, groupMask=2)
+    timeHelper.sleep(3)
+
+    print('Takeoff with correct mask (1) -> should work')
+    allcfs.takeoff(targetHeight=0.5, duration=3.0, groupMask=1)
+    timeHelper.sleep(3)
+    allcfs.land(targetHeight=0.02, duration=3.0)
+    timeHelper.sleep(3)
+
+
+if __name__ == '__main__':
+    main()
diff --git a/crazyflie_examples/setup.cfg b/crazyflie_examples/setup.cfg
index 06e3a1c1a..b1b614c9e 100644
--- a/crazyflie_examples/setup.cfg
+++ b/crazyflie_examples/setup.cfg
@@ -3,6 +3,7 @@ console_scripts =
     hello_world = crazyflie_examples.hello_world:main
     nice_hover = crazyflie_examples.nice_hover:main
     figure8 = crazyflie_examples.figure8:main
+    group_mask = crazyflie_examples.group_mask:main
     multi_trajectory = crazyflie_examples.multi_trajectory:main
     cmd_full_state = crazyflie_examples.cmd_full_state:main
     set_param = crazyflie_examples.set_param:main
diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py
index 025cac736..762869619 100644
--- a/crazyflie_py/crazyflie_py/crazyflie.py
+++ b/crazyflie_py/crazyflie_py/crazyflie.py
@@ -114,8 +114,6 @@ def __init__(self, node, cfname, paramTypeDict):
 
         # self.tf = tf
 
-        # rospy.wait_for_service(prefix + '/set_group_mask')
-        # self.setGroupMaskService = rospy.ServiceProxy(prefix + '/set_group_mask', SetGroupMask)
         self.emergencyService = node.create_client(Empty, prefix + '/emergency')
         self.emergencyService.wait_for_service()
         self.takeoffService = node.create_client(Takeoff, prefix + '/takeoff')
@@ -186,29 +184,31 @@ def __init__(self, node, cfname, paramTypeDict):
         # self.cmdVelocityWorldMsg.header.seq = 0
         # self.cmdVelocityWorldMsg.header.frame_id = '/world'
 
-    # def setGroupMask(self, groupMask):
-    #     """Sets the group mask bits for this robot.
+    def setGroupMask(self, groupMask):
+        """Sets the group mask bits for this robot.
 
-    #     The purpose of groups is to make it possible to trigger an action
-    #     (for example, executing a previously-uploaded trajectory) on a subset
-    #     of all robots without needing to send more than one radio packet.
-    #     This is important to achieve tight, synchronized 'choreography'.
+        The purpose of groups is to make it possible to trigger an action
+        (for example, executing a previously-uploaded trajectory) on a subset
+        of all robots without needing to send more than one radio packet.
+        This is important to achieve tight, synchronized 'choreography'.
 
-    #     Up to 8 groups may exist, corresponding to bits in the groupMask byte.
-    #     When a broadcast command is triggered on the :obj:`CrazyflieServer` object
-    #     with a groupMask argument, the command only affects those robots whose
-    #     groupMask has a nonzero bitwise-AND with the command's groupMask.
-    #     A command with a groupMask of zero applies to all robots regardless of
-    #     group membership.
+        Up to 8 groups may exist, corresponding to bits in the groupMask byte.
+        When a broadcast command is triggered on the :obj:`CrazyflieServer` object
+        with a groupMask argument, the command only affects those robots whose
+        groupMask has a nonzero bitwise-AND with the command's groupMask.
+        A command with a groupMask of zero applies to all robots regardless of
+        group membership.
 
-    #     Some individual robot (not broadcast) commands also support groupMask,
-    #     but it is not especially useful in that case.
+        Some individual robot (not broadcast) commands also support groupMask,
+        but it is not especially useful in that case.
 
-    #     Args:
-    #         groupMask (int): An 8-bit integer representing this robot's
-    #             membership status in each of the <= 8 possible groups.
-    #     """
-    #     self.setGroupMaskService(groupMask)
+        Args:
+            groupMask (int): An 8-bit integer representing this robot's
+                membership status in each of the <= 8 possible groups.
+        """
+        # Note that this requires a recent firmware; older firmware versions
+        #      do not have such a parameter.
+        self.setParam('hlCommander.groupmask', groupMask)
 
     # def enableCollisionAvoidance(self, others, ellipsoidRadii):
     #     """Enables onboard collision avoidance.