Blueye SDK Mission Planner going to abort after starting mission

[Urlaxle]

Describe the bug
Following the example found in mission.py we attempted to run a mission where we went to the seafloor, took an image, and went back up to the surface. When running our code it seemed like the thrusters started to move slightly (we could see some bubbles) and then just stopped.

In order to debug we set up a simpler mission that we could run on the bench trying to control the tilt of the camera instead. To get more information we also started to print the mission status to the terminal. When we attempted to run this mission nothing happened, however, since we printed the mission status we could see that it goes inactive->running->aborted, see included image. (Sorry for taking the image with a phone instead of taking a screenshot.) If this also happened with the initial mission it could explain why it seemed like the thrusters was about to start to spin and then just stopped. Another thing we found strange is that the desired tilt value is not in the mission instruction.

To Reproduce
Steps to reproduce the behavior:

1. Turn on the drone and spool
2. Connect to the blueye network
3. Run the related code

Related code:

 import rclpy 
 from rclpy.node import Node
 import sys
 import blueye.sdk
 
 class BlueyeMissionPlanner(Node):

       def __init__(self):                                                                                                                 
         super().__init__('blueye_mission_planner')
         print("Connecting to Drone")
         self.connect_to_blueye()
         print("Getting Mission")
         mission = self.get_test_mission()
         print("Checks that Drone is ready to receive mission")
         self.blueye_drone.mission.clear()
         ready = self.blueye_drone.mission.get_status().state == bp.MissionState.MISSION_STATE_INACTIVE
         print("Ready: ", ready)
         print(self.blueye_drone.mission.get_status())
         self.timer = self.create_timer(1.0, self.status_callback)
 
         # If the drone is ready, send the test mission
         if ready:
             self.blueye_drone.mission.send_new(mission)
             self.blueye_drone.mission.run()
             print("Running mission")
         else:
             self.get_logger().error("Drone not ready. Exiting..")
             sys.exit()
         
     def connect_to_blueye(self):
         self.blueye_drone = blueye.sdk.Drone()
         if not self.blueye_drone:
             self.get_logger().error("Not able to communicate with Blueye. Exiting..")
             sys.exit()
         return True
             
     def status_callback(self):
         print(self.blueye_drone.mission.get_active())
         print(self.blueye_drone.mission.get_status())
 
     def get_test_mission(self):
         tilt_camera_1 = bp.Instruction(tilt_main_camera_command={
             "tilt_angle": {"value": 0.0}})
         tilt_camera_2 = bp.Instruction(tilt_main_camera_command={
             "tilt_angle": {"value": 30.0}}) 
         tilt_camera_3 = bp.Instruction(tilt_main_camera_command={
             "tilt_angle": {"value":-30.0}}) 
         return bp.Mission(instructions=[tilt_camera_2, tilt_camera_3, tilt_camera_1])
         
 def main(args=None):
     rclpy.init(args=args)
     mission_planner = BlueyeMissionPlanner()
     rclpy.spin(mission_planner)
     rclpy.shutdown()
     
 if __name__ == "__main__":
     main()
                    

Expected behavior
We expected the mission to run and the camera to point upwards, downwards and then straight forward again.

Additional details (please complete the following information):

• Ubuntu 22.04
• Python 3.10.12
• blueye-protocol 2.6.0
• blueye.sdk 2.3.0
• ros2 humble
  Blueye.sdk installed with pip install git+https://github.com/blueye-robotics/blueye.sdk.git@add-mission-planning to get the mission planning branch.

Additional context
We tested the Blueye mission planner on a phone with the same Blueye and was there able to run a mission setting the tilt angle to 30, -30 and then back to 0.

[follesoe]

Thanks for such a thorough and precise bug report @Urlaxle 🙌!

The issue is most likely that each instruction needs a unique numeric ID. For instance, you can loop the instructions and assign the index as the Id field.

We will investigate further on our end, and see how we can make this more clear, either through documentation, or better error messages from the drone. For instance, if the problem is in fact non-unique IDs on the instructions, the mission executor should not load the mission and report it as invalid/not loaded.

[sindrehan]

I'm pretty sure the issue is timing related - If the run command immediately follows the send_new command it appears the drone will just ignore the mission.

Adding a couple of while loops solves the problem:

from blueye.sdk import Drone
import blueye.protocol as bp
import time

tilt_camera_1 = bp.Instruction(tilt_main_camera_command={
    "tilt_angle": {"value": 0.0}})
tilt_camera_2 = bp.Instruction(tilt_main_camera_command={
    "tilt_angle": {"value": 30.0}})

tilt_camera_3 = bp.Instruction(tilt_main_camera_command={
    "tilt_angle": {"value": -30.0}})
mission = bp.Mission(instructions=[tilt_camera_2, tilt_camera_3, tilt_camera_1])

d = Drone()

if not d.mission.get_status().state == bp.MissionState.MISSION_STATE_INACTIVE:
    d.mission.clear()
    # Wait until the mission state becomes MISSION_STATE_INACTIVE
    while d.mission.get_status().state != bp.MissionState.MISSION_STATE_INACTIVE:
        time.sleep(0.1)

d.mission.send_new(mission)
# Wait until the mission state becomes MISSION_STATE_READY
while d.mission.get_status().state != bp.MissionState.MISSION_STATE_READY:
    time.sleep(0.1)

d.mission.run()

[aviggen]

That makes sense @sindrehan. We are also waiting for the MISSION_STATE_READY in the app before we can issue the run command.

[follesoe]

@sindrehan If so, the example/flow described in mission.py should be updated, or mission.py extended to cover this requirement.

Also; would be nice to add a simple example mission to https://github.com/BluEye-Robotics/blueye.sdk/tree/add-mission-planning/examples for a mission that can be run on land to verify the SDK and mission planner.

[aviggen]

I think it would be beneficial if you could add a subscriber for the NotificationTel as well @sindrehan. This will give more insight into what the Mission Supervisor is doing.

NotificationType

[Urlaxle]

I'm pretty sure the issue is timing related - If the run command immediately follows the send_new command it appears the drone will just ignore the mission.

Adding a couple of while loops solves the problem:

from blueye.sdk import Drone
import blueye.protocol as bp
import time

tilt_camera_1 = bp.Instruction(tilt_main_camera_command={
    "tilt_angle": {"value": 0.0}})
tilt_camera_2 = bp.Instruction(tilt_main_camera_command={
    "tilt_angle": {"value": 30.0}})

tilt_camera_3 = bp.Instruction(tilt_main_camera_command={
    "tilt_angle": {"value": -30.0}})
mission = bp.Mission(instructions=[tilt_camera_2, tilt_camera_3, tilt_camera_1])

d = Drone()

if not d.mission.get_status().state == bp.MissionState.MISSION_STATE_INACTIVE:
    d.mission.clear()
    # Wait until the mission state becomes MISSION_STATE_INACTIVE
    while d.mission.get_status().state != bp.MissionState.MISSION_STATE_INACTIVE:
        time.sleep(0.1)

d.mission.send_new(mission)
# Wait until the mission state becomes MISSION_STATE_READY
while d.mission.get_status().state != bp.MissionState.MISSION_STATE_READY:
    time.sleep(0.1)

d.mission.run()

Thank you for your fast responses. Have you tested this code block on a Blueye and confirmed that it worked? I would like to go test the mission planner more properly, but it takes a while to get to the site where we store our Blueyes, so I just want to make sure that this was most likely the issue before going there.

Another thing is the tilt_angle value, should we expect to see that inside the instruction block when running drone.get_active()?

Once again than you for your help.

[sindrehan]

Yes, I've tested the script with a drone and it works as expected. The reason that I did catch the error before was that I only tested the commands from the interpreter (not running from a script) so the timing was never an issue.

Yes, the tilt_angle value is included in the output from get_active, however, due to the way the protobuf messages are implemented, if the angle is the default value (ie. 0) it will not be printed.
E.g.

mission = bp.Mission(instructions=[bp.Instruction(tilt_main_camera_command={"tilt_angle": {"value": 0}})])
print(mission)

Will give

instructions {
  tilt_main_camera_command {
    tilt_angle {
    }
  }
}

However if one does

print(mission.instructions[0].tilt_main_camera_command.tilt_angle.value)

the output will be 0.0

[Urlaxle]

Hi, we performed another test of the mission planner today and got some mixed results.

Initially we attempted to run the tilt code from above with the addition of sleeps. Sometimes this worked, and sometimes it did not work, i.e mission status goes straight to aborted.

We also noted a behavior where it didn't seem like the Blueye checked the current tilt angle before going to the next instruction. If we sent an instruction to go to 30 and then back to 0 we could see that it started to tilt towards 30 but then started to go back to 0 again before reaching 30. If we sent a wait instruction in-between the tilt instructions the tilt would go all the way. This may be the desired behavior from your side as the instruction could simply be to set a set-point for the tilt unit, and then move on to the next instruction but I just wanted to let you know in case its not.

Since the mission planner seemed to work occasionally we moved on to test the dive to seabed, take an image and then resurface. As with the tilt mission we had some successful deployments, but most were still aborted. However, by increasing the sleep time before running the mission we got significantly more successful deployments, see the code below.

import blueye.protocol as bp
import time
from blueye.sdk import Drone

print("Connecting to the drone")
drone = Drone()

go_to_seabed = bp.Instruction(go_to_seabed_command={"desired_speed": 0.5})
take_picture = bp.Instruction(camera_command={
    "camera_action": bp.CameraAction.CAMERA_ACTION_TAKE_PHOTO
})
go_to_surface = bp.Instruction(go_to_surface_command={"desired_speed": 0.5})
mission = bp.Mission(instructions=[go_to_seabed, take_picture, go_to_surface])

print("Clears any old missions")
if not drone.mission.get_status().state == bp.MissionState.MISSION_STATE_INACTIVE:
    drone.mission.clear()
    # Wait until the mission state becomes MISSION_STATE_INACTIVE
    while drone.mission.get_status().state != bp.MissionState.MISSION_STATE_INACTIVE:
        time.sleep(0.1)

print("Checks that the drone is ready")
ready = drone.mission.get_status().state == bp.MissionState.MISSION_STATE_INACTIVE

if ready:
    print("Send new mission")
    drone.mission.send_new(mission)
    time.sleep(0.3)
    print("Run mission")
    drone.mission.run()
    time.sleep(0.3) # If this is 0.1 second we often dont get into the while loop
    
    print("Mission is running")

    with open("log.txt", "w") as f:
        while drone.mission.get_status().state != bp.MissionState.MISSION_STATE_READY and bp.MissionState.MISSION_STATE_COMPLETED != drone.mission.get_status().state: 
           time.sleep(0.2)
           try:
               f.write(str(drone.mission.get_status()))
               f.write(str(drone.mission.get_active()))
           except:
                pass
print("Mission done")

Even though increasing the sleep time helped a lot, the mission planner still seemed to go to aborted about 2/5 times. I will add a example log of a successful and unsuccessful dive here. Let me know if it would help you if I record some additional information doing the dives.

log_success.txt
log_fail.txt

To me looking at the logs it seems like the mission is received by the Blueye then for some reason it decides to abort instead of execute. I don't know why it goes to abort, but I have seen a functionality in the BlueyeApp that allows you to resume a paused/aborted mission from its current step. If the current mission, and its progress is stored on the Blueye would it be possible for me to attempt to resume the mission through the blueye.sdk if I see that the state goes into aborted?

[aviggen]

Hi, thanks for solid feedback again. Some quick feedback from my phone here. Instead of waiting, you should rather wait and check for the mission status to be MISSION_STATE_READY before you send the run command. Also I would recommend using the Foxglove program to inspect the different states and transitions. You should especially have a look at the NotificationsTel which tells you why it aborted.

Going to depth might be related to that you are traveling at 0.5, try with 0.3 m/s maybe. It can also be related to buoyancy trimming. If it is too much off it can abort since it thinks it is stuck, hitting the bottom or out of tether.

The camera instruction is like you describe. Just fire and forget, so adding a wait will be necessary to tilt up and down. It's made for setting the camera tilt after a waypoint or depth command etc.

[Urlaxle]

Thanks for the tips I will give Foxglove a try to see if it tells us why we go to the aborted state.

As mentioned in my previous reply it could be useful to have the option to attempt to resume the mission after its aborted similar to the functionality in the Blueye app. That way we could use software to monitor the reason for the abortion through NotoficationsTel, and if we think its just a mistake try to resume our mission.

I could keep track of this myself by looking at the amount of "completed_instructions_ids:" lines I get in when looking through the returned string of drone.mission.get_active() and then create a new mission with the renaming tasks, but it you already have the functionality with a cleaner implementation I would rather use that.

One thing I forgot to mention in my previous post was that when monitoring the drone.mission.get_active() string, a new line "completed_instruction_ids:0" seems to be added every time an instruction is completed. This can be seen in the "log_success.txt" where one of these lines are added when we reach the bottom, another after we take the image, and a last one when the drone resurfaces. Leading to the following string:

time_elapsed: 19
completed_instruction_ids: 0
completed_instruction_ids: 0
completed_instruction_ids: 0
total_number_of_instructions: 3
instructions {
  go_to_seabed_command {
    desired_speed: 0.5
  }
}
instructions {
  camera_command {
    camera_action: CAMERA_ACTION_TAKE_PHOTO
  }
}
instructions {
  go_to_surface_command {
    desired_speed: 0.5
  }
}
default_surge_speed: 0.6
default_heave_speed: 0.3
default_circle_of_acceptance: 2

Is this intentional or is it supposed to append new ids to the string after an instruction is complete like: completed_instruction_ids: 0,1,2?

[aviggen]

It should be possible to hit mission.run() again when you are in the ABORTED state.

You are right the completed_instructions_ids should include all the completed ids.

The ID should be set from the SDK when creating the mission. It can be an "i" that just increments. Maybe you'll get it then?