[as2_platform_tello] position and velocity control

Hey,

So we've had success in hooking the tello up to optitrack via aerostack and are playing around with the velocity and position controller using the teleop.

We're getting some circling for position control and poor following behaviour in velocity control mode. In particular

In Velocity Control Mode with the teleop panel. If you give a velocity in direction 1 using an arrow key and after a bit press hover, then give it another direction2, it will stillfly in direction 1 for a short period before going to direction2
In position control Mode. After tuning the PID down to 1.5, the first time you request it to move to a location, it will move to it but overshoot and begin circling. Subsequent move requests appear to be ignored

I'm not sure if these are issues with the teleop panel, PID controller or the tello itself!

Have you guys had any luck with tello flight? Have you guys had a go at re-tuning the PID for the tello?

P.s. I found you need to also bind to local port so you can automatically reconnect to the tello after a restart!

Additional Question - with the python control script, whats the best way to abort a go_to motion? When testing it's drifting a lot so I ctrl+c the script, but that doesnt stop the go_to since its blocking! I use the teleop to land it, but then I have to restart the whole stack to fly with the script again.

Cheers guys, Mickey

Hi @mhl787156,

Some errors have been fixed in the platform_update branch. We have tested it with OptiTrack, and it works well. In the following days, we will update the project and add this to the documentation.

We tested this, and the control using the keyboard was good enough. The control response had a bit of a delay, but the hover function worked well.
For the testing, we didn't modify the controller parameters.

We encourage you to test this branch of the as2_platform_tello. Just some parameter changes.

Regarding the additional question, what do you mean by the Python control script? With keyboard teleoperation, you can open a behavior control window where you can pause all active behaviors, in which case it will enter hover mode. If you test the new branch, check if you can relaunch everything after a land. It should be possible, if not, let us know

Hi @RPS98

Amazing thanks for that, am pulling and testing it now.

Most things seem to be working (Had to update the config, platform_config_file is now config_file. One bit which seems to have broken is the state estimator config parameter file no longer reads the drone specific values (though I recognise thats probably because i've just updated aerostack core after a month). When running, the /tello2/state_estimator.rigid_body_name is now set to the default of '1'. The /** parameters are actually read though. Used to work well, has something changed? I checked the src for state_estimator and things look the same...

/**:
  ros__parameters:
    use_ignition_tf: False
    mocap_topic: "/optitrack/rigid_bodies"
    twist_smooth_filter_cte: 0.1
    orientation_smooth_filter_cte: 0.1

/tello1/state_estimator:
  ros__parameters:
    rigid_body_name: "tello1"

/tello2/state_estimator:
  ros__parameters:
    rigid_body_name: "tello2"

Ah so by python control script, I mean that I am using the as2_python_api to write a python script to control the drone to do things. I would like the ability to stop the script and for the drone to automatically land. Previously I was finding that the teleop would override and stop further take-offs. Will see how the new code works once I can launch it properly!

On connection, I also manage to fly with teleop, but every now and again I think the tello disconnects by throwing an invalid argument error. Reconnects just fine if I just rerun the command. Thought i'd let you know.

ros2 launch as2_platform_tello tello_platform.launch.py namespace:=tello2 use_sim_time:=false config_file:=config/platform.yaml
[INFO] [launch]: All log files can be found below /home/mickey/.ros/log/2024-06-20-16-37-42-289445-mickey-delta-laptop-1006191
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [as2_platform_tello_node-1]: process started with pid [1006198]
[as2_platform_tello_node-1] [INFO] [1718897862.784982081] [tello2.platform]: Construct with name [platform]
[as2_platform_tello_node-1] [INFO] [1718897862.789039922] [as2_mode]: Control mode: UNSET 
[as2_platform_tello_node-1] [INFO] [1718897862.789076052] [as2_mode]: Control mode: HOVER YAW_ANGLE BODY_FLU_FRAME 
[as2_platform_tello_node-1] [INFO] [1718897862.789085824] [as2_mode]: Control mode: SPEED YAW_SPEED BODY_FLU_FRAME 
[as2_platform_tello_node-1] [INFO] [1718897862.804768249] [tello2.platform]: Connecting to Tello at 192.168.1.42:8889
[as2_platform_tello_node-1] [INFO] [1718897862.947521027] [tello2.platform]: Tello SDK version: 30
[as2_platform_tello_node-1] [INFO] [1718897869.475551995] [tello2.platform]: Transition [ARM] : New State [LANDED]
[as2_platform_tello_node-1] [INFO] [1718897872.223261381] [tello2.platform]: Transition [TAKE_OFF] : New State [TAKING_OFF]
[as2_platform_tello_node-1] [INFO] [1718897877.063888657] [tello2.platform]: Transition [TOOK_OFF] : New State [FLYING]
[as2_platform_tello_node-1] [INFO] [1718897877.124797481] [tello2.platform]: Flight Time: 4s
[as2_platform_tello_node-1] 
[as2_platform_tello_node-1] [WARN] [1718897877.156497548] [tello2.platform]: Invalid transition: FLYING -> TOOK_OFF
[as2_platform_tello_node-1] [INFO] [1718897892.985031806] [tello2.platform]: Flight Time: 20s
[as2_platform_tello_node-1] 
[as2_platform_tello_node-1] [INFO] [1718897908.998068365] [tello2.platform]: Flight Time: 36s
[as2_platform_tello_node-1] 
[as2_platform_tello_node-1] [INFO] [1718897924.962724737] [tello2.platform]: Flight Time: 52s
[as2_platform_tello_node-1] 
[as2_platform_tello_node-1] [INFO] [1718897940.976238834] [tello2.platform]: Flight Time: 68s
[as2_platform_tello_node-1] 
[as2_platform_tello_node-1] terminate called after throwing an instance of 'std::invalid_argument'
[as2_platform_tello_node-1]   what():  stoi
[ERROR] [as2_platform_tello_node-1]: process has died [pid 1006198, exit code -6, cmd '/home/mickey/ros2/delta_aerostack2_ws/install/as2_platform_tello/lib/as2_platform_tello/as2_platform_tello_node --ros-args -r __node:=platform -r __ns:=/tello2 --params-file config/platform.yaml --params-file /tmp/launch_params_uyy_1vt8 --params-file /tmp/launch_params_6w5f9t38'].

Hi @mhl787156,

Regarding the state estimator parameters, we are making changes in aerostack2, specifically in PR525. Currently, the version in main does not allow specifying namespace or node in the configuration file that is passed. Sorry for the inconvenience, we are resolving that bug right now and hope to fix it soon.

I recommend that you temporarily pass a configuration file to each instance, where you change the rigid body name. You can create something like state_estimator_tello1.yaml and state_estimator_tello2.yaml, and in tmuxinator, select the file based on the namespace.

On the other hand, we had not detected that issue about the stoi exception. Thanks for reporting it, we will try to replicate and debug it.

Thank you very much for your feedback, it is very helpful!

I also wanted to ask if you have managed to fly two Tello drones simultaneously, and what setup you are using for it. We have not been able to perform this test as we do not have two Tellos with the v2 firmware version to connect them to the same access point. Our main question is about camera access, how to manage the reading direction of each one. Thank you very much for your help.

Hi @RPS98

Ah no worries! I thought it might be something like that - yes I will do separate files for now :)

For two drones - we have the Tello EDU's so we put them both into AP mode and connect to them over the network. However we have not attempted to access the video streams yet. I used to be aware that AP mode would actually disable the video stream. However I recently came across the port command in sdk 3.0 which seems to be able to switch the tello port for the video stream which might actually resolve that. It's on my list of things to test. The backup plan was essentially to buy a lot of wifi dongles and connect one tello to each.

I'll be testing this in the following few weeks so will keep you posted. I believe the EDUs can be updated to sdk3.0 (mine seem to be)

I wanted to double check your axis conventions between optitrack, controller, tello and teleop since I think something is messed up on my end - when I try and teleop in pose mode, the drone just ends up circling the target location, but never getting there!

When using velocity mode on teleop, I assume the velocity directions are relative to tello/odom, and not the tello/base_link? Since I can start the drone facing in any direction and it will still perform actions relative to that. I'm confused because when I plot the motion_reference twists, it doesn't match the self_localization twist. In fact it seems that x and y axis are switched or reversed? Can you clarify what frame things are in?

Screenshot from 2024-06-21 12-55-42

Following on from this then one possibility is I'm missing an axis conversion from optitrack into ROS. I'm using the mocap4r2_optitrack and I notice they do not rearrange the axis before sending to ROS2. In Optitrack i'm using the natnet and have set up-axis to Z, does this match your setup?
Does the pose mode of your teleop work fine with no issues?

Thanks guys, just me working on this, so I really appreciate your time :)

Just additional. Trying to get the drone to fly to [0, 0, height] and it just goes in the opposite direction! Again potentially something wrong with my axis.

python script is the following:

def drone_run(drone_interface: DroneInterface):
    """ Run the mission """

    speed = 0.5
    takeoff_height = 1.0
    height = 1.0
    sleep_time = 2.0

    ##### ARM OFFBOARD #####
    print("Arm")
    sleep(sleep_time)
    drone_interface.arm()
    sleep(sleep_time)

    ##### TAKE OFF #####
    drone_interface.takeoff(takeoff_height, speed=1.0)
    sleep(sleep_time)

    drone_interface.go_to.go_to_point([0, 0, height], speed=speed)

    ##### LAND #####
    print("Landing")
    drone_interface.land(speed=0.5)
    print("Land done")

    drone_interface.disarm()

def main():
    """ Main """
    rclpy.init()
    uav = DroneInterface(drone_id="tello2", verbose=False, use_sim_time=False)
    drone_run(uav)
    uav.shutdown()
    rclpy.shutdown()

    print("Clean exit")

if __name__ == '__main__':
    main()

[Side note, if it fails and I have to ctrl+c the vehicle during the go_to, it seems I need to restart the drone before I can re-run this test python script]

The issue with the drone flying in circles is often caused by a mismatch between the orientation received from the OptiTrack and the one aerostack2 thinks it has. To verify this, I recommend using RViz without flying the drone, and checking that the transformations between earth, map, odom, and base_link are correct.

When in doubt about the frame_id of the messages, you can check their frame_id in their header. The keyboard_teleop publishes the references in earth, and the self-localization twist is in base_link, which is why the graphs don't match, as they depend on the drone's orientation. Only if the drone faces the x-axis of earth will the graphs align.
You should verify this with RViz, and keep in mind that when creating the rigid body in OptiTrack, the x-axis of OptiTrack should align with the forward direction of the drone, and the z-axis of OptiTrack should align with the up direction of the drone.
We have tested both the pose and the twist, and both work correctly. We have also tested the project's mission.py. However, we still need to conduct some additional tests.

Lastly, yes, it is necessary to restart the platform when the node disconnects, as the Tello drone does not allow rebinding.

Hi @RPS98

Thanks for confirming my suspicions! After investigating I actually think that the Earth frame is off 90 degrees CCW around z!

If I understand correctly. Ros2 is X forward, y left and z up. But in this image you see that forward comes out the side of the drone! (Would explain why teleop-ing left causes the drone to move forward, hence the spiral)

How is Earth initialised and can the orientation be adjusted? It looks like it is implicitly defined by the static transform to tello2/map.

I've been trying to set a static transform between earth and map (optitrack) but that doesn't seem to change the earth orientation.

EDIT!

Ah! You mentioned this in point 2! I have my drone orientation wrong! sorry my bad it didn't click until I just looked at it again - let me test.

It works now! Thank you, my optitrack arena axis was indeed rotated 90 degrees off. Thanks for all the help!

aerostack2 / as2_platform_tello

[as2_platform_tello] position and velocity control #7