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()