def _scan_multi_robot(self, proto_type=config.DEFAULT_PROTO_TYPE):
""" Automatic scanning of robots in the network
:return:
"""
robot_list = []
ip_list = conn.scan_robot_ip_list(10)
for i, ip in enumerate(ip_list):
sdk_conn = conn.SdkConnection()
proxy_addr = (ip, config.ROBOT_PROXY_PORT)
proto = protocol.ProtoSetSdkConnection()
proto._connection = 1
proto._host = protocol.host2byte(9, 6)
if config.LOCAL_IP_STR:
proto._ip = config.LOCAL_IP_STR
else:
proto._ip = '0.0.0.0'
proto._port = random.randint(config.ROBOT_SDK_PORT_MIN,
config.ROBOT_SDK_PORT_MAX)
msg = protocol.Msg(robot.ROBOT_DEFAULT_HOST,
protocol.host2byte(9, 0), proto)
result, local_ip = sdk_conn.switch_remote_route(msg, proxy_addr)
proto._ip = local_ip
logger.info("request connection ip:{0} port:{1}".format(
proto._ip, proto._port))
if result:
conn1 = conn.Connection((proto._ip, proto._port),
(ip, config.ROBOT_DEVICE_PORT),
protocol=proto_type)
logger.info("connection {0}".format(conn1))
cli = client.Client(9, 6, conn1)
rob = robot.Robot(cli)
robot_list.append(rob)
return robot_list
def init():
ep_robot = robot.Robot()
ep_robot.initialize(conn_type="sta")
ep_version = ep_robot.get_version()
print("Robot Version: {0}".format(ep_version))
return ep_robot
def __init__(self, cfg, q):
self._cfg = cfg
self._q = q
self._detect_q = detect_q
self._is_need_stop = False
self._is_running = False
self._camera = None
self._robotic_conn = None
self._robotic_ctrl = None
self._msg_interval = self._cfg['msg_interval']
self._last_msg_time = None
self._detect_info = None
try:
# 初始化
self._robot = robot.Robot()
# 初始化相机
self._robot.initialize(conn_type='sta',
sn=self._cfg['robot_master_sn'])
self._camera = self._robot.camera
# 启动接收图像分析线程的分析结果线程
# DetectInfoRecv(self).start()
except Exception as err:
self._robot.close()
logging.error(f"exception: {err}")
def init_device(self, timeout=5, conn_type="sta"):
'''
conn_type
sta 机器人接入局域网
ap 机器人作为热点
'''
# todo timeout
self.ep_robot = robot.Robot()
self.ep_robot.initialize(conn_type=conn_type)
self.pub_notification("RoboMaster 已连接", type="SUCCESS")
def vision_thread(cam_x, cam_y, cam_z, ref_x, ref_y, args):
ep_robot = robot.Robot()
ep_robot.initialize(conn_type="ap")
# Constant parameters used in Aruco methods
ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
# Load camera intrinsic parameters
fs = cv2.FileStorage(args.calibration_config, cv2.FILE_STORAGE_READ)
camera_matrix = fs.getNode("camera_matrix").mat()
distortion = fs.getNode("distortion_coefficients").mat()
# Initialize ep camera
ep_camera = ep_robot.camera
ep_camera.start_video_stream(display=False)
cam_x, cam_y, cam_z = 0.0, 0.0, 0.0
while True:
# Capturing each frame of our video stream
QueryImg = ep_camera.read_cv2_image()
# grayscale image
gray = cv2.cvtColor(QueryImg, cv2.COLOR_BGR2GRAY)
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
refmark_id = -1
if ids is not None:
for i in range(len(ids)):
if ids[i] == args.ref_id:
refmark_id = i
if refmark_id != -1:
rvecs, tvecs, objPoints = aruco.estimatePoseSingleMarkers(
corners, 0.5, camera_matrix, distortion)
rvec, tvec = rvecs[refmark_id], tvecs[refmark_id]
rotM = cv2.Rodrigues(rvecs[refmark_id])[0]
cam_x, cam_y, cam_z = np.dot(-np.matrix(rotM).T, tvec.T)
# for debug use
cv2.putText(QueryImg, 'cam_x: {}'.format(cam_x), (50, 50),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
cv2.putText(QueryImg, 'cam_y: {}'.format(cam_y), (50, 100),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
cv2.putText(QueryImg, 'cam_z: {}'.format(cam_z), (50, 150),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
cv2.imshow('QueryImage', QueryImg)
if cv2.waitKey(1) & 0xff == ord('q'):
break
import time
from robomaster import robot
from robomaster import camera
import cv2
import numpy as np
if __name__ == '__main__':
ep_robot = robot.Robot()
ep_robot.initialize(conn_type="sta")
ep_version = ep_robot.get_version()
print("Robot Version: {0}".format(ep_version))
chassis = ep_robot.chassis
ep_camera = ep_robot.camera
ep_camera.start_video_stream(display=False, resolution=camera.STREAM_360P)
action = chassis.move(x=0, y=0, z=180, xy_speed=0.5, z_speed=30)
print(action)
print(dir(action))
#print(action.wait_for_completed(timeout=0.1))
#print(action.wait_for_completed(timeout=0.1))
#while not action.wait_for_completed(timeout=0.1):
#for i in range(0, 30):
while action._percent < 95:
img = ep_camera.read_cv2_image(strategy="newest")
cv2.imshow("Robot", img)
def __init__(self):
self.ep_robot = robot.Robot()