IMRCLab/crazyswarm2

cf.get_position() in simulation

開放

#815 建立於 2026年1月26日

 (6 則留言) (0 個反應) (0 位負責人)Python (126 個分叉)auto 404
enhancementgood first issue

倉庫指標

星標
 (244 顆星)
PR 合併指標
 (PR 指標待抓取)

描述

We would like to use get_position() in the simulation. However, we are currently unable to use this function. Is there a way to use this feature of crazyswarm2. The crazyswarm2 website says this function is not available for simulation. Can we activate this function by configuring the related scripts? We would like to test our swarm scenario in the simulation environment first.

Note: This function was working with crazyswarm1 properly in the simulation.

Update: The terminal shows all position data are zero:

:~/ros2_ws$ ros2 launch crazyflie_examples launch.py script:=hello_world backend:=sim
[INFO] [launch]: All log files can be found below /home/ntukenmez3/.ros/log/2026-01-27-14-58-20-648042-ae-icps-407120-376260
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [crazyflie_server-1]: process started with pid [376261]
[INFO] [teleop-2]: process started with pid [376263]
[INFO] [joy_node-3]: process started with pid [376265]
[INFO] [rviz2-4]: process started with pid [376267]
[INFO] [hello_world-5]: process started with pid [376269]
[teleop-2] [INFO] [1769543900.736940773] [teleop]: Mode changed to high_level
[crazyflie_server-1] 2026-01-27 14:58:21.000 [RTPS_TRANSPORT_SHM Error] Failed init_port fastrtps_port17661: open_and_lock_file failed -> Function open_port_internal
[crazyflie_server-1] 2026-01-27 14:58:21.000 [RTPS_TRANSPORT_SHM Error] Failed init_port fastrtps_port17663: open_and_lock_file failed -> Function open_port_internal
[crazyflie_server-1] 2026-01-27 14:58:21.001 [RTPS_TRANSPORT_SHM Error] Failed init_port fastrtps_port17667: open_and_lock_file failed -> Function open_port_internal
[hello_world-5] [INFO] [1769543901.295114367] [hello_world]: [Status] Taking off...
[crazyflie_server-1] [INFO] [1769543901.312688293] [crazyflie_server]: [cf1] takeoff(height=1.0 m,duration=2.5 s,group_mask=0)
[rviz2-4] [INFO] [1769543901.339869182] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-4] [INFO] [1769543901.339977121] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-4] [INFO] [1769543901.374813937] [rviz2]: Stereo is NOT SUPPORTED
[hello_world-5] [INFO] [1769543907.084053230] [hello_world]: [Status] Hovering - Live Position Data:
[hello_world-5] [INFO] [1769543907.084361694] [hello_world]: Position -> X: 0.00 | Y: 0.00 | Z: 0.00
[hello_world-5] [INFO] [1769543907.480641656] [hello_world]: Position -> X: 0.00 | Y: 0.00 | Z: 0.00
[hello_world-5] [INFO] [1769543907.881262823] [hello_world]: Position -> X: 0.00 | Y: 0.00 | Z: 0.00
[hello_world-5] [INFO] [1769543908.278586090] [hello_world]: Position -> X: 0.00 | Y: 0.00 | Z: 0.00
[hello_world-5] [INFO] [1769543908.677177133] [hello_world]: Position -> X: 0.00 | Y: 0.00 | Z: 0.00
[hello_world-5] [INFO] [1769543909.081072246] [hello_world]: Position -> X: 0.00 | Y: 0.00 | Z: 0.00
[hello_world-5] [INFO] [1769543909.489809147] [hello_world]: Position -> X: 0.00 | Y: 0.00 | Z: 0.00
[hello_world-5] [INFO] [1769543909.891654133] [hello_world]: Position -> X: 0.00 | Y: 0.00 | Z: 0.00

hello_world.py is:

from crazyflie_py import Crazyswarm
import numpy as np

TAKEOFF_DURATION = 2.5
HOVER_DURATION = 5.0

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    cf = swarm.allcfs.crazyflies[0]
    
    # Initialize the logger
    logger = swarm.allcfs.get_logger()

    # 1. Takeoff
    logger.info("[Status] Taking off...")
    cf.takeoff(targetHeight=1.0, duration=TAKEOFF_DURATION)
    timeHelper.sleep(TAKEOFF_DURATION)

    # 2. Hover and Print Position
    logger.info("[Status] Hovering - Live Position Data:")
    
    # Note: ROS2 loggers don't support end='\r' effectively.
    # We will print the status at a slightly lower frequency to avoid flooding the console.
    frequency = 5 
    for _ in range(int(HOVER_DURATION * frequency)):
        pos = cf.get_position()
        # logger.info handles the newline automatically
        logger.info(f"Position -> X: {pos[0]:.2f} | Y: {pos[1]:.2f} | Z: {pos[2]:.2f}")
      #  logger.info(f"Position -> X: {pos[5]:.2f}")
        timeHelper.sleep(1.0 / frequency)

    # 3. Land
    logger.info("[Status] Landing...")
    cf.land(targetHeight=0.04, duration=2.5)
    timeHelper.sleep(2.5)
    logger.info("[Status] Mission Complete.")

if __name__ == '__main__':
    main()

貢獻者指南