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