How to implement real-time interrupts? about mavsdk-python HOT 29 CLOSED

@joe-marrone
Already been said here, but perhaps in not so many words ...
There are two ways to do what you want.

1. Use missions.
   • Have behaviour defined in a mission(s). If you need to change behaviour in response to some input you can upload a new mission, OR create a mission that has all your possible operations defined and use changing the sequence number to move to different "subroutines" within the mission. The former is far more flexible because you don't need to know your operations in advance, and can be plenty fast, particularly if your missions are small and are coming from the companion.
   • Remember that the system will continue to execute the current mission waypoint during upload and won't update to the new mission until the mission uploaded successfully and you reached the next waypoint (something to think about - you can pause mission/update sequence number to fix this).
2. Use Gotos and command protocol
   • Essentially you can execute gotos using the API.
   • Send commands to execute specific actions (ie trigger camera)

I tend to recommend using missions because the set of supported commands and the way they operate is well understood - the set of commands supported outside missions is not so well documented.

from mavsdk-python.

@JonasVautherin goto() goes to a place and stops there. It just does a MAVLink reposition command.

@julianoes I edited my comment above which you can now ignore (if you got via email). so you're using https://mavlink.io/en/messages/common.html#MAV_CMD_DO_REPOSITION
After this you just revert to HOLD mode then?

from mavsdk-python.

@JonasVautherin @joe-marrone The trajectory protocol has been implemented on PX4 (only) in the obstacle avoidance protocol. Jonas, yes, modifies the current mission, based on a current/next waypoint provided by the vehicle.

There is detailed information on how it works here.
NOte:

• Trajectory messages are still marked as work in progress - this could change, breaking whatever you do.
• The trajectory messages are generic, but PX4's implementation is specific to missions. The path still follows the original mission, just moving around obstacles.

You could just use offboard mode to push the vehicle where you want. Collision avoidance uses this approach here.

This is somewhat as described in previous posts - I personally prefer using missions, because as I said, the subset of supported commands is smaller in command protocol than mission protocol. I still don't understand what it is about your use case that means this is not the best option.

from mavsdk-python.

@julianoes thanks I should have seen that.

from mavsdk-python.

So I can see the need to get notified about keyboard or on-board sensor signals to then change the mission, and this can be accomplished using the SDK (at least in my opinion).

However, I don't think what you need is actual interrupts. Real time interrupts, as far as I know, are very low level and might be used on an OS level or when interfacing to sensors in the drivers. So usually this is for scheduling or just reducing latency to very low numbers. This might be applicable in the case where you have a gyro loop running at 1 kHz and you need super fast response times in order to control attitude as smooth as possible, however, higher level mission planning is not as time critical because the dynamics are way slower. For instance if you change a mission, or request a vehicle to stop it will take some time in the range of 1/10 seconds for the vehicle to slow down anyway, so you don't need microseconds latency for the sensor event to trigger, a couple of milliseconds should be fine.

I assume that you should be able trigger an event in Python asyncio and then react to it.
For the keyboard, you could use something like this: https://stackoverflow.com/questions/35223896/listen-to-keypress-with-asyncio

from mavsdk-python.

Thanks for the comments about real time interrupts. For now I just want to be able to stop a mission in progress and redirect it. My first attempt was to use mission.py as the starting point. I would like to have the mission stop at each waypoint and ask the question Do you want to continue the mission?. Based on the keyboard response, either continue the current mission, or divert into a new mission. I am looking for a way to intercept the async def run(): task and insert a pause at each waypoint to ask the question. So far nothing I have tried works.

