Skip to content

Commit 8b61bc1

Browse files
srmainwaringsrmainwaring
committed
camtrack: handle camera tracking image status updates
- 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 Signed-off-by: Rhys Mainwaring <[email protected]>
1 parent 59a46ad commit 8b61bc1

File tree

4 files changed

+149
-157
lines changed

4 files changed

+149
-157
lines changed

‎MAVProxy/modules/mavproxy_camtrack/__init__.py‎

Lines changed: 23 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -59,25 +59,23 @@ def __init__(self, mpstate):
5959

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

62-
# TODO: NOTE: unused
62+
# NOTE: unused except for status
6363
# mavlink messages
6464
self._last_gimbal_device_information = None
6565
self._last_gimbal_manager_status = None
6666
self._last_gimbal_device_information = None
6767
self._last_gimbal_device_attitude_status = None
6868
self._last_autopilot_state_for_gimbal_device = None
69-
self._last_camera_tracking_image_status = None
69+
self._last_camera_information = None
7070

71-
# Discovery
71+
# discovery
7272
self._do_request_gimbal_manager_information = True
7373
self._do_request_gimbal_manager_status = True
7474
self._do_request_gimbal_device_information = True
7575
self._do_request_autopilot_state_for_gimbal_device = True
7676
self._do_request_camera_information = True
7777
self._do_request_camera_tracking_image_status = True
7878

79-
# data
80-
8179
# control update rate to GUI
8280
self._msg_list = []
8381
self._fps = 30.0
@@ -129,33 +127,33 @@ def mavlink_packet(self, msg):
129127
if mtype == "HEARTBEAT":
130128
self.handle_heartbeat(msg)
131129

132-
# working - must be requested
130+
# must be requested
133131
elif mtype == "GIMBAL_MANAGER_INFORMATION":
134132
self.handle_gimbal_manager_information(msg)
135133

136-
# working - must be requested (should be broadcast)
134+
# must be requested (should be broadcast)
137135
elif mtype == "GIMBAL_MANAGER_STATUS":
138136
self.handle_gimbal_manager_status(msg)
139137

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

144-
# working - boradcast
142+
# broadcast
145143
elif mtype == "GIMBAL_DEVICE_ATTITUDE_STATUS":
146144
self.handle_gimbal_device_attitude_status(msg)
147145

148-
# working - must be requested
146+
# must be requested
149147
elif mtype == "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE":
150148
self.handle_autopilot_state_for_gimbal_device(msg)
151149

152-
# working - must be requested
150+
# must be requested
153151
elif mtype == "CAMERA_INFORMATION":
154152
self.handle_camera_information(msg)
155153

154+
# must be requested
156155
elif mtype == "CAMERA_TRACKING_IMAGE_STATUS":
157-
# TODO: add handler
158-
print(msg)
156+
self.handle_camera_tracking_image_status(msg)
159157

160158
# TODO: NOTE: disabled
161159
# check command_ack
@@ -272,8 +270,18 @@ def handle_autopilot_state_for_gimbal_device(self, msg):
272270
self._last_autopilot_state_for_gimbal_device = msg
273271

274272
def handle_camera_information(self, msg):
275-
# print(msg)
276-
pass
273+
self._last_camera_information = msg
274+
275+
def handle_camera_tracking_image_status(self, msg):
276+
# TODO: refactor to use enums in onboard_controller.py
277+
if (
278+
msg.tracking_status == mavutil.mavlink.CAMERA_TRACKING_STATUS_FLAGS_ACTIVE
279+
and msg.tracking_mode == mavutil.mavlink.CAMERA_TRACKING_MODE_RECTANGLE
280+
and msg.target_data == mavutil.mavlink.CAMERA_TRACKING_TARGET_DATA_IN_STATUS
281+
):
282+
self.camera_view.set_tracked_rectangle(
283+
msg.rec_top_x, msg.rec_top_y, msg.rec_bottom_x, msg.rec_bottom_y
284+
)
277285

278286
def check_events(self):
279287
"""Check for events on the camera view"""
@@ -329,7 +337,7 @@ def send_messages(self):
329337
if self._do_request_camera_tracking_image_status:
330338
self.send_set_message_interval_message(
331339
mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS,
332-
1000 * 1000, # 1Hz
340+
1000 * 50, # 20Hz
333341
response_target=1, # flight-stack default
334342
)
335343
self._do_request_camera_tracking_image_status = False
@@ -355,7 +363,6 @@ def send_gimbal_manager_configure(self):
355363
gimbal_devid, # param7
356364
)
357365

358-
# MAVProxy.modules.mavproxy_misc.py
359366
def send_request_message(self, message_id, p1=0):
360367
self.master.mav.command_long_send(
361368
self.settings.target_system, # target_system
@@ -388,23 +395,6 @@ def send_set_message_interval_message(
388395
response_target, # param7
389396
)
390397

391-
def request_camera_information(self):
392-
# send CAMERA_INFORMATION request
393-
# mavutil.mavlink.MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION
394-
pass
395-
396-
def request_gimbal_manager_information(self):
397-
pass
398-
399-
def request_gimbal_manager_status(self):
400-
pass
401-
402-
def request_gimbal_device_information(self):
403-
pass
404-
405-
def request_autopilot_state_for_gimbal_device(self):
406-
pass
407-
408398
def idle_task(self):
409399
"""Idle tasks"""
410400
self.check_events()

‎MAVProxy/modules/mavproxy_camtrack/camera_view.py‎

Lines changed: 21 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,10 @@ def __init__(self, mpstate, title, rtsp_url, fps=30):
6969
)
7070
self.im.set_gstreamer(gst_pipeline)
7171

72+
def set_tracked_rectangle(self, top_left_x, top_left_y, bot_right_x, bot_right_y):
73+
"""Set the tracked rectangle (normalised coords)"""
74+
self.im.set_tracked_rectangle(top_left_x, top_left_y, bot_right_x, bot_right_y)
75+
7276
def close(self):
7377
"""Close the GUI"""
7478
# TODO: MPImage does not have a close_event
@@ -96,13 +100,6 @@ def check_events(self):
96100
if isinstance(event, MPImageFrameCounter):
97101
self.frame_counter = event.frame
98102
continue
99-
100-
# if isinstance(event, MPImageTrackPoint):
101-
# continue
102-
103-
# if isinstance(event, MPImageTrackRectangle):
104-
# continue
105-
106103
if (
107104
hasattr(event, "ClassName")
108105
and event.ClassName == "wxMouseEvent"
@@ -119,8 +116,8 @@ def check_events(self):
119116
self.im.start_tracker(event.X, event.Y, twidth, twidth)
120117

121118
# TODO: move / encapsulate
122-
print(f"xres: {xres}, yres: {yres}")
123-
print(f"event.X: {event.X}, event.Y: {event.X}, twidth: {twidth}")
119+
# print(f"xres: {xres}, yres: {yres}")
120+
# print(f"event.X: {event.X}, event.Y: {event.X}, twidth: {twidth}")
124121
top_left_x = event.X / xres
125122
top_left_y = event.Y / yres
126123
bot_right_x = (event.X + twidth) / xres
@@ -135,8 +132,6 @@ def check_events(self):
135132
else:
136133
pass
137134

138-
# Camera tracking commands. Communication is GCS -> FC
139-
140135
def send_camera_track_point(self, point_x, point_y, radius):
141136
"""
142137
https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT
@@ -147,11 +142,11 @@ def send_camera_track_point(self, point_x, point_y, radius):
147142
src_cmp = self.mpstate.master().source_component
148143
tgt_sys = self.camera_sysid
149144
tgt_cmp = self.camera_cmpid
150-
print(
151-
f"Send COMMAND_LONG: CAMERA_TRACK_POINT: "
152-
f"src_sys: {src_sys}, src_cmp: {src_cmp} "
153-
f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
154-
)
145+
# print(
146+
# f"Send COMMAND_LONG: CAMERA_TRACK_POINT: "
147+
# f"src_sys: {src_sys}, src_cmp: {src_cmp} "
148+
# f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
149+
# )
155150
target_camera = 0
156151
self.mpstate.master().mav.command_long_send(
157152
tgt_sys, # target_system
@@ -179,11 +174,11 @@ def send_camera_track_rectangle(
179174
src_cmp = self.mpstate.master().source_component
180175
tgt_sys = self.camera_sysid
181176
tgt_cmp = self.camera_cmpid
182-
print(
183-
f"Send COMMAND_LONG: CAMERA_TRACK_RECTANGLE: "
184-
f"src_sys: {src_sys}, src_cmp: {src_cmp} "
185-
f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
186-
)
177+
# print(
178+
# f"Send COMMAND_LONG: CAMERA_TRACK_RECTANGLE: "
179+
# f"src_sys: {src_sys}, src_cmp: {src_cmp} "
180+
# f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
181+
# )
187182
target_camera = 0
188183
self.mpstate.master().mav.command_long_send(
189184
tgt_sys, # target_system
@@ -207,11 +202,11 @@ def send_camera_stop_tracking(self):
207202
src_cmp = self.mpstate.master().source_component
208203
tgt_sys = self.camera_sysid
209204
tgt_cmp = self.camera_cmpid
210-
print(
211-
f"Send COMMAND_LONG: CAMERA_STOP_TRACKING: "
212-
f"src_sys: {src_sys}, src_cmp: {src_cmp} "
213-
f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
214-
)
205+
# print(
206+
# f"Send COMMAND_LONG: CAMERA_STOP_TRACKING: "
207+
# f"src_sys: {src_sys}, src_cmp: {src_cmp} "
208+
# f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}"
209+
# )
215210
target_camera = 0
216211
self.mpstate.master().mav.command_long_send(
217212
tgt_sys, # target_system
@@ -227,34 +222,6 @@ def send_camera_stop_tracking(self):
227222
0, # param7
228223
)
229224

230-
def set_message_interval_image_status(self):
231-
"""
232-
https://mavlink.io/en/messages/common.html#CAMERA_TRACKING_IMAGE_STATUS
233-
https://mavlink.io/en/messages/common.html#MAV_CMD_SET_MESSAGE_INTERVAL
234-
"""
235-
tgt_sys = self.camera_sysid
236-
tgt_comp = self.camera_cmpid
237-
print(
238-
f"Send COMMAND_LONG: SET_MESSAGE_INTERVAL: CAMERA_TRACKING_IMAGE_STATUS: "
239-
f"tgt_sys: {tgt_sys}, tgt_comp: {tgt_comp}"
240-
)
241-
message_id = mavutil.mavlink.CAMERA_TRACKING_IMAGE_STATUS
242-
interval = 0 # default rate
243-
response_target = 1 # address of requestor
244-
self.mpstate.master().mav.command_long_send(
245-
tgt_sys, # target_system
246-
tgt_comp, # target_component
247-
mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # command
248-
0, # confirmation
249-
message_id, # param1
250-
interval, # param2
251-
0, # param3
252-
0, # param4
253-
0, # param5
254-
0, # param6
255-
response_target, # param7
256-
)
257-
258225

259226
if __name__ == "__main__":
260227
from optparse import OptionParser

0 commit comments

Comments
 (0)