crazyflie_py: add support for setting groupMask (IMRCLab#384)

* crazyflie_py: add support for setting groupMask

See IMRCLab#203

Only works with recent firmware versions.

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()

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

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,37 @@ 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):
+ """
+ Set 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.
+ try:
+ self.setParam('hlCommander.groupmask', groupMask)
+ except KeyError:
+ self.node.get_logger().error('setGroupMask: Your firmware is too old - Please update.')
 
  # def enableCollisionAvoidance(self, others, ellipsoidRadii):
  # """Enables onboard collision avoidance.