Skip to content

Commit

Permalink
camtrack: handle camera tracking image status updates
Browse files Browse the repository at this point in the history 
- tracker_image: override proces_event to handle update to tracked rectangle
- onboard controller: image status sends current tracked rectangle
- camera_view: update mavlink handlers
- camera_view: remove unused code
- camera_view: handle tracking image status
- tracker_image: override proces_event to handle update to tracked rectangle (fix)
- camera_view: request image status at 20Hz
- camera_view: remove unused code
- camera_view: remove unused code
- camera_view: suppress debug prints
- tracker_image: remove unused code
- onboard_controller: remove unused code
- onboard_controller: suppress debug prints
- tracker_image: update docstring
- tracker_image: add set_position method to Tracker base class
- tracker_image: add set_position method to TrackerCSTR
- tracker_image: set position of tracked rectangle
- onboard_controller: clean up track request print statements
- camtrack: onboard_controller: add timeout and restart

Signed-off-by: Rhys Mainwaring <[email protected]>
  • Loading branch information
@srmainwaring
srmainwaring committed Oct 9, 2024
1 parent 59a46ad commit 9119a92
Show file tree
Hide file tree
Showing 4 changed files with 165 additions and 161 deletions.
56 changes: 23 additions & 33 deletions MAVProxy/modules/mavproxy_camtrack/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,25 +59,23 @@ def __init__(self, mpstate):

self.camera_view = CameraView(self.mpstate, "Camera Tracking", rtsp_url)

# TODO: NOTE: unused
# NOTE: unused except for status
# mavlink messages
self._last_gimbal_device_information = None
self._last_gimbal_manager_status = None
self._last_gimbal_device_information = None
self._last_gimbal_device_attitude_status = None
self._last_autopilot_state_for_gimbal_device = None
self._last_camera_tracking_image_status = None
self._last_camera_information = None

# Discovery
# discovery
self._do_request_gimbal_manager_information = True
self._do_request_gimbal_manager_status = True
self._do_request_gimbal_device_information = True
self._do_request_autopilot_state_for_gimbal_device = True
self._do_request_camera_information = True
self._do_request_camera_tracking_image_status = True

# data

# control update rate to GUI
self._msg_list = []
self._fps = 30.0
Expand Down Expand Up @@ -129,33 +127,33 @@ def mavlink_packet(self, msg):
if mtype == "HEARTBEAT":
self.handle_heartbeat(msg)

# working - must be requested
# must be requested
elif mtype == "GIMBAL_MANAGER_INFORMATION":
self.handle_gimbal_manager_information(msg)

# working - must be requested (should be broadcast)
# must be requested (should be broadcast)
elif mtype == "GIMBAL_MANAGER_STATUS":
self.handle_gimbal_manager_status(msg)

# not working - limited implementation in AP_Mount
elif mtype == "GIMBAL_DEVICE_INFORMATION":
self.handle_gimbal_device_information(msg)

# working - boradcast
# broadcast
elif mtype == "GIMBAL_DEVICE_ATTITUDE_STATUS":
self.handle_gimbal_device_attitude_status(msg)

# working - must be requested
# must be requested
elif mtype == "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE":
self.handle_autopilot_state_for_gimbal_device(msg)

# working - must be requested
# must be requested
elif mtype == "CAMERA_INFORMATION":
self.handle_camera_information(msg)

# must be requested
elif mtype == "CAMERA_TRACKING_IMAGE_STATUS":
# TODO: add handler
print(msg)
self.handle_camera_tracking_image_status(msg)

# TODO: NOTE: disabled
# check command_ack
Expand Down Expand Up @@ -272,8 +270,18 @@ def handle_autopilot_state_for_gimbal_device(self, msg):
self._last_autopilot_state_for_gimbal_device = msg

def handle_camera_information(self, msg):
# print(msg)
pass
self._last_camera_information = msg

def handle_camera_tracking_image_status(self, msg):
# TODO: refactor to use enums in onboard_controller.py
if (
msg.tracking_status == mavutil.mavlink.CAMERA_TRACKING_STATUS_FLAGS_ACTIVE
and msg.tracking_mode == mavutil.mavlink.CAMERA_TRACKING_MODE_RECTANGLE
and msg.target_data == mavutil.mavlink.CAMERA_TRACKING_TARGET_DATA_IN_STATUS
):
self.camera_view.set_tracked_rectangle(
msg.rec_top_x, msg.rec_top_y, msg.rec_bottom_x, msg.rec_bottom_y
)

def check_events(self):
"""Check for events on the camera view"""
Expand Down Expand Up @@ -329,7 +337,7 @@ def send_messages(self):
if self._do_request_camera_tracking_image_status:
self.send_set_message_interval_message(
mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS,
1000 * 1000, # 1Hz
1000 * 50, # 20Hz
response_target=1, # flight-stack default
)
self._do_request_camera_tracking_image_status = False
Expand All @@ -355,7 +363,6 @@ def send_gimbal_manager_configure(self):
gimbal_devid, # param7
)

# MAVProxy.modules.mavproxy_misc.py
def send_request_message(self, message_id, p1=0):
self.master.mav.command_long_send(
self.settings.target_system, # target_system
Expand Down Expand Up @@ -388,23 +395,6 @@ def send_set_message_interval_message(
response_target, # param7
)

def request_camera_information(self):
# send CAMERA_INFORMATION request
# mavutil.mavlink.MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION
pass

def request_gimbal_manager_information(self):
pass

def request_gimbal_manager_status(self):
pass

def request_gimbal_device_information(self):
pass

def request_autopilot_state_for_gimbal_device(self):
pass

def idle_task(self):
"""Idle tasks"""
self.check_events()
Expand Down
75 changes: 21 additions & 54 deletions MAVProxy/modules/mavproxy_camtrack/camera_view.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,10 @@ def __init__(self, mpstate, title, rtsp_url, fps=30):
)
self.im.set_gstreamer(gst_pipeline)

def set_tracked_rectangle(self, top_left_x, top_left_y, bot_right_x, bot_right_y):
"""Set the tracked rectangle (normalised coords)"""
self.im.set_tracked_rectangle(top_left_x, top_left_y, bot_right_x, bot_right_y)

def close(self):
"""Close the GUI"""
# TODO: MPImage does not have a close_event
Expand Down Expand Up @@ -96,13 +100,6 @@ def check_events(self):
if isinstance(event, MPImageFrameCounter):
self.frame_counter = event.frame
continue

# if isinstance(event, MPImageTrackPoint):
# continue

# if isinstance(event, MPImageTrackRectangle):
# continue

if (
hasattr(event, "ClassName")
and event.ClassName == "wxMouseEvent"
Expand All @@ -119,8 +116,8 @@ def check_events(self):
self.im.start_tracker(event.X, event.Y, twidth, twidth)

# TODO: move / encapsulate
print(f"xres: {xres}, yres: {yres}")
print(f"event.X: {event.X}, event.Y: {event.X}, twidth: {twidth}")
# print(f"xres: {xres}, yres: {yres}")
# print(f"event.X: {event.X}, event.Y: {event.X}, twidth: {twidth}")
top_left_x = event.X / xres
top_left_y = event.Y / yres
bot_right_x = (event.X + twidth) / xres
Expand All @@ -135,8 +132,6 @@ def check_events(self):
else:
pass

# Camera tracking commands. Communication is GCS -> FC

def send_camera_track_point(self, point_x, point_y, radius):
"""
https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT
Expand All @@ -147,11 +142,11 @@ def send_camera_track_point(self, point_x, point_y, radius):
src_cmp = self.mpstate.master().source_component
tgt_sys = self.camera_sysid
tgt_cmp = self.camera_cmpid
print(
f"Send COMMAND_LONG: CAMERA_TRACK_POINT: "
f"src_sys: {src_sys}, src_cmp: {src_cmp} "
f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
)
# print(
# f"Send COMMAND_LONG: CAMERA_TRACK_POINT: "
# f"src_sys: {src_sys}, src_cmp: {src_cmp} "
# f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
# )
target_camera = 0
self.mpstate.master().mav.command_long_send(
tgt_sys, # target_system
Expand Down Expand Up @@ -179,11 +174,11 @@ def send_camera_track_rectangle(
src_cmp = self.mpstate.master().source_component
tgt_sys = self.camera_sysid
tgt_cmp = self.camera_cmpid
print(
f"Send COMMAND_LONG: CAMERA_TRACK_RECTANGLE: "
f"src_sys: {src_sys}, src_cmp: {src_cmp} "
f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
)
# print(
# f"Send COMMAND_LONG: CAMERA_TRACK_RECTANGLE: "
# f"src_sys: {src_sys}, src_cmp: {src_cmp} "
# f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
# )
target_camera = 0
self.mpstate.master().mav.command_long_send(
tgt_sys, # target_system
Expand All @@ -207,11 +202,11 @@ def send_camera_stop_tracking(self):
src_cmp = self.mpstate.master().source_component
tgt_sys = self.camera_sysid
tgt_cmp = self.camera_cmpid
print(
f"Send COMMAND_LONG: CAMERA_STOP_TRACKING: "
f"src_sys: {src_sys}, src_cmp: {src_cmp} "
f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
)
# print(
# f"Send COMMAND_LONG: CAMERA_STOP_TRACKING: "
# f"src_sys: {src_sys}, src_cmp: {src_cmp} "
# f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
# )
target_camera = 0
self.mpstate.master().mav.command_long_send(
tgt_sys, # target_system
Expand All @@ -227,34 +222,6 @@ def send_camera_stop_tracking(self):
0, # param7
)

def set_message_interval_image_status(self):
"""
https://mavlink.io/en/messages/common.html#CAMERA_TRACKING_IMAGE_STATUS
https://mavlink.io/en/messages/common.html#MAV_CMD_SET_MESSAGE_INTERVAL
"""
tgt_sys = self.camera_sysid
tgt_comp = self.camera_cmpid
print(
f"Send COMMAND_LONG: SET_MESSAGE_INTERVAL: CAMERA_TRACKING_IMAGE_STATUS: "
f"tgt_sys: {tgt_sys}, tgt_comp: {tgt_comp}"
)
message_id = mavutil.mavlink.CAMERA_TRACKING_IMAGE_STATUS
interval = 0 # default rate
response_target = 1 # address of requestor
self.mpstate.master().mav.command_long_send(
tgt_sys, # target_system
tgt_comp, # target_component
mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # command
0, # confirmation
message_id, # param1
interval, # param2
0, # param3
0, # param4
0, # param5
0, # param6
response_target, # param7
)


if __name__ == "__main__":
from optparse import OptionParser
Expand Down
Loading

0 comments on commit 9119a92

Please sign in to comment.