<gh_stars>0
# encoding: UTF-8
#!/usr/bin/env python2
import cv2 as cv
import os
import numpy as np
import time
import rospy
from visualization_msgs.msg import Marker
from moving_utils import Movement

# y轴偏移量
pump_y = -55
# x轴偏移量
pump_x = 15


class Detect_marker(Movement):
    def __init__(self):
        super(Detect_marker, self).__init__()
        # set cache of real coord
        self.cache_x = self.cache_y = 0

        # which robot
        self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
        self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
        self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
        self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
        self.raspi = False
        if "dev" in self.robot_m5:
            self.Pin = [2, 5]
        elif "dev" in self.robot_wio:
            self.Pin = [20, 21]
            for i in self.move_coords:
                i[2] -= 20
        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
            import RPi.GPIO as GPIO
            self.GPIO = GPIO
            GPIO.setwarnings(False)
            GPIO.setmode(GPIO.BCM)
            GPIO.setup(20, GPIO.OUT)
            GPIO.setup(21, GPIO.OUT)
            self.raspi = True
        if self.raspi:
            self.gpio_status(False)
        else:
            self.pub_pump(False, self.Pin)
        # Creating a Camera Object
        cap_num = 0
        self.cap = cv.VideoCapture(cap_num, cv.CAP_V4L)
        # Get ArUco marker dict that can be detected.
        self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
        # Get ArUco marker params.
        self.aruco_params = cv.aruco.DetectorParameters_create()
        self.calibrationParams = cv.FileStorage(
            "calibrationFileName.xml", cv.FILE_STORAGE_READ)
        # Get distance coefficient.
        self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat()

        height = self.cap.get(4)
        focal_length = width = self.cap.get(3)
        center = [width / 2, height / 2]
        # Calculate the camera matrix.
        self.camera_matrix = np.array(
            [
                [focal_length, 0, center[0]],
                [0, focal_length, center[1]],
                [0, 0, 1],
            ],
            dtype=np.float32,
        )
        # init a node and a publisher
        rospy.init_node("encode_marker", anonymous=True)
        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)

        self.marker = Marker()
        self.marker.header.frame_id = "/joint1"
        self.marker.ns = "cube"
        self.marker.type = self.marker.CUBE
        self.marker.action = self.marker.ADD
        self.marker.scale.x = 0.04
        self.marker.scale.y = 0.04
        self.marker.scale.z = 0.04
        self.marker.color.a = 1
        self.marker.color.r = 0.3
        self.marker.color.g = 0.3
        self.marker.color.b = 0.3

        # marker position initial
        self.marker.pose.position.x = 0
        self.marker.pose.position.y = 0
        self.marker.pose.position.z = 0.03
        self.marker.pose.orientation.x = 0
        self.marker.pose.orientation.y = 0
        self.marker.pose.orientation.z = 0
        self.marker.pose.orientation.w = 1.0

    # Grasping motion
    def move(self, x, y):
        if self.raspi:
            coords = [
                [145.6, -64.9, 285.2, 179.88, 7.67, 179],
                [130.1, -155.6, 243.9, 178.99, 5.38, -179.9]
            ]
        else:
            coords = [
                [135.0, -65.5, 280.1, 178.99, 5.38, -179.9],
                [136.1, -141.6, 243.9, 178.99, 5.38, -179.9]
            ]

        # publish marker
        self.marker.header.stamp = rospy.Time.now()
        self.marker.pose.position.x = (coords[0][0]-x)/1000.0
        self.marker.pose.position.y = (coords[0][1]-y)/1000.0
        self.pub.publish(self.marker)

        # send coordinates to move mycobot
        self.pub_coords(coords[0], 30, 1)
        time.sleep(2)
        self.pub_coords([coords[0][0]-x, coords[0][1]-y,
                        240, 178.99, 5.38, -179.9], 25, 1)
        time.sleep(2)
        self.pub_coords([coords[0][0]-x, coords[0][1]-y,
                        200, 178.99, 5.38, -179.9], 25, 1)
        time.sleep(2)
        if "dev" in self.robot_m5 or self.raspi:
            self.pub_coords([coords[0][0]-x, coords[0][1]-y,
                            90, 178.99, 5.38, -179.9], 25, 1)
        elif "dev" in self.robot_wio:
            self.pub_coords([coords[0][0]-x+20, coords[0][1] -
                            y-10, 70, 178.99, 5.38, -179.9], 25, 1)
        time.sleep(2)
        if self.raspi:
            self.gpio_status(True)
        else:
            self.pub_pump(True, self.Pin)
        time.sleep(1)
        self.pub_coords(coords[0], 30, 1)
        time.sleep(3)
        self.pub_coords(coords[1], 30, 1)
        time.sleep(2)
        if self.raspi:
            self.gpio_status(False)
        else:
            self.pub_pump(False, self.Pin)
        # publish marker
        time.sleep(1)
        self.marker.header.stamp = rospy.Time.now()
        self.marker.pose.position.x = coords[1][0]/1000.0
        self.marker.pose.position.y = coords[1][1]/1000.0
        self.pub.publish(self.marker)

        self.pub_coords(coords[0], 30, 1)
        time.sleep(2)

    def gpio_status(self, flag):
        if flag:
            self.GPIO.output(20, 0)
            self.GPIO.output(21, 0)
        else:
            self.GPIO.output(20, 1)
            self.GPIO.output(21, 1)

    # decide whether grab cube
    def decide_move(self, x, y):

        print(x, y)
        # detect the cube status move or run
        if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5:  # mm
            self.cache_x, self.cache_y = x, y
            return
        else:
            self.cache_x = self.cache_y = 0
            if "dev" in self.robot_jes:
                if x > -20:
                    y += 10
                if y > -25:
                    x -= 5
                x += 10

            self.move(x, y)

    # init mycobot
    def init_mycobot(self):

        for _ in range(5):
            print _
            self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, 179], 20, 1)
            time.sleep(0.5)

    def run(self):
        global pump_y, pump_x
        self.init_mycobot()
        num = sum_x = sum_y = 0
        while cv.waitKey(1) < 0:
            success, img = self.cap.read()
            if not success:
                print("It seems that the image cannot be acquired correctly.")
                break

            # transfrom the img to model of gray
            gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
            # Detect ArUco marker.
            corners, ids, rejectImaPoint = cv.aruco.detectMarkers(
                gray, self.aruco_dict, parameters=self.aruco_params
            )

            if len(corners) > 0:
                if ids is not None:
                    # get informations of aruco
                    ret = cv.aruco.estimatePoseSingleMarkers(
                        corners, 0.03, self.camera_matrix, self.dist_coeffs
                    )
                    # rvec:rotation offset,tvec:translation deviator
                    (rvec, tvec) = (ret[0], ret[1])
                    (rvec - tvec).any()
                    xyz = tvec[0, 0, :]
                    # calculate the coordinates of the aruco relative to the pump
                    xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1]
                                                               * 1000+pump_x, 2), round(xyz[2]*1000, 2)]

                    for i in range(rvec.shape[0]):
                        # draw the aruco on img
                        cv.aruco.drawDetectedMarkers(img, corners)
                        cv.aruco.drawAxis(
                            img,
                            self.camera_matrix,
                            self.dist_coeffs,
                            rvec[i, :, :],
                            tvec[i, :, :],
                            0.03,
                        )

                        if num < 40:
                            if self.raspi:
                                sum_x -= 30
                            sum_x += xyz[1]
                            sum_y += xyz[0]
                            num += 1
                        elif num == 40:
                            self.decide_move(sum_x/40.0, sum_y/40.0)
                            num = sum_x = sum_y = 0

            cv.imshow("encode_image", img)


if __name__ == "__main__":
    detect = Detect_marker()
    detect.run()

# encoding: UTF-8
#!/usr/bin/env python2
import cv2 as cv
import os
import numpy as np
import time
import rospy
from visualization_msgs.msg import Marker
from moving_utils import Movement

# y轴偏移量
pump_y = -55
# x轴偏移量
pump_x = 15


class Detect_marker(Movement):
    def __init__(self):
        super(Detect_marker, self).__init__()
        # set cache of real coord
        self.cache_x = self.cache_y = 0

        # which robot
        self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
        self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
        self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
        self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
        self.raspi = False
        if "dev" in self.robot_m5:
            self.Pin = [2, 5]
        elif "dev" in self.robot_wio:
            self.Pin = [20, 21]
            for i in self.move_coords:
                i[2] -= 20
        elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
            import RPi.GPIO as GPIO
            self.GPIO = GPIO
            GPIO.setwarnings(False)
            GPIO.setmode(GPIO.BCM)
            GPIO.setup(20, GPIO.OUT)
            GPIO.setup(21, GPIO.OUT)
            self.raspi = True
        if self.raspi:
            self.gpio_status(False)
        else:
            self.pub_pump(False, self.Pin)
        # Creating a Camera Object
        cap_num = 0
        self.cap = cv.VideoCapture(cap_num, cv.CAP_V4L)
        # Get ArUco marker dict that can be detected.
        self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
        # Get ArUco marker params.
        self.aruco_params = cv.aruco.DetectorParameters_create()
        self.calibrationParams = cv.FileStorage(
            "calibrationFileName.xml", cv.FILE_STORAGE_READ)
        # Get distance coefficient.
        self.dist_coeffs = self.calibrationParams.getNode("distCoeffs").mat()

        height = self.cap.get(4)
        focal_length = width = self.cap.get(3)
        center = [width / 2, height / 2]
        # Calculate the camera matrix.
        self.camera_matrix = np.array(
            [
                [focal_length, 0, center[0]],
                [0, focal_length, center[1]],
                [0, 0, 1],
            ],
            dtype=np.float32,
        )
        # init a node and a publisher
        rospy.init_node("encode_marker", anonymous=True)
        self.pub = rospy.Publisher('/cube', Marker, queue_size=1)

        self.marker = Marker()
        self.marker.header.frame_id = "/joint1"
        self.marker.ns = "cube"
        self.marker.type = self.marker.CUBE
        self.marker.action = self.marker.ADD
        self.marker.scale.x = 0.04
        self.marker.scale.y = 0.04
        self.marker.scale.z = 0.04
        self.marker.color.a = 1
        self.marker.color.r = 0.3
        self.marker.color.g = 0.3
        self.marker.color.b = 0.3

        # marker position initial
        self.marker.pose.position.x = 0
        self.marker.pose.position.y = 0
        self.marker.pose.position.z = 0.03
        self.marker.pose.orientation.x = 0
        self.marker.pose.orientation.y = 0
        self.marker.pose.orientation.z = 0
        self.marker.pose.orientation.w = 1.0

    # Grasping motion
    def move(self, x, y):
        if self.raspi:
            coords = [
                [145.6, -64.9, 285.2, 179.88, 7.67, 179],
                [130.1, -155.6, 243.9, 178.99, 5.38, -179.9]
            ]
        else:
            coords = [
                [135.0, -65.5, 280.1, 178.99, 5.38, -179.9],
                [136.1, -141.6, 243.9, 178.99, 5.38, -179.9]
            ]

        # publish marker
        self.marker.header.stamp = rospy.Time.now()
        self.marker.pose.position.x = (coords[0][0]-x)/1000.0
        self.marker.pose.position.y = (coords[0][1]-y)/1000.0
        self.pub.publish(self.marker)

        # send coordinates to move mycobot
        self.pub_coords(coords[0], 30, 1)
        time.sleep(2)
        self.pub_coords([coords[0][0]-x, coords[0][1]-y,
                        240, 178.99, 5.38, -179.9], 25, 1)
        time.sleep(2)
        self.pub_coords([coords[0][0]-x, coords[0][1]-y,
                        200, 178.99, 5.38, -179.9], 25, 1)
        time.sleep(2)
        if "dev" in self.robot_m5 or self.raspi:
            self.pub_coords([coords[0][0]-x, coords[0][1]-y,
                            90, 178.99, 5.38, -179.9], 25, 1)
        elif "dev" in self.robot_wio:
            self.pub_coords([coords[0][0]-x+20, coords[0][1] -
                            y-10, 70, 178.99, 5.38, -179.9], 25, 1)
        time.sleep(2)
        if self.raspi:
            self.gpio_status(True)
        else:
            self.pub_pump(True, self.Pin)
        time.sleep(1)
        self.pub_coords(coords[0], 30, 1)
        time.sleep(3)
        self.pub_coords(coords[1], 30, 1)
        time.sleep(2)
        if self.raspi:
            self.gpio_status(False)
        else:
            self.pub_pump(False, self.Pin)
        # publish marker
        time.sleep(1)
        self.marker.header.stamp = rospy.Time.now()
        self.marker.pose.position.x = coords[1][0]/1000.0
        self.marker.pose.position.y = coords[1][1]/1000.0
        self.pub.publish(self.marker)

        self.pub_coords(coords[0], 30, 1)
        time.sleep(2)

    def gpio_status(self, flag):
        if flag:
            self.GPIO.output(20, 0)
            self.GPIO.output(21, 0)
        else:
            self.GPIO.output(20, 1)
            self.GPIO.output(21, 1)

    # decide whether grab cube
    def decide_move(self, x, y):

        print(x, y)
        # detect the cube status move or run
        if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5:  # mm
            self.cache_x, self.cache_y = x, y
            return
        else:
            self.cache_x = self.cache_y = 0
            if "dev" in self.robot_jes:
                if x > -20:
                    y += 10
                if y > -25:
                    x -= 5
                x += 10

            self.move(x, y)

    # init mycobot
    def init_mycobot(self):

        for _ in range(5):
            print _
            self.pub_coords([145.6, -64.9, 285.2, 179.88, 7.67, 179], 20, 1)
            time.sleep(0.5)

    def run(self):
        global pump_y, pump_x
        self.init_mycobot()
        num = sum_x = sum_y = 0
        while cv.waitKey(1) < 0:
            success, img = self.cap.read()
            if not success:
                print("It seems that the image cannot be acquired correctly.")
                break

            # transfrom the img to model of gray
            gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
            # Detect ArUco marker.
            corners, ids, rejectImaPoint = cv.aruco.detectMarkers(
                gray, self.aruco_dict, parameters=self.aruco_params
            )

            if len(corners) > 0:
                if ids is not None:
                    # get informations of aruco
                    ret = cv.aruco.estimatePoseSingleMarkers(
                        corners, 0.03, self.camera_matrix, self.dist_coeffs
                    )
                    # rvec:rotation offset,tvec:translation deviator
                    (rvec, tvec) = (ret[0], ret[1])
                    (rvec - tvec).any()
                    xyz = tvec[0, 0, :]
                    # calculate the coordinates of the aruco relative to the pump
                    xyz = [round(xyz[0]*1000+pump_y, 2), round(xyz[1]
                                                               * 1000+pump_x, 2), round(xyz[2]*1000, 2)]

                    for i in range(rvec.shape[0]):
                        # draw the aruco on img
                        cv.aruco.drawDetectedMarkers(img, corners)
                        cv.aruco.drawAxis(
                            img,
                            self.camera_matrix,
                            self.dist_coeffs,
                            rvec[i, :, :],
                            tvec[i, :, :],
                            0.03,
                        )

                        if num < 40:
                            if self.raspi:
                                sum_x -= 30
                            sum_x += xyz[1]
                            sum_y += xyz[0]
                            num += 1
                        elif num == 40:
                            self.decide_move(sum_x/40.0, sum_y/40.0)
                            num = sum_x = sum_y = 0

            cv.imshow("encode_image", img)


if __name__ == "__main__":
    detect = Detect_marker()
    detect.run()
# encoding: UTF-8 #!/usr/bin/env python2 # y轴偏移量 # x轴偏移量 # set cache of real coord # which robot # Creating a Camera Object # Get ArUco marker dict that can be detected. # Get ArUco marker params. # Get distance coefficient. # Calculate the camera matrix. # init a node and a publisher # marker position initial # Grasping motion # publish marker # send coordinates to move mycobot # publish marker # decide whether grab cube # detect the cube status move or run # mm # init mycobot # transfrom the img to model of gray # Detect ArUco marker. # get informations of aruco # rvec:rotation offset,tvec:translation deviator # calculate the coordinates of the aruco relative to the pump # draw the aruco on img