[SOLVED] Connection multiples crazyflies with cflib on ROS2

[fjmanasalvarez]

Hi everyone,

I am a PhD student and I am developing a multi-agent experimental platform. One of the robots I use is the crazyflie. The platform I am developing in ROS2 and the positioning system is Vicon Tracker. My idea is that when I have consolidated all the code and operation of the robots, I can make the Github repositories public so that anyone can use them.

So far, I have successfully worked with a single crazyflie using the crazyflie-lib-python library. Below I show the code of my ROS2 node. It is a compilation from the examples and guides on the web (surely there are more efficient ways to do it).

Code: Select all

import logging
import time
import rclpy
from threading import Timer

from rclpy.node import Node
from std_msgs.msg import String
from geometry_msgs.msg import Pose
from uned_crazyflie_config.msg import StateEstimate
from uned_crazyflie_config.msg import Cmdsignal

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig

CONTROL_MODE = 'HighLevel'
"""
HighLevel: Trajectory
OffBoard: Trajectory + Position
LowLevel: Trajectory + Position + Attitude
"""
end_test = False
xy_warn = 1.0
xy_lim = 1.3
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)


class CMD_Motion():
    def __init__(self, logger):
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0
        self.thrust = 0
        self.x = 0.0
        self.y = 0.0
        self.z = 0.0
        self.logger = logger

    def ckeck_pose(self):
        # X Check
        if abs(self.x) > xy_warn:
            if abs(self.x) > xy_warn:
                self.logger.error('X: Error')
                if self.x > 0:
                    self.x = 0.9
                else:
                    self.x = -0.9
                self.logger.warning('New Point: %s' % self.pose_str_())
            else:
                self.logger.warning('X: Warning')
        # Y Check
        if abs(self.y) > xy_warn:
            if abs(self.y) > xy_warn:
                self.logger.error('Y: Error')
                if self.y > 0:
                    self.y = 0.9
                else:
                    self.y = -0.9
                self.logger.warning('New Point: %s' % self.pose_str_())
            else:
                self.logger.warning('Y: Warning')

    def str_(self):
        return ('Thrust: ' + str(self.thrust) + ' Roll: ' + str(self.roll) + ' Pitch: ' + str(self.pitch)+' Yaw: ' + str(self.yaw))

    def pose_str_(self):
        return ('X: ' + str(self.x) + ' Y: ' + str(self.y) + ' Z: ' + str(self.z)+' Yaw: ' + str(self.yaw))

    def send_pose_data_(self, cf):
        cf.commander.send_position_setpoint(self.x, self.y, self.z, self.yaw)

    def send_offboard_setpoint_(self, cf):
        cf.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust)


class Logging:
    def __init__(self, link_uri, parent):
        """ Initialize and run the example with the specified link_uri """

        self._cf = Crazyflie(rw_cache='./cache')
        self.parent = parent
        # Connect some callbacks from the Crazyflie API
        self._cf.connected.add_callback(self._connected)
        self._cf.disconnected.add_callback(self._disconnected)
        self._cf.connection_failed.add_callback(self._connection_failed)
        self._cf.connection_lost.add_callback(self._connection_lost)

        self.parent.get_logger().info('Connecting to %s' % link_uri)

        # Try to connect to the Crazyflie
        self._cf.open_link(link_uri)

        # Variable used to keep main loop occupied until disconnect
        self.is_connected = True

    def _connected(self, link_uri):
        """ This callback is called form the Crazyflie API when a Crazyflie
        has been connected and the TOCs have been downloaded."""
        self.parent.get_logger().info('Connected to %s' % link_uri)
        # The definition of the logconfig can be made before connecting
        self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=50)
        self._lg_stab.add_variable('stateEstimate.x', 'float')
        self._lg_stab.add_variable('stateEstimate.y', 'float')
        self._lg_stab.add_variable('stateEstimate.z', 'float')
        self._lg_stab.add_variable('stabilizer.roll', 'float')
        self._lg_stab.add_variable('stabilizer.pitch', 'float')
        # self._lg_stab.add_variable('stabilizer.yaw', 'float')
        self._lg_stab.add_variable('controller.cmd_thrust', 'float')
        # The fetch-as argument can be set to FP16 to save space
        # in the log packet
        # self._lg_stab.add_variable('pm.vbat', 'FP16')

        try:
            self._cf.log.add_config(self._lg_stab)
            # This callback will receive the data
            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
            # This callback will be called on errors
            self._lg_stab.error_cb.add_callback(self._stab_log_error)
            # Start the logging
            self._lg_stab.start()
        except KeyError as e:
            print('Could not start log configuration,'
                  '{} not found in TOC'.format(str(e)))
        except AttributeError:
            self.parent.get_logger().info('Could not add Stabilizer log config, bad configuration.')

    def _stab_log_error(self, logconf, msg):
        self.parent.get_logger().info('Error when logging %s: %s' % (logconf.name, msg))

    def _stab_log_data(self, timestamp, data, logconf):
        self.parent.data_callback(timestamp, data)

    def _connection_failed(self, link_uri, msg):
        self.parent.get_logger().info('Connection to %s failed: %s' % (link_uri, msg))
        self.is_connected = False

    def _connection_lost(self, link_uri, msg):
        self.parent.get_logger().info('Connection to %s lost: %s' % (link_uri, msg))

    def _disconnected(self, link_uri):
        self.parent.get_logger().info('Disconnected from %s' % link_uri)
        self.is_connected = False
        rclpy.shutdown()


class CFDriver(Node):
    def __init__(self):
        super().__init__('cf_driver')
        # Params
        self.declare_parameter('cf_uri', 'radio://0/80/2M/E7E7E7E7E7')
        # Publisher
        self.publisher_ = self.create_publisher(StateEstimate, 'cf_data', 10)
        # Subscription
        self.sub_order = self.create_subscription(String, 'cf_order', self.order_callback, 10)
        self.sub_pose = self.create_subscription(Pose, 'pose', self.newpose_callback, 10)
        self.sub_goal_pose = self.create_subscription(Pose, 'goal_pose', self.goalpose_callback, 10)
        self.sub_cmd = self.create_subscription(Cmdsignal, 'cf_cmd_control', self.cmd_control_callback, 10)
        timer_period = 0.01  # seconds
        self.iterate_loop = self.create_timer(timer_period, self.iterate)
        self.initialize()

    def initialize(self):
        self.get_logger().info('CrazyflieDriver::inicialize() ok.')
        dron_id = self.get_parameter('cf_uri').get_parameter_value().string_value
        self.get_logger().info('Crazyflie ID: %s' % dron_id)
        cflib.crtp.init_drivers()
        self.scf = Logging(dron_id, self)
        self.scf.init_pose = False
        self.scf._is_flying = False
        self.cmd_motion_ = CMD_Motion(self.get_logger())
        self.scf._cf.commander.set_client_xmode(True)
        time.sleep(2.0)
        # Disable Flow deck to EKF
        # self.scf._cf.param.set_value('motion.disable', '1')
        # Init Kalman Filter
        self.scf._cf.param.set_value('stabilizer.estimator', '2')
        # Set the std deviation for the quaternion data pushed into the
        # kalman filter. The default value seems to be a bit too low.
        self.scf._cf.param.set_value('locSrv.extQuatStdDev', 0.06)
        # Reset Estimator
        self.scf._cf.param.set_value('kalman.resetEstimation', '1')
        time.sleep(0.1)
        self.scf._cf.param.set_value('kalman.resetEstimation', '0')

    def take_off(self):
        self.get_logger().info('CrazyflieDriver::Take Off.')
        self.cmd_motion_.z = self.cmd_motion_.z + 1.0
        self.scf._is_flying = True

    def gohome(self):
        self.get_logger().info('CrazyflieDriver::Go Home.')
        self.cmd_motion_.x = 0.025
        self.cmd_motion_.y = 1.164
        self.cmd_motion_.ckeck_pose()

    def descent(self):
        self.cmd_motion_.z = 0.18
        self.get_logger().info('CrazyflieDriver::Descent.')
        t_end = Timer(2, self.take_land)
        t_end.start()

    def take_land(self):
        self.get_logger().info('CrazyflieDriver::Take Land.')
        self.cmd_motion_.z = 0.0
        self.scf._cf.commander.send_setpoint(0.0, 0.0, 0, 0)
        self.scf._cf.commander.send_stop_setpoint()
        self.scf.init_pose = False
        self.scf._is_flying = False

    def iterate(self):
        if CONTROL_MODE == 'HighLevel':
            if (self.cmd_motion_.z > 0.05 and self.scf._is_flying):
                self.cmd_motion_.send_pose_data_(self.scf._cf)
        if CONTROL_MODE == 'OffBoard':
            if self.scf._is_flying:
                self.cmd_motion_.send_offboard_setpoint_(self.scf._cf)

    def data_callback(self, timestamp, data):
        msg = StateEstimate()
        msg.timestamp = timestamp
        msg.x = data['stateEstimate.x']
        msg.y = data['stateEstimate.y']
        msg.z = data['stateEstimate.z']
        msg.roll = data['stabilizer.roll']
        msg.pitch = data['stabilizer.pitch']
        # msg.yaw = data['stabilizer.yaw']
        msg.thrust = data['controller.cmd_thrust']
        self.publisher_.publish(msg)

    def order_callback(self, msg):
        self.get_logger().info('Order: "%s"' % msg.data)
        if msg.data == 'take_off':
            if self.scf._is_flying:
                self.get_logger().info('Already flying')
            else:
                self.take_off()
        if msg.data == 'land':
            if self.scf._is_flying:
                self.descent()
            else:
                self.get_logger().info('In land')
        if msg.data == 'gohome':
            if self.scf._is_flying:
                self.gohome()
            else:
                self.get_logger().info('In land')

    def cmd_control_callback(self, msg):
        self.cmd_motion_.roll = msg.roll
        self.cmd_motion_.pitch = msg.pitch
        self.cmd_motion_.yaw = msg.yaw
        self.cmd_motion_.thrust = msg.thrust

    def newpose_callback(self, msg):
        self.scf._cf.extpos.send_extpos(msg.position.x, msg.position.y, msg.position.z)
        if not self.scf.init_pose:
            self.scf.init_pose = True
            self.cmd_motion_.x = msg.position.x
            self.cmd_motion_.y = msg.position.y
            self.cmd_motion_.z = msg.position.z
            self.get_logger().info(self.cmd_motion_.pose_str_())

    def goalpose_callback(self, msg):
        self.get_logger().info('CrazyflieDriver::New Goal Pose.')
        self.cmd_motion_.x = msg.position.x
        self.cmd_motion_.y = msg.position.y
        self.cmd_motion_.z = msg.position.z
        self.cmd_motion_.ckeck_pose()
        self.get_logger().info(self.cmd_motion_.pose_str_())


def main(args=None):
    rclpy.init(args=args)
    cf_driver = CFDriver()
    rclpy.spin(cf_driver)

    cf_driver.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

The problem happened to me when I tried to launch two crazyflies from different nodes. I have configured each drone with its corresponding address and have tested them separately and they work fine. When I launch a first node, the connection is done normally and successfully. When I launch the second node, with the first one working, the first node goes offline and the second node starts working properly. There must be some problem managing the connections. Below I show the outputs of the terminals that I use:
Terminal 1:

Code: Select all

C:\dev_ws>ros2 run uned_crazyflie_driver crazyflie_driver --ros-args -r __ns:=/dron02 -p cf_uri:=radio://0/90/2M/E7E7E7E701
1636376048.620721 [47]        452: using network interface Ethernet 3 (udp/10.196.88.156) selected arbitrarily from: Ethernet 3, Vicon
[INFO] [1636376048.780584600] [dron02.cf_driver]: CrazyflieDriver::inicialize() ok.
[INFO] [1636376048.782061300] [dron02.cf_driver]: Crazyflie ID: radio://0/90/2M/E7E7E7E701!
[INFO] [1636376049.695421100] [dron02.cf_driver]: Connecting to radio://0/90/2M/E7E7E7E701
[INFO] [1636376049.949080600] [dron02.cf_driver]: Connected to radio://0/90/2M/E7E7E7E701
[INFO] [1636376051.830017300] [dron02.cf_driver]: X: -0.369266869844768 Y: -0.18720681336609457 Z: 0.04878305984254154 Yaw: 0
Disconnected from radio://0/90/2M/E7E7E7E701
Connection to radio://0/90/2M/E7E7E7E701 lost: Too many packets lost
Traceback (most recent call last):
  File "C:\dev_ws\install\lib\uned_crazyflie_driver\crazyflie_driver-script.py", line 33, in <module>
    sys.exit(load_entry_point('uned-crazyflie-driver==0.0.0', 'console_scripts', 'crazyflie_driver')())
  File "C:\dev_ws\install\Lib\site-packages\uned_crazyflie_driver\crazyflie_driver.py", line 319, in main
    rclpy.shutdown()
  File "C:\dev\ros2_galactic\Lib\site-packages\rclpy\__init__.py", line 106, in shutdown
    _shutdown(context=context)
  File "C:\dev\ros2_galactic\Lib\site-packages\rclpy\utilities.py", line 58, in shutdown
    return context.shutdown()
  File "C:\dev\ros2_galactic\Lib\site-packages\rclpy\context.py", line 99, in shutdown
    rclpy_implementation.rclpy_shutdown(capsule)
rclpy._rclpy_pybind11.RCLError: failed to shutdown: context is zero-initialized, at C:\ci\ws\src\ros2\rcl\rcl\src\rcl\init.c:239

C:\dev_ws>ros2 run uned_crazyflie_driver crazyflie_driver --ros-args -r __ns:=/dron02 -p cf_uri:=radio://0/90/2M/E7E7E7E701
1636376100.416872 [47]       2320: using network interface Ethernet 3 (udp/10.196.88.156) selected arbitrarily from: Ethernet 3, Vicon
[INFO] [1636376100.578160600] [dron02.cf_driver]: CrazyflieDriver::inicialize() ok.
[INFO] [1636376100.579674000] [dron02.cf_driver]: Crazyflie ID: radio://0/90/2M/E7E7E7E701!
[INFO] [1636376101.496633100] [dron02.cf_driver]: Connecting to radio://0/90/2M/E7E7E7E701
[INFO] [1636376101.697315200] [dron02.cf_driver]: Connected to radio://0/90/2M/E7E7E7E701
[INFO] [1636376103.629415400] [dron02.cf_driver]: X: -0.36925592528235024 Y: -0.18720525492592363 Z: 0.04886372794516637 Yaw: 0
Disconnected from radio://0/90/2M/E7E7E7E701
Connection to radio://0/90/2M/E7E7E7E701 lost: Too many packets lost
Traceback (most recent call last):
  File "C:\dev_ws\install\lib\uned_crazyflie_driver\crazyflie_driver-script.py", line 33, in <module>
    sys.exit(load_entry_point('uned-crazyflie-driver==0.0.0', 'console_scripts', 'crazyflie_driver')())
  File "C:\dev_ws\install\Lib\site-packages\uned_crazyflie_driver\crazyflie_driver.py", line 319, in main
    rclpy.shutdown()
  File "C:\dev\ros2_galactic\Lib\site-packages\rclpy\__init__.py", line 106, in shutdown
    _shutdown(context=context)
  File "C:\dev\ros2_galactic\Lib\site-packages\rclpy\utilities.py", line 58, in shutdown
    return context.shutdown()
  File "C:\dev\ros2_galactic\Lib\site-packages\rclpy\context.py", line 99, in shutdown
    rclpy_implementation.rclpy_shutdown(capsule)
rclpy._rclpy_pybind11.RCLError: failed to shutdown: context is zero-initialized, at C:\ci\ws\src\ros2\rcl\rcl\src\rcl\init.c:239

C:\dev_ws>

Terminal 2:

Code: Select all

C:\dev_ws>ros2 run uned_crazyflie_driver crazyflie_driver --ros-args -r __ns:=/dron01 -p cf_uri:=radio://0/80/2M/E7E7E7E701
1636376068.044557 [47]       6196: using network interface Ethernet 3 (udp/10.196.88.156) selected arbitrarily from: Ethernet 3, Vicon
[INFO] [1636376068.205843100] [dron01.cf_driver]: CrazyflieDriver::inicialize() ok.
[INFO] [1636376068.207327200] [dron01.cf_driver]: Crazyflie ID: radio://0/80/2M/E7E7E7E701!
[INFO] [1636376069.140450800] [dron01.cf_driver]: Connecting to radio://0/80/2M/E7E7E7E701
[INFO] [1636376069.254667200] [dron01.cf_driver]: Connected to radio://0/80/2M/E7E7E7E701
[INFO] [1636376071.259647700] [dron01.cf_driver]: X: -0.4052272810379942 Y: 0.17168734783840148 Z: 0.05049661105599154 Yaw: 0
Disconnected from radio://0/80/2M/E7E7E7E701
Connection to radio://0/80/2M/E7E7E7E701 lost: Too many packets lost
Traceback (most recent call last):
  File "C:\dev_ws\install\lib\uned_crazyflie_driver\crazyflie_driver-script.py", line 33, in <module>
    sys.exit(load_entry_point('uned-crazyflie-driver==0.0.0', 'console_scripts', 'crazyflie_driver')())
  File "C:\dev_ws\install\Lib\site-packages\uned_crazyflie_driver\crazyflie_driver.py", line 319, in main
    rclpy.shutdown()
  File "C:\dev\ros2_galactic\Lib\site-packages\rclpy\__init__.py", line 106, in shutdown
    _shutdown(context=context)
  File "C:\dev\ros2_galactic\Lib\site-packages\rclpy\utilities.py", line 58, in shutdown
    return context.shutdown()
  File "C:\dev\ros2_galactic\Lib\site-packages\rclpy\context.py", line 99, in shutdown
    rclpy_implementation.rclpy_shutdown(capsule)
rclpy._rclpy_pybind11.RCLError: failed to shutdown: context is zero-initialized, at C:\ci\ws\src\ros2\rcl\rcl\src\rcl\init.c:239

C:\dev_ws>ros2 run uned_crazyflie_driver crazyflie_driver --ros-args -r __ns:=/dron01 -p cf_uri:=radio://0/80/2M/E7E7E7E701
1636376116.318798 [47]      14232: using network interface Ethernet 3 (udp/10.196.88.156) selected arbitrarily from: Ethernet 3, Vicon
[INFO] [1636376116.481263800] [dron01.cf_driver]: CrazyflieDriver::inicialize() ok.
[INFO] [1636376116.482745900] [dron01.cf_driver]: Crazyflie ID: radio://0/80/2M/E7E7E7E701!
[INFO] [1636376117.382589000] [dron01.cf_driver]: Connecting to radio://0/80/2M/E7E7E7E701
[INFO] [1636376117.501576800] [dron01.cf_driver]: Connected to radio://0/80/2M/E7E7E7E701
[INFO] [1636376119.508944000] [dron01.cf_driver]: X: -0.405229687418066 Y: 0.17167834828075124 Z: 0.05047674811465245 Yaw: 0

I hope someone knows where I'm wrong. The idea of ​​this implementation is to be able to launch new crazyflies at any time during the trial. For this reason I haven't used the swarm class. If this implementation is not possible, I will choose to create another node within the repository for when swarms are launched (drones can be assigned as an attribute of the node).

Regards

[kimberly]

Hi!

In this case you probably have to use one crazyradio per crazyflie if you don't want to use the swarm class. Then you just have to change the first number of the URI from 0 to 1 to use the second detected crazyradio and so on.

If you are doing anything with ROS, we would highly recommend you to use Crazyswarm if you haven't come across it. A ROS2 port of that package is in the works right now, so perhaps you would like to help out in testing that?

[fjmanasalvarez]

Great, I'll give it a try these days to confirm.

I have seen and used the Crazyswarm project in ROS Melodic (and now in Noetic). What I have not seen (and have been looking for a long time) is Crazyswarm2. It is good to know and I will try it, of course. In my opinion, maybe it would be good to add some indication on the Crazyswarm page to the Crazyswarm2 version to improve its visibility or add tags in github to make it easier to find.

[kimberly]

Yes definitely agree, but I guess the reason why this is not visible yet is that the port is not completely finished. Once it is fully converted, the authors will make it more public.