How to access real time pose data to implement feedback based control?

[gaurav713914]

We have tried to figure out how to access real time pose data using the crazyflie_server.cpp backend to implement feedback based control. We also tried getting data from the topic "/poses" using the following python script

# from pycrazyswarm import Crazyswarm
from crazyflie_py import Crazyswarm

from motion_capture_tracking_interfaces.msg import NamedPoseArray, NamedPose
from geometry_msgs.msg import Pose

import time
import rclpy
from rclpy.node import Node

TAKEOFF_DURATION = 2.5
HOVER_DURATION = 5.0

class Log_Pose(Node):

    def __init__(self):
        super().__init__('log_pose')
        self.subscription = self.create_subscription(NamedPose, '/poses', self.on_logging_pose, 10)
        self.NamedPoseArraymsg = NamedPose()

    def on_logging_pose(self):
        print(self.NamedPoseArraymsg.pose)

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper

    pose = Log_Pose()
    pose.on_logging_pose()

    dur = time.time() + 10

    while time.time() < dur:
        rclpy.spin_once(pose)

if __name__ == "__main__":
    main()

which gave the following output:

Is this a valid method to get the pose data to use in our feedback based control law? It'd be great if you could suggest a fix to this issue and direct us towards being able to get the real time pose data.

[gaurav713914]

We have got the data from crazyflie_server logging. The data is coming at the default publishing rate of 10 Hz. We want the publishing rate to be higher, our motion capture system is currently capturing at the rate of 100 Hz and supports up-to 240 Hz.

It will be good if we can use /poses topic to get the data so that we do not need to have different subscriber for each crazyflie data.

[whoenig]

This can be adjusted in the config file: https://github.com/IMRCLab/crazyswarm2/blob/main/crazyflie/config/crazyflies.yaml#L79 (Note: the firmware does not support rates higher than 100 Hz without modifications).

[gaurav713914]

Thank you for your help, we are now able to run unicycle model with feedback!