This is what the script looks like (You'll note that I added one more waypoint to the sample script and changed it to my coordinates):

#!/usr/bin/env python3
# aaa

import asyncio

from dronecode_sdk import connect as dronecode_sdk_connect
from dronecode_sdk import (MissionItem)

drone = dronecode_sdk_connect(host="127.0.0.1")


async def run():
    mission_items = []
    mission_items.append(MissionItem(42.7122310,
                                     -71.4605572,
                                     25,
                                     10,
                                     True,
                                     float('nan'),
                                     float('nan'),
                                     MissionItem.CameraAction.NONE,
                                     float('nan'),
                                     float('nan')))
    mission_items.append(MissionItem(42.7122330,
                                     -71.4610000,
                                     25,
                                     10,
                                     True,
                                     float('nan'),
                                     float('nan'),
                                     MissionItem.CameraAction.NONE,
                                     float('nan'),
                                     float('nan')))
    mission_items.append(MissionItem(42.7124330,
                                     -71.4610500,
                                     25,
                                     10,
                                     True,
                                     float('nan'),
                                     float('nan'),
                                     MissionItem.CameraAction.NONE,
                                     float('nan'),
                                     float('nan')))
    mission_items.append(MissionItem(42.7124500,
                                     -71.4600000,
                                     25,
                                     10,
                                     True,
                                     float('nan'),
                                     float('nan'),
                                     MissionItem.CameraAction.NONE,
                                     float('nan'),
                                     float('nan')))
    await drone.mission.set_return_to_launch_after_mission(True)

    print("-- Uploading mission")
    await drone.mission.upload_mission(mission_items)

    print("-- Arming")
    await drone.action.arm()

    print("-- Starting mission")
    await drone.mission.start_mission()


async def print_mission_progress():
    async for mission_progress in drone.mission.mission_progress():
        print(f"Mission progress: {mission_progress.current_item_index}/{mission_progress.mission_count}")


async def observe_is_in_air():
    """ Monitors whether the drone is flying or not and
    returns after landing """

    was_in_air = False

    async for is_in_air in drone.telemetry.in_air():
        if is_in_air:
            was_in_air = is_in_air

        if was_in_air and not is_in_air:
            await asyncio.get_event_loop().shutdown_asyncgens()
            return

async def pause_mission():
    pause = input("Continue Mission: Y/N?" + "\n")
    if str(pause) == 'y':
        print("Continuing Mission")
    else:
        print("Starting new mission" + "\n")
        await asyncio.sleep(2)
        mission_items.append(MissionItem(42.7123000,
					     -71.4606000,
					     15,
					     10,
					     True,
					     float('nan'),
					     float('nan'),
					     MissionItem.CameraAction.NONE,
					     float('nan'),
					     float('nan')))
        await asyncio.sleep(3)
        return

def setup_tasks():
    asyncio.ensure_future(run())
    asyncio.ensure_future(print_mission_progress())
    asyncio.ensure_future(pause_mission())

if __name__ == "__main__":
    setup_tasks()
    asyncio.get_event_loop().run_until_complete(observe_is_in_air())

The mission actually completed with errors as seen in the following response:

joe-pl@joe-PL:~/DronecodeSDK/DronecodeSDK-Python/examples$ python3.6 mission_4_wp-pause.py
Continue Mission: Y/N?
n
Starting new mission

-- Uploading mission
-- Arming
-- Starting mission
Task exception was never retrieved
future: <Task finished coro=<pause_mission() done, defined at mission_4_wp-pause.py:85> exception=NameError("name 'mission_items' is not defined",)>
Traceback (most recent call last):
  File "mission_4_wp-pause.py", line 93, in pause_mission
    mission_items.append(MissionItem(42.7123000,
NameError: name 'mission_items' is not defined
Mission progress: 0/4
Mission progress: 1/4
Mission progress: 2/4
Mission progress: 3/4
Mission progress: 4/4
joe-pl@joe-PL:~/DronecodeSDK/DronecodeSDK-Python/examples$ 

from mavsdk-python.

Yes that's not really how it works, I'm afraid.

A mission doesn't have a concept of pause until new instructions arrive. What you could do is wait for the mission progress to update and then do mission_pause. Once you want to continue you do mission_start again. This is slightly clunky because it means you depend on the mission progress subscription which is not guaranteed to work. E.g. when the link drops, you don't get it.

Therefore, it seems to me you might be better off by using Action::goto which means you send it manually somewhere. Once arrived you can check if you're good to continue and do the next goto.

from mavsdk-python.

OK. But then how do you people use lidar sensors or other sensors that control where the drone positions itself? It seems to me that the drone is flying an autonomous mission but modifies that mission based on an input from an external source, not from the stored mission. That's what I want to simulate with SITL using Python. Is this not possible?

Joe

from mavsdk-python.

That's not an easy thing to do actually and right now for obstacle avoidance we are doing some "hacks" in the position controller, see:
https://github.com/PX4/Firmware/blob/6672284871d4cd4163e99d68e75831d0495f58c8/src/modules/mc_pos_control/mc_pos_control_main.cpp#L737-L740

Maybe we could add an API for the Dronecode SDK which allows you to set obstacles and avoidance trajectories to be flown.

from mavsdk-python.

Somehow I was under the impression that it was possible to use a companion computer to run Python scripts to feed information, like new waypoints, to the Pixhawk in real time. Am I wrong in the assumption?

For the time being, I will try a script that does a mission one waypoint at a time, stop, and request a new waypoint that will be entered manually. That way I will be managing the mission in "faux" real time.
Any thoughts about doing it this way?

And I would vote for an API that provides functionality allowing missions to be paused and modified by a Python script (running on a companion computer) based on external data, whether is comes from the keyboard or an on-board sensor. Currently, standard off-the-shelf products like the DJI drone series have this capability programmed in, so it's been done already. However, most of this capability is proprietary as you know, hence unavailable to designers like me trying to develop a specific type of solution.

Thanks,
Joe

from mavsdk-python.

Somehow I was under the impression that it was possible to use a companion computer to run Python scripts to feed information, like new waypoints, to the Pixhawk in real time.

Sure, depending on what you mean with "in real time".

allowing missions to be paused

You can pause missions as I wrote above.

and modified by a Python script (running on a companion computer) based on external data

You can always modify your mission but right now you need to re-upload the whole mission which might take some time. For the case of a companion computer it might not be too bad because you have a high bandwidth, stable connection.

DJI drone series have this capability programmed in

Ok, so they can stop at each waypoint and ask you to confirm? I didn't know that.

from mavsdk-python.

Let me approach this another way. When the battery is low enough that the mission has to be aborted, PX4 will interrupt the mission in real-time and change it to RTL. Right? Essentially the firmware gets a signal from the battery sensor that it is time to land, and changes the mission. What I want to do is provide a similar kind of interrupt, but from some external signal, like a keyboard input or another type of sensor. That way I can modify the mission on-the-fly in case it is necessary. And the nature of this interrupt would not require micro or millisecond response. The response time would be on the same order of magnitude as a regular mission making a decision to change from one waypoint to the next. I hope that makes things clearer.
Also, I tried to find information on Action::goto but could not find any. Can you point me to a reference?

Thanks,
Joe

from mavsdk-python.

PX4 will interrupt the mission in real-time and change it to RTL. Right?

If the parameters are set the right way, yes. If it's terribly low it will just land.

The response time would be on the same order of magnitude as a regular mission making a decision to change from one waypoint to the next.

Yes, that should totally be doable. The only caveat is that you need to re-upload the mission (or the next waypoint) but that's ok.

Can you point me to a reference?

The C++ reference is here:
https://sdk.dronecode.org/en/api_reference/classdronecode__sdk_1_1_action.html#classdronecode__sdk_1_1_action_1ab6bcb58fdac1aa7e699f89ed605c7666
I'm not sure about the Python one. @JonasVautherin do we have Python reference docs already?

from mavsdk-python.

I'm not sure about the Python one. @JonasVautherin do we have Python reference docs already?

We don't have a Python one yet. Right now we must rely on the examples and the proto files. Looking at action.proto, goto() is not yet implemented. It will come at some point for sure, but I'm not sure if that would solve this issue (or would it?).

from mavsdk-python.

I could be one way to solve the issue. The alternative is to just set one more waypoint every time.

from mavsdk-python.

So goto() interrupts the mission and goes back to hold mode, right?

from mavsdk-python.

I had another thought. When I fly an autonomous mission on the real drone and switch my transmitter to another flight mode, say MANUAL, or ALT, the mission stops immediately. Can I capture the way that's done using a python script? Let's say I'm flying a mission on the SITL and I inject a keyboard command that emulates a switch to ALT mode so the mission stops and it holds position. Then once it has stopped be able to give it a new mission from the keyboard, and make the new mission selectable depending on which key is pressed.

My ultimate goal is to be able to create scenarios in which I can simulate a drone that's flying mission A, stop the mission by sending a command to the drone, then direct it to start mission B. If I can verify that this works on the SITL, then I want to use that same Python script on a Raspberry Pi to control the actual drone.

I know I'm harping on this issue, but this type of functionality will open up new ways to control drones that are used for something more than just taking pictures and videos. Drones doing real work will have to "think for themselves" which will require mission changes all the time. We're getting closer to the Star Wars version of drones being the servants of mankind ;^)

Joe

from mavsdk-python.

@JonasVautherin goto() goes to a place and stops there. It just does a MAVLink reposition command.

from mavsdk-python.

@joe-marrone

ALT mode so the mission stops and it holds position

ALT mode hold altitude, you probably mean POSCTL to hold position. Otherwise, the wind will push you away.

emulates a switch to ALT mode so the mission stops

Yes, as written above, you do mission_pause() and it stops.

We're getting closer to the Star Wars version of drones being the servants of mankind ;^)

😄

from mavsdk-python.

@team: I found this in the MAVLink Developer Guide:

The path planning protocol (a.k.a. trajectory interface) is a general-purpose protocol for a system to
 request dynamic path planning from another system (i.e. for an autopilot to request a path from a
 companion computer).

The protocol is primarily intended for cases where constraints on the path to a destination are unknown
 or may change dynamically, but it can also be used for any other path management activities. Examples
 include: obstacle avoidance when following a preplanned mission, determining paths for self 
forming/healing swarms, offloading geofence management to a companion computer, etc.

This sounds exactly like what I want to do.

Comments?

Joe

from mavsdk-python.

Are you wanting to make some kind of obstacle avoidance?

I am not super familiar with the path planning protocol, but in my understanding, it will not allow you to define a new mission. It is more about modifying the trajectory to the next waypoint (at least that's how it is used in obstacle avoidance).

This is not (yet) supported in the SDK, though. Would you be willing to contribute? First step would be to define the API, then write the C++ implementation and then it would get auto-generated in Python :-).

from mavsdk-python.

Thanks for the replies. OK, so the trajectory protocol is not supported. The phrase that caught my attention is path management activities. That sounds like more than just collision avoidance. I'd love to jump in and contribute, but I'm not a programmer. Instead I will have to use existing supported tools.

My goal for the "change mission" approach is related to applications where the original mission is no longer needed. It needs to be stopped where it is, and redirected using a new mission. I'm looking for something like an RTL command that immediately stops a mission and allows another "mission" to start, which in the case of RTL may not be considered a mission as such, but why couldn't a command like RTL be used to trigger loading another real mission stored on a companion computer?

An example would be something like this. I've started a mission that takes the drone out to a remote point to look for a missing person. While under way, updated information says that a different location should be investigated. We need to interrupt the current mission and redirect the drone. I could develop a new mission on QGC and upload it, but I want to do this autonomously. How? By uploading the new coordinates directly to the companion CPU where a Python script takes over and redirects the mission. This is more efficient and saves time.

This may sound like an artificial and trivial example, but I hope it gets the idea across.

Joe

from mavsdk-python.

but why couldn't a command like RTL be used to trigger loading another real mission stored on a companion computer?

I wouldn't use RTL, just use mission_pause from the SDK, upload the new mission and continue.

from mavsdk-python.

@julianoes
Thanks, I'll try that.
Another question about the mission.py code. In this snippet, where is is_in_air and was_in_air` defined?

async def observe_is_in_air():
    """ Monitors whether the drone is flying or not and
    returns after landing """

    was_in_air = False

    async for is_in_air in drone.telemetry.in_air():
        if is_in_air:
            was_in_air = is_in_air

        if was_in_air and not is_in_air:
            await asyncio.get_event_loop().shutdown_asyncgens()
            return

Joe

from mavsdk-python.

@joe-marrone what do you mean defined? The information comes from drone.telemetry.in_air() which returns a bool.

This is Python, so you don't need a declaration, if that's what you mean.

from mavsdk-python.

Closing due to inactivity, please reopen (or open a new issue) if needed.

from mavsdk-python.

@joe-marrone I'm finding myself wanting to accomplish the same task as you here. What solution did you end up going with?

from mavsdk-python.

from mavsdk-python.

That's unfortunate. Thanks for your response!

from mavsdk-python.