Following on from this thread, I have updated the BlueOS and ArduSub versions, but now the keyboard controls through a python script does not work at all. How can I control the ROV through a python script?
Hi @atanu585,
I’ve moved your comment to a new thread because it’s not related to your previous issue of some of the thrusters not working.
It’s very hard to help without even knowing what kind of control commands you’re sending to the autopilot. Are you able to share the script you’re using?
Assuming you’ve already confirmed your script is establishing a MAVLink connection to the autopilot, common issues to check would be that you’re sending regular heartbeats and control inputs (or have disabled the relevant failsafes), and making sure you’re arming the vehicle before trying to command it to move.
Hi @EliotBR ,
Thank you for creating this new thread. Here is the script I am using for controlling the ROV:
from pymavlink import mavutil
import keyboard
import cv2
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
import datetime
import time
import gi
gi.require_version('Gst', '1.0')
from gi.repository import Gst
class Video():
def __init__(self, port=5600):
Gst.init(None)
self.port = port
self.latest_frame = self._new_frame = None
# [Software component diagram](https://www.ardusub.com/software/components.html)
# UDP video stream (:5600)
self.video_source = 'udpsrc port={}'.format(self.port)
# [Rasp raw image](http://picamera.readthedocs.io/en/release-0.7/recipes2.html#raw-image-capture-yuv-format)
# Cam -> CSI-2 -> H264 Raw (YUV 4-4-4 (12bits) I420)
self.video_codec = '! application/x-rtp, payload=96 ! rtph264depay ! h264parse ! avdec_h264'
# Python don't have nibble, convert YUV nibbles (4-4-4) to OpenCV standard BGR bytes (8-8-8)
self.video_decode = \
'! decodebin ! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert'
# Create a sink to get data
self.video_sink_conf = \
'! appsink emit-signals=true sync=false max-buffers=2 drop=true'
self.video_pipe = None
self.video_sink = None
self.run()
def start_gst(self, config=None):
""" Start gstreamer pipeline and sink
Pipeline description list e.g:
[
'videotestsrc ! decodebin', \
'! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert',
'! appsink'
]
Args:
config (list, optional): Gstreamer pileline description list
"""
if not config:
config = \
[
'videotestsrc ! decodebin',
'! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert',
'! appsink'
]
command = ' '.join(config)
self.video_pipe = Gst.parse_launch(command)
self.video_pipe.set_state(Gst.State.PLAYING)
self.video_sink = self.video_pipe.get_by_name('appsink0')
@staticmethod
def gst_to_opencv(sample):
"""Transform byte array into np array
Args:
sample (TYPE): Description
Returns:
TYPE: Description
"""
buf = sample.get_buffer()
caps_structure = sample.get_caps().get_structure(0)
array = np.ndarray(
(
caps_structure.get_value('height'),
caps_structure.get_value('width'),
3
),
buffer=buf.extract_dup(0, buf.get_size()), dtype=np.uint8)
return array
def frame(self):
""" Get Frame
Returns:
np.ndarray: latest retrieved image frame
"""
if self.frame_available:
self.latest_frame = self._new_frame
# reset to indicate latest frame has been 'consumed'
self._new_frame = None
return self.latest_frame
def frame_available(self):
"""Check if a new frame is available
Returns:
bool: true if a new frame is available
"""
return self._new_frame is not None
def run(self):
""" Get frame to update _new_frame
"""
self.start_gst(
[
self.video_source,
self.video_codec,
self.video_decode,
self.video_sink_conf
])
self.video_sink.connect('new-sample', self.callback)
def callback(self, sink):
sample = sink.emit('pull-sample')
self._new_frame = self.gst_to_opencv(sample)
return Gst.FlowReturn.OK
############################################################## function to send RC values
def set_rc_channel_pwm(Channel_id, pwm=1500):
if Channel_id < 0 or Channel_id > 18:
print("Channel does not exist.")
return
if Channel_id < 10:
rc_channel_values = [65535 for _ in range(10)]
rc_channel_values[Channel_id - 1] = pwm
master.mav.rc_channels_override_send(
master.target_system, # target_system
master.target_component, # target_component
*rc_channel_values) # RC channel list, in microseconds.
############################################################## function to send camera tilt angle
def look_at(tilt, roll=0, pan=0):
print(f"Tilt angle is set as: {tilt_angle / 100} degrees")
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
1,
tilt,
roll,
pan,
0, 0, 0,
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
########################################Control##########################################################
class Control:
def __init__(self):
self.backward_start_time = None # Initialize backward start time
self.light = 1100
self.mode = 'manual'
self.state = 'CONTROL'
def set_control_mode(self):
k_manual_mode = keyboard.is_pressed('ctrl+m')
k_auto_mode = keyboard.is_pressed('ctrl+t')
if k_manual_mode:
self.mode = 'manual'
def stop_moving(self):
set_rc_channel_pwm(3, 1500)
set_rc_channel_pwm(4, 1500)
set_rc_channel_pwm(5, 1500)
set_rc_channel_pwm(6, 1500)
set_rc_channel_pwm(9, 1600)
self.state = 'STOP'
def manual_control(self):
k_w = keyboard.is_pressed('w')
k_s = keyboard.is_pressed('s')
k_a = keyboard.is_pressed('a')
k_d = keyboard.is_pressed('d')
k_q = keyboard.is_pressed('q')
k_e = keyboard.is_pressed('e')
k_up = keyboard.is_pressed('up')
k_down = keyboard.is_pressed('down')
k_plus = keyboard.is_pressed('+')
k_minus = keyboard.is_pressed('-')
if k_w:
channel_n = 5
set_rc_channel_pwm(channel_n, 1580)
print('w')
elif k_s:
channel_n = 5
set_rc_channel_pwm(channel_n, 1450)
print('s')
else:
set_rc_channel_pwm(5, 1500)
if k_a:
channel_n = 6
set_rc_channel_pwm(channel_n, 1450)
print('a')
elif k_d:
channel_n = 6
set_rc_channel_pwm(channel_n, 1550)
print('d')
else:
set_rc_channel_pwm(6, 1500)
if k_q:
channel_n = 4
set_rc_channel_pwm(channel_n, 1450)
print('q')
elif k_e:
channel_n = 4
set_rc_channel_pwm(channel_n, 1550)
print('e')
else:
set_rc_channel_pwm(4, 1500)
if k_up:
channel_n = 3
set_rc_channel_pwm(channel_n, 1550)
print('up')
elif k_down:
channel_n = 3
set_rc_channel_pwm(channel_n, 1450)
print('down')
else:
set_rc_channel_pwm(3, 1500)
if k_plus:
channel_n = 9
self.light += 10
self.light = min(self.light, 1900)
set_rc_channel_pwm(channel_n, self.light)
print(self.light)
elif k_minus:
channel_n = 9
self.light -= 10
self.light = max(self.light, 1100)
set_rc_channel_pwm(channel_n, self.light)
print(self.light)
#####################################################
if __name__ == '__main__':
# Create the connection
master = mavutil.mavlink_connection('udpin:192.168.2.1:14550')
# Wait a heartbeat before sending commands
master.wait_heartbeat()
# Arm
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0,
1, 0, 0, 0, 0, 0, 0)
# Initialize Video object
video = Video()
# Initialize Control object
control = Control()
# Initial tilt angle
tilt_angle = -30 # in centidegrees
look_at(tilt_angle\*100)
# Generate filename with current time
current_time = datetime.datetime.now().strftime("%Y%m%d\_%H%M%S")
video_filename = f"output_{current_time}.avi"
# Initialize VideoWriter object to save the video
#fourcc = cv2.VideoWriter_fourcc(*'XVID')
#out = cv2.VideoWriter(video_filename, fourcc, 15.0, (640, 480))
detection_scores_log = [] # Log for detection scores
print('Initialising stream...')
waited = 0
start_time = time.time()
while not video.frame_available():
waited += 1
print('\r Frame not available (x{})'.format(waited), end='')
cv2.waitKey(30)
print('\nSuccess!\nStarting streaming - press "Esc" to quit.')
running = True
while running:
start_time = time.time()
control.set_control_mode()
if video.frame_available():
frame = video.frame()
# angle, ... = flange_detector.detect_and_draw(frame) # 注释掉
cv2.imshow('Camera', frame)
#out.write(frame)
# 给 loop 一个固定的频率，比如 10 Hz
elapsed = time.time() - start_time
sleep_time = max(0, (1/30) - elapsed)
time.sleep(sleep_time)
if control.mode == 'manual':
control.manual_control()
# Check for key presses to adjust tilt angle
if keyboard.is_pressed('p'):
tilt_angle -= 1000 # Increase tilt angle by 1 degree
look_at(tilt_angle)
print(f"Tilt angle increased to: {tilt_angle / 100} degrees")
elif keyboard.is_pressed('l'):
tilt_angle += 1000 # Decrease tilt angle by 1 degree
look_at(tilt_angle)
print(f"Tilt angle decreased to: {tilt_angle / 100} degrees")
# Allow frame to display, and check if user wants to quit
if cv2.waitKey(1) & 0xFF == 27: # ESC is 27
running = False
# control._log(save_plot=True, plot_filename=f"plot_{current_time}.png", csv_filename=f"logs_{current_time}.csv")
# Properly shut down the GStreamer pipeline
video.video_pipe.set_state(Gst.State.NULL)
cv2.destroyAllWindows()
# Release the VideoWriter object
#out.release()
Hi @atanu585 -
Can you share what you’re trying to accomplish with your script? Are you trying to drive the vehicle via your keyboard? While also streaming video? Is this in an effort to make your own GCS (Ground Control Software) ?
If so, have you tried Cockpit? Driving with the keyboard has been accomplished by some users, and in general it is very easy to customize to your needs. Recording video and snapshots with metadata is also possible.