from DobotRPC import DobotlinkAdapter, RPCClient
import time

# ********出入库涉及到的全局变量******
# 智能仓库库外点与库内点
P1_out = [250, 48]
P1_in = [202, 50]
# 超市库外点与库内点
P2_out = [100, 48]
P2_in = [52, 50]
# **********************************

# *************************抓取涉及到的全局变量*******************************************
# 小车置物盒上方固定检测点
p_x0 = 0
p_y0 = -223
p_z0 = 110
p_R0 = -45

p_R0_grap = -110  # 从小车置物盒抓取角度，防控制盒干涉
p_R0_throw = p_R0

# 地面上方固定检测点
p_x1 = 223
p_y1 = 0
p_z1 = 110
p_R1 = -75

# 机械臂检测零点偏移量
move_x0 = 0
move_y0 = 0
move_time = 0  # 记录移动次数

# 小车置物盒固定放置物品点参数
p_x2 = -60
p_y2 = -233
p_z2 = 25
p_R2 = -75
thetaX = 0  # 放置偏移
catch_num = 1  # 物品计数

# 机械臂目标坐标
p_x = 0
p_y = 0
p_R = 0

# 仓库固定放置点
X1 = 250
Y1 = 60
Z1 = 33

X2 = 250
Y2 = -60
Z2 = 33

# 目标id索引
object_id = 0
'''
 OBJECT_INDEX = {"orange":0,"cabbage":1,"water":2,"Beijin":3,"ShangHai":4,
 "ShenZhen":5}
 *************************************************************************************
'''


class BetaGoApi(object):
    def __init__(self, port_name=None):
        self.__dobotlink = DobotlinkAdapter(RPCClient(), is_sync=True)
        self._port_name = port_name
        self.rgb_number = {
            "LED_1": 1,
            "LED_2": 2,
            "LED_3": 3,
            "LED_4": 4,
            "LED_ALL": 5
        }
        self.calibra_h_car = [
            [1.12958192e+00, 3.54961903e-02,
             -1.24693734e+02],  # 此处后续需改为读取上位机标定保存的数据
            [1.22406525e-01, -8.24737872e-01, -1.76587713e+02],
            [-3.10021358e-04, -1.68163066e-04, 1.00000000e+00]
        ]
        self.camera_h = 159  # 摄像头距离标定平面高度
        self.theta_h = 0  # 物体平面相对标定平面高度

    def get_portname(func):
        def wrapper(self, *args, **kwargs):
            if self._port_name:
                return func(self, self._port_name, *args, **kwargs)
            else:
                return func(self, *args, **kwargs)

        return wrapper

    @get_portname
    def set_running_mode(self, port_name, mode):
        return self.__dobotlink.MagicianGO.SetRunningMode(portName=port_name,
                                                          runningMode=mode)

    @get_portname
    def set_move_speed_direct(self, port_name, direction, speed):
        return self.__dobotlink.MagicianGO.SetMoveSpeedDirect(
            portName=port_name, dir=direction, speed=speed)

    @get_portname
    def set_move_speed(self, port_name, x, y, r):
        return self.__dobotlink.MagicianGO.SetMoveSpeed(portName=port_name,
                                                        x=x,
                                                        y=y,
                                                        r=r)

    @get_portname
    def set_rotate(self, port_name, r, Vr):
        return self.__dobotlink.MagicianGO.SetRotate(portName=port_name,
                                                     r=r,
                                                     Vr=Vr,
                                                     isQueued=True,
                                                     isWaitForFinish=True,
                                                     timeout=604800000)

    @get_portname
    def set_move_dist(self, port_name, x, y, Vx, Vy):
        return self.__dobotlink.MagicianGO.SetMoveDist(portName=port_name,
                                                       x=x,
                                                       y=y,
                                                       Vx=Vx,
                                                       Vy=Vy,
                                                       isQueued=True,
                                                       isWaitForFinish=True,
                                                       timeout=604800000)

    @get_portname
    def set_move_pos(self, port_name, x, y, s):
        speed_res = self.__dobotlink.MagicianGO.GetSpeedometer(
            portName=port_name)
        self.__dobotlink.MagicianGO.SetRotate(portName=port_name,
                                              r=-(speed_res["yaw"]),
                                              Vr=s,
                                              isQueued=True,
                                              isWaitForFinish=True,
                                              timeout=604800000)
        return self.__dobotlink.MagicianGO.SetMovePos(portName=port_name,
                                                      x=x,
                                                      y=y,
                                                      s=s,
                                                      isQueued=True,
                                                      isWaitForFinish=True,
                                                      timeout=604800000)

    @get_portname
    def set_arc_rad(self, port_name, velocity, radius, angle, mode):
        return self.__dobotlink.MagicianGO.SetArcRad(portName=port_name,
                                                     velocity=velocity,
                                                     radius=radius,
                                                     angle=angle,
                                                     mode=mode,
                                                     isQueued=True,
                                                     isWaitForFinish=True,
                                                     timeout=604800000)

    @get_portname
    def set_arc_cent(self, port_name, velocity, x, y, angle, mode):
        return self.__dobotlink.MagicianGO.SetArcCent(portName=port_name,
                                                      velocity=velocity,
                                                      x=x,
                                                      y=y,
                                                      angle=angle,
                                                      mode=mode,
                                                      isQueued=True,
                                                      isWaitForFinish=True,
                                                      timeout=604800000)

    @get_portname
    def set_coord_closed_loop(self, port_name, isEnable, angle):
        return self.__dobotlink.MagicianGO.SetCoordClosedLoop(
            portName=port_name, isEnable=isEnable, angle=angle)

    @get_portname
    def set_increment_closed_loop(self, port_name, x, y, angle):
        return self.__dobotlink.MagicianGO.SetIncrementClosedLoop(
            portName=port_name,
            x=x,
            y=y,
            angle=angle,
            isQueued=True,
            isWaitForFinish=True,
            timeout=604800000)

    @get_portname
    def set_rgb_light(self, port_name, number, effect, r, g, b, cycle, counts):
        if type(number) == "str":
            return self.__dobotlink.MagicianGO.SetLightRGB(
                portName=port_name,
                number=self.rgb_number[number],
                effect=effect,
                r=r,
                g=g,
                b=b,
                cycle=cycle,
                counts=counts)
        else:
            return self.__dobotlink.MagicianGO.SetLightRGB(portName=port_name,
                                                           number=number,
                                                           effect=effect,
                                                           r=r,
                                                           g=g,
                                                           b=b,
                                                           cycle=cycle,
                                                           counts=counts)

    @get_portname
    def set_buzzer_sound(self, port_name, index, tone, beat):
        return self.__dobotlink.MagicianGO.SetBuzzerSound(portName=port_name,
                                                          index=index,
                                                          tone=tone,
                                                          beat=beat)

    @get_portname
    def get_ultrasonic_data(self, port_name):
        u_data = self.__dobotlink.MagicianGO.GetUltrasoundData(
            portName=port_name)
        u_res = {
            "front": format(u_data["front"], ".2f"),
            "back": format(u_data["back"], ".2f"),
            "left": format(u_data["left"], ".2f"),
            "right": format(u_data["right"], ".2f")
        }
        return u_res

    @get_portname
    def set_odometer_data(self, port_name, x, y, yaw):
        return self.__dobotlink.MagicianGO.SetSpeedometer(portName=port_name,
                                                          x=x,
                                                          y=y,
                                                          yaw=yaw)

    @get_portname
    def get_odometer_data(self, port_name):
        o_data = self.__dobotlink.MagicianGO.GetSpeedometer(portName=port_name)
        o_res = {
            "x": format(o_data["x"], ".2f"),
            "y": format(o_data["y"], ".2f"),
            "yaw": format(o_data["yaw"], ".2f")
        }
        return o_res

    @get_portname
    def get_power_voltage(self, port_name):
        v_data = self.__dobotlink.MagicianGO.GetBatteryVoltage(
            portName=port_name)
        v_res = {
            "PowerVoltage": format(v_data["PowerVoltage"], ".2f"),
            "PowerPercentage": format(v_data["PowerPercentage"], ".2f")
        }
        return v_res

    @get_portname
    def get_imu_angle(self, port_name):
        a_data = self.__dobotlink.MagicianGO.GetImuAngle(portName=port_name)
        a_res = {
            "yaw": format(a_data["yaw"], ".2f"),
            "roll": format(a_data["roll"], ".2f"),
            "pitch": format(a_data["pitch"], ".2f")
        }
        return a_res

    @get_portname
    def get_imu_speed(self, port_name):
        s_data = self.__dobotlink.MagicianGO.GetImuSpeed(portName=port_name)
        s_res = {
            "ax": format(s_data["ax"], ".2f"),
            "ay": format(s_data["ay"], ".2f"),
            "az": format(s_data["az"], ".2f"),
            "gx": format(s_data["gx"], ".2f"),
            "gy": format(s_data["gy"], ".2f"),
            "gz": format(s_data["gz"], ".2f")
        }
        return s_res

    @get_portname
    def set_auto_trace(self, port_name, trace):
        if trace:
            self.__dobotlink.MagicianGO.SetTraceLoop(portName=port_name,
                                                     enable=True)
        else:
            self.__dobotlink.MagicianGO.SetTraceLoop(portName=port_name,
                                                     enable=False)
        return self.__dobotlink.MagicianGO.SetTraceAuto(portName=port_name,
                                                        isTrace=trace)

    @get_portname
    def set_trace_speed(self, port_name, speed):
        return self.__dobotlink.MagicianGO.SetTraceSpeed(portName=port_name,
                                                         speed=speed)

    @get_portname
    def set_trace_pid(self, port_name, p, i, d):
        return self.__dobotlink.MagicianGO.SetTracePid(portName=port_name,
                                                       p=p,
                                                       i=i,
                                                       d=d)

    @get_portname
    def get_trace_angle(self, port_name):
        an_data = self.__dobotlink.MagicianGO.GetCarCameraAngle(
            portName=port_name)
        an_res = {"angle": format(an_data["angle"], ".2f")}
        return an_res

    @get_portname
    def get_arm_camera_obj(self, port_name):
        return self.__dobotlink.MagicianGO.GetArmCameraObj(portName=port_name)

    @get_portname
    def get_car_camera_obj(self, port_name):
        return self.__dobotlink.MagicianGO.GetCarCameraObj(portName=port_name)

    @get_portname
    def get_arm_camera_tag(self, port_name):
        return self.__dobotlink.MagicianGO.GetArmCameraTag(portName=port_name)

    # 进入仓库
    def into_park_space(self, garage_class=0):
        '''小车入库,garage_class=0为进入仓库，1为进入超市'''
        if garage_class == 0:
            point = P1_in  # 智能仓库库内点
        else:
            point = P2_in  # 超市库内点
        self.set_auto_trace(trace=0)
        # self.set_auto_trace(trace=0) #关闭巡线
        self.set_move_speed_direct(direction=0, speed=-20)  # 后退
        # self.set_move_speed_direct(direction=0, speed=-20)  #后退
        time.sleep(1)
        self.set_move_speed_direct(direction=0, speed=0)
        self.set_trace_pid(p=0.5, i=0, d=0.5)
        self.set_trace_speed(speed=20)  # 入库必该速度巡线进入比较稳定
        self.set_auto_trace(trace=1)  # 打开巡线
        while True:
            value1 = self.get_odometer_data()
            if (value1['x'] < point[0] + 20):
                print("value1['x'] < point[0] + 20")
                self.set_auto_trace(trace=0)
                self.set_move_speed_direct(direction=0, speed=10)
                value1 = self.get_odometer_data()
                if (value1['x'] < point[0]):
                    print("value1['x'] < point[0]")
                    self.set_move_speed_direct(direction=0, speed=0)
                    break

    # 出仓
    def out_park_space(self, garage_class=0):
        if garage_class == 0:
            point = P1_out  # 智能仓库库外点
        else:
            point = P2_out  # 超市库外点
        self.set_move_speed_direct(direction=0, speed=-10)
        while 1:
            value1 = self.get_odometer_data()
            if (value1['x'] > point[0]):
                self.set_move_speed_direct(direction=0, speed=0)
                break

    def is_nearby(self, Point, num=40, err=2):
        ''' 判断有没有到指定点的位置附近,默认周围大小为20cm,如果出现到点不动作得到情况调大num默认值,如果定位不准确减小err默认值'''
        # global DeviationAngle
        Nx = Point[0]
        Ny = Point[1]
        value = self.get_odometer_data()
        print("x:" + str(value['x']) + "y:" + str(value['y']) + " r :" +
              str(value['yaw']))
        if (Nx + num) > value['x'] > (Nx - num) and (Ny + num) > value['y'] > (
                Ny - num):
            value = self.get_odometer_data()
            if ((self.get_yaw()[0] == 'X') or (self.get_yaw()[0] == '-X')):
                # DeviationAngle = self.get_yaw()[1]
                if (Nx + err > value['x'] > Nx - err):
                    return True
                else:
                    return False
            elif ((self.get_yaw()[0] == 'Y') or (self.get_yaw()[0] == '-Y')):
                # DeviationAngle = self.get_yaw()[1]
                if (Ny + err > value['y'] > Ny - err):
                    return True
                else:
                    return False
        return False

    # 备注：该函数is_nearby()接口需要调用到，不需开放给用户
    def get_yaw(self):
        ''' 返回当前行进方向 EWSN东西南北，同时返回偏离小车直行方向（XY）的角度值，偏左为负，偏右为正数'''
        value = self.get_odometer_data()
        if (45 > value['yaw'] > -45):
            return ['X', 0 - value['yaw']]
        elif (180 > value['yaw'] > 135 or -135 > value['yaw'] > -180):
            if 180 > value['yaw'] > 135:
                return ['-X', 180 - value['yaw']]
            else:
                return ['-X', -180 - value['yaw']]
        elif (-45 > value['yaw'] > -135):
            return ['-Y', -90 - value['yaw']]
        elif (135 > value['yaw'] > 45):
            return ['Y', 90 - value['yaw']]
        else:
            return ['O', 0]

    def _dot(self, a, b):
        s = 0
        for i in range(0, len(b)):
            s += a[i] * b[i]
        return s

    def _mul(self, A, b):
        return [self._dot(row, b) for row in A]

    def _imgh1_to_imgh2(self, Mx, My, needTranXY, AprilTag_H):
        '''物体平面与标定平面图像坐标转换'''
        if needTranXY:
            p_x_camera = 223 + 48  # 摄像头中心所在的机器x坐标
            p_y_camera = 0  # 48是末端到摄像头中心的距离
        else:
            p_x_camera = 0
            p_y_camera = -223 - 48
        x1 = Mx - p_x_camera
        y1 = My - p_y_camera
        self.theta_h = AprilTag_H - 5  # 5为标定平面高度，后续改为10(小方块标定高度)
        tran_Mx = x1 * (1 - self.theta_h / self.camera_h) + p_x_camera
        tran_My = y1 * (1 - self.theta_h / self.camera_h) + p_y_camera
        return tran_Mx, tran_My

    def imgxy_to_armxy(self,
                       px,
                       py,
                       needTranXY,
                       suck_AprilTag=False,
                       AprilTag_H=5):
        '''图像坐标转机器坐标'''
        img_point = [px, py, 1]
        q = self._mul(self.calibra_h_car, img_point)
        dobot_point = [qt / q[-1] for qt in q[:-1]]
        Mx = dobot_point[0]
        My = dobot_point[1]
        if needTranXY:  # 在仓库置物盒上方，需要坐标转换
            temp = Mx
            Mx = -My
            My = temp
        if suck_AprilTag:
            Mx, My = self._imgh1_to_imgh2(Mx, My, needTranXY, AprilTag_H)
        return Mx, My

    # 由于下面两个函数调用到lite的api，所以在此处封装两个lite接口
    @get_portname
    def _set_ptpcmd(self,
                    port_name,
                    ptp_mode: int,
                    x: float,
                    y: float,
                    z: float,
                    r: float,
                    is_queued=True,
                    is_wait=True):
        return self.__dobotlink.MagicianLite.SetPTPCmd(portName=port_name,
                                                       ptpMode=ptp_mode,
                                                       x=x,
                                                       y=y,
                                                       z=z,
                                                       r=r,
                                                       isQueued=is_queued,
                                                       isWaitForFinish=is_wait,
                                                       timeout=86400000)

    @get_portname
    def _set_endeffector_gripper(self,
                                 port_name,
                                 enable: bool,
                                 on: bool,
                                 is_queued=False):
        return self.__dobotlink.MagicianLite.SetEndEffectorGripper(
            portName=port_name, enable=enable, on=on, isQueued=is_queued)

    def grab_obj_cartofloor(self, object_class=0):
        '''从小车上抓取生鲜/快递到地面置物盒,object_class=0、1、2,分别代表生鲜快递、生鲜、快递'''
        global p_x0, p_x1, p_x2, p_x, p_y, p_y0, p_y1, p_y2, p_z0, p_z1, p_z2
        global p_R, p_R0, p_R1, p_R2, p_R0_grap
        global X1, X2, X3, Y1, Y2, Y3, Z1, Z2, Z3
        global move_x0, move_y0, move_time
        global object_id
        detect_flag = True
        need_trans = False
        NoObj_time = 0
        cannotReach_time = 0
        move_x0 = 0
        move_y0 = 0
        move_time = 0
        if object_class == 0:  # 0代表生鲜和快递
            index_1 = -1
            index_2 = 6
        elif object_class == 1:  # 1代表生鲜
            index_1 = -1
            index_2 = 3
        else:  # object_class == 2:代表快递
            index_1 = 2
            index_2 = 6
        self._set_endeffector_gripper(enable=True, on=True)  # 闭合
        time.sleep(0.5)

        while detect_flag:
            self._set_ptpcmd(ptp_mode=1,
                             x=p_x0 + move_x0,
                             y=p_y0 + move_y0,
                             z=p_z0,
                             r=p_R0)
            time.sleep(0.8)
            datalist = self.get_arm_camera_obj()  # 检测物品
            NoObj_time = 0
            while datalist[
                    'count'] == 0 and NoObj_time < 5:  # 防止某一瞬间没检测到物品导致认为没有物品
                datalist = self.get_arm_camera_obj()
                NoObj_time += 1
            if datalist['count']:  # 是否有物体
                for i in range(datalist['count']):
                    if datalist['dl_obj'][i]['id'] > index_1 and datalist[
                            'dl_obj'][i]['id'] < index_2:  # 取快递/生鲜进行抓取
                        cx = datalist['dl_obj'][i][
                            'x'] + datalist['dl_obj'][i]['w'] / 2
                        cy = datalist['dl_obj'][i][
                            'y'] + datalist['dl_obj'][i]['h'] / 2
                        p_x, p_y = self.imgxy_to_armxy(cx, cy, need_trans)
                        object_id = datalist['dl_obj'][i]['id']

                        if -100 < p_x < 90 and -260 < p_y < -195:
                            cannotReach_time = 0
                        else:
                            print("Can't reach!")
                            # 防止误检测盒子外部物体时，一直while循环
                            # 处理方式：连续3次检测到无法到达抓取位置的物体，则结束while循环
                            cannotReach_time += 1
                            if cannotReach_time > 3:
                                detect_flag = False
                            continue  # 无法到达抓取位置，则抓取下一个物体

                        if object_id == 2:  # 水瓶特殊抓取策略
                            w = datalist['dl_obj'][i]['w']
                            h = datalist['dl_obj'][i]['h']
                            if w < h:  # 水瓶偏向“竖”放在小车置物盒上
                                p_R = p_R1
                            else:  # 水瓶偏向“横”放在小车置物盒上
                                p_R = p_R0_grap

                        self._set_endeffector_gripper(enable=False,
                                                      on=True)  # 常态
                        self._set_ptpcmd(1, p_x, p_y, 35, p_R)  # 物体上方过度点
                        self._set_endeffector_gripper(enable=True,
                                                      on=False)  # 张开
                        time.sleep(0.5)  # 延迟一会
                        self._set_ptpcmd(1, p_x, p_y, 17, p_R)  # 抓取位置
                        self._set_endeffector_gripper(enable=True,
                                                      on=True)  # 闭合
                        time.sleep(0.5)  # 延迟一会抓稳
                        self._set_ptpcmd(1, p_x, p_y, 9, p_R)  # 下压抓稳位置
                        self._set_ptpcmd(1, p_x0, p_y0, p_z0,
                                         p_R)  # 先回归位，作为中转点
                        self._set_ptpcmd(1, p_x1, p_y1, p_z1, p_R0)  # 仓库上方点

                        if (object_id == 0) or (object_id == 1) or (
                                object_id == 3) or (object_id == 4):  # 橙子\白菜
                            self._set_ptpcmd(1, X1, Y1, Z1, p_R0)  # 丢到固定位置1
                            self._set_endeffector_gripper(enable=True,
                                                          on=False)  # 张开
                        else:  # 水瓶
                            self._set_ptpcmd(1, X2, Y2, Z2, p_R0)  # 丢到固定位置2
                            self._set_endeffector_gripper(enable=True,
                                                          on=False)  # 张开
                        time.sleep(0.5)
                        self._set_ptpcmd(1, p_x1, p_y1, p_z1, p_R0)  # 仓库上方点
                        self._set_endeffector_gripper(enable=True,
                                                      on=True)  # 闭合
                        break  # 抓取检测到的第一个目标成功，则结束for循环

                    elif i == datalist[
                            'count'] - 1:  # 循环到最后一个检测到的物体不是目标物体，移动机械臂一下，防止漏检
                        move_time += 1
                        if move_time == 1:  # 第一次没检测到物体，机械臂往里面走10mm
                            move_x0 = 0
                            move_y0 = 10
                            continue
                        elif move_time == 2:  # 第二次没检测到物体，机械臂往外面走10mm
                            move_x0 = 0
                            move_y0 = -10
                            continue
                        else:
                            print("No target object!")
                            detect_flag = False
            else:
                move_time += 1
                if move_time == 1:  # 第一次没检测到物体，机械臂往里面走10mm
                    move_x0 = 0
                    move_y0 = 10
                    continue
                elif move_time == 2:  # 第二次没检测到物体，机械臂往外面走10mm
                    move_x0 = 0
                    move_y0 = -10
                    continue
                else:
                    print("No object!")
                    detect_flag = False
        self._set_ptpcmd(ptp_mode=1, x=0, y=-200, z=50, r=0)
        self._set_endeffector_gripper(enable=False, on=True)

    def grab_obj_floortoCar(self, object_class=0):
        '''从地面置物盒抓取生鲜快递到小车上,object_class=0、1、2,分别代表生鲜快递、生鲜、快递'''
        global p_x0, p_x1, p_x2, p_x, p_y, p_y0, p_y1, p_y2, p_z0, p_z1, p_z2
        global p_R, p_R0, p_R1, p_R2, p_R0_throw
        global object_id
        thetaX = 0  # 放置置物盒位置增量
        catch_num = 0  # 物品计数
        detect_flag = True
        need_trans = True
        NoObj_time = 0
        cannotReach_time = 0
        move_x0 = 0
        move_y0 = 0
        move_time = 0
        if object_class == 0:  # 代表生鲜和快递
            index_1 = -1
            index_2 = 6
        elif object_class == 1:  # 1代表生鲜
            index_1 = -1
            index_2 = 3
        else:  # object_class == 2:代表快递
            index_1 = 2
            index_2 = 6
        self._set_endeffector_gripper(enable=True, on=True)  # 闭合
        time.sleep(0.5)

        while detect_flag and catch_num < 3:  # 有物体且已抓取的数量小于3个
            self._set_ptpcmd(1, p_x1 + move_x0, p_y1 + move_y0, p_z1, p_R1)
            time.sleep(0.8)
            # 检测物品
            datalist = self.get_arm_camera_obj()
            NoObj_time = 0
            while datalist['count'] == 0 and NoObj_time < 5:
                datalist = self.get_arm_camera_obj()
                NoObj_time += 1
            if datalist['count']:  # 有物体且已抓取的数量小于3个
                for i in range(datalist['count']):  # 取第一个目标物体进行抓取
                    if datalist['dl_obj'][i]['id'] > index_1 and datalist[
                            'dl_obj'][i]['id'] < index_2:  # 取目标物体进行抓取
                        catch_num += 1  # 已抓取物品数量+1
                        cx = datalist['dl_obj'][i][
                            'x'] + datalist['dl_obj'][i]['w'] / 2
                        cy = datalist['dl_obj'][i][
                            'y'] + datalist['dl_obj'][i]['h'] / 2
                        p_x, p_y = self.imgxy_to_armxy(cx, cy, need_trans)
                        object_id = datalist['dl_obj'][i]['id']
                        if 180 < p_x < 317 and -284 < p_y < 160:
                            cannotReach_time = 0
                        else:
                            print("Can't reach!")
                            # 防止误检测盒子外部物体时，一直while循环
                            # 处理方式：连续3次检测到无法到达抓取位置的物体，则结束while循环
                            cannotReach_time += 1
                            if cannotReach_time > 3:
                                detect_flag = False
                            continue  # 无法到达抓取位置，则抓取下一个物体
                        if object_id == 2:  # 水瓶特殊抓取策略
                            w = datalist['dl_obj'][i]['w']
                            h = datalist['dl_obj'][i]['h']
                            if w < h:  # 水瓶偏向“竖”放在地面置物盒上
                                p_R = p_R0
                                p_R0_throw = p_R0
                            else:
                                p_R = p_R1
                                p_R0_throw = -170
                        self._set_endeffector_gripper(enable=False,
                                                      on=True)  # 常态
                        self._set_ptpcmd(1, p_x, p_y, 35, p_R)  # 物体上放过度点
                        self._set_endeffector_gripper(enable=True,
                                                      on=False)  # 张开
                        time.sleep(0.5)  # 延迟一会抓稳
                        self._set_ptpcmd(1, p_x, p_y, 17, p_R)  # 抓取位置
                        self._set_endeffector_gripper(enable=True,
                                                      on=True)  # 闭合
                        time.sleep(0.5)  # 延迟一会抓稳
                        self._set_ptpcmd(1, p_x, p_y, 9, p_R)  # 下压抓稳位置
                        self._set_ptpcmd(1, p_x1, p_y1, p_z1,
                                         p_R1)  # 先回归位，作为中转点
                        self._set_ptpcmd(1, p_x0, p_y0, p_z0,
                                         p_R)  # 小车置物盒上方零位点
                        self._set_ptpcmd(
                            1, p_x2 + thetaX, p_y2, p_z2,
                            p_R0_throw)  # 抓取到的物品按从左到右，从上到下顺序放置在置物盒
                        self._set_endeffector_gripper(enable=True,
                                                      on=False)  # 张开
                        time.sleep(0.5)
                        self._set_ptpcmd(1, p_x0, p_y0, p_z0,
                                         p_R0)  # 回到小车置物盒上方零位点
                        self._set_endeffector_gripper(enable=True,
                                                      on=True)  # 闭合
                        time.sleep(1)  # 这里需要等待1s秒钟等待机械臂末端完全闭合
                        # 检测物品数量，判断是否抓取成功并放置置物盒
                        datalist = self.get_arm_camera_obj()
                        NoObj_time = 0
                        while datalist['count'] == 0 and NoObj_time < 5:
                            datalist = self.get_arm_camera_obj()
                            NoObj_time += 1
                        if datalist[
                                'count'] == catch_num:  # 目前置物盒物体数量是否等于已抓取数量
                            thetaX += 60  # 设置下次放置点
                        else:  # 如果数量和不等于抓取数量（可能未成功抓取，或者漏检测物体），则机械臂往里面走10mm再次检测
                            move_x0 = 0
                            move_y0 = 10
                            self._set_ptpcmd(1, p_x0 + move_x0, p_y0 + move_y0,
                                             p_z0, p_R0)  # 小车置物盒上方零位点
                            time.sleep(0.5)
                            datalist = self.get_arm_camera_obj()
                            NoObj_time = 0
                            while datalist['count'] == 0 and NoObj_time < 5:
                                datalist = self.get_arm_camera_obj()
                                NoObj_time += 1
                            if datalist['count'] == catch_num:
                                thetaX += 60  # 设置下次放置点
                            else:
                                catch_num -= 1
                        break  # 抓取检测到的第一个目标成功，则结束for循环

                    elif i == datalist[
                            'count'] - 1:  # 循环到最后一个检测到的物体不是目标物体，移动机械臂一下，防止漏检
                        move_time += 1
                        if move_time == 1:  # 第一次没检测到物体，机械臂往里面走10mm
                            move_x0 = -10
                            move_y0 = 0
                            continue
                        elif move_time == 2:  # 第二次没检测到物体，机械臂往外面走10mm
                            move_x0 = 10
                            move_y0 = 0
                            continue
                        else:
                            print("No target object!")
                            detect_flag = False
            else:
                move_time += 1
                if move_time == 1:  # 第一次没检测到物体，机械臂往里面走10mm
                    move_x0 = -10
                    move_y0 = 0
                    continue
                elif move_time == 2:  # 第二次没检测到物体，机械臂往外面走10mm
                    move_x0 = 10
                    move_y0 = 0
                    continue
                else:
                    print("No object!")
                    detect_flag = False
        print("There is no target object or the number has exceeded 3!")
        self._set_ptpcmd(ptp_mode=1, x=0, y=-200, z=50, r=0)
        self._set_endeffector_gripper(enable=False, on=True)

    # 到置物盒检测位置
    def set_ptp_box(self):
        self._set_ptpcmd(1, p_x0, p_y0, p_z0, p_R0)

    # 到地面上方检测位置
    def set_ptp_floor(self):
        self._set_ptpcmd(1, p_x1, p_y1, p_z1, p_R1)
