Merge pull request #143 from elephantrobotics/280AR-track-opt

280 ar track opt

mycobot_280/mycobot_280arduino/CMakeLists.txt

@@ -27,6 +27,7 @@ catkin_install_python(PROGRAMS
   scripts/listen_real.py
   scripts/listen_real_of_topic.py
   scripts/simple_gui.py
+  scripts/detect_stag.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

mycobot_280/mycobot_280arduino/camera_calibration/EyesInHand_matrix.json

@@ -0,0 +1 @@
+[[0.7978586214708513, 0.59877174890628, -0.06995722161683607, -42.33432105525292], [0.6010697464630348, -0.7990332868561791, 0.016154453957772947, -35.01038302335407], [0.04622531807890511, 0.05493813982585287, 0.9974191800647204, 39.342478235028345], [0.0, 0.0, 0.0, 1.0]]

mycobot_280/mycobot_280arduino/camera_calibration/camera_calibration.py

@@ -0,0 +1,307 @@
+import cv2
+from uvc_camera import UVCCamera
+import stag
+import numpy as np
+import json
+import time
+import os
+from scipy.linalg import svd
+from pymycobot import *
+from marker_utils import *
+import shutil
+import glob
+
+
+ports = glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyACM*') + glob.glob('/dev/ttyAMA*')
+print(ports)
+if ports:
+    arm_port = ports[0]
+else:
+    raise Exception("No MyCobot device found")
+
+
+mc = MyCobot280(port=arm_port, baudrate=1000000)  # 设置端口
+            
+np.set_printoptions(suppress=True, formatter={'float_kind': '{:.2f}'.format})
+
+
+
+class camera_detect:
+    #Camera parameter initialize
+    def __init__(self, camera_id, marker_size, mtx, dist):
+        self.camera_id = camera_id
+        self.mtx = mtx
+        self.dist = dist
+        self.marker_size = marker_size
+        self.camera = UVCCamera(self.camera_id, self.mtx, self.dist)
+        self.camera_open()
+
+        self.origin_mycbot_horizontal = [-90, -35.85, -52.91, 88.59, 0, 0.0]
+        self.origin_mycbot_level = [-90, 5, -104, 14, 0, 0]
+        
+        self.IDENTIFY_LEN = 300
+   
+        # Initialize EyesInHand_matrix to None or load from a document if available
+        self.EyesInHand_matrix = None
+        file_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+        self.matrix_file_path = os.path.join(file_dir, "config","EyesInHand_matrix.json")
+    
+        self.load_matrix()
+
+    def save_matrix(self, filename="EyesInHand_matrix.json"):
+        # Save the EyesInHand_matrix to a JSON file
+        if self.EyesInHand_matrix is not None:
+            with open(filename, 'w') as f:
+                json.dump(self.EyesInHand_matrix.tolist(), f)
+                
+            try:
+                # 复制文件到目标路径
+                shutil.copy(filename, self.matrix_file_path)
+                print(f"File copied to {self.matrix_file_path}")
+            except IOError as e:
+                print(f"Failed to copy file: {e}")
+    
+    def load_matrix(self, filename="EyesInHand_matrix.json"):
+        # Load the EyesInHand_matrix from a JSON file, if it exists
+        try:
+            with open(filename, 'r') as f:
+                self.EyesInHand_matrix = np.array(json.load(f))
+        except FileNotFoundError:
+            print("Matrix file not found. EyesInHand_matrix will be initialized later.")
+            
+    def CvtRotationMatrixToEulerAngle(self, pdtRotationMatrix):
+        pdtEulerAngle = np.zeros(3)
+        pdtEulerAngle[2] = np.arctan2(pdtRotationMatrix[1, 0], pdtRotationMatrix[0, 0])
+        fCosRoll = np.cos(pdtEulerAngle[2])
+        fSinRoll = np.sin(pdtEulerAngle[2])
+        pdtEulerAngle[1] = np.arctan2(-pdtRotationMatrix[2, 0],
+                                      (fCosRoll * pdtRotationMatrix[0, 0]) + (fSinRoll * pdtRotationMatrix[1, 0]))
+        pdtEulerAngle[0] = np.arctan2((fSinRoll * pdtRotationMatrix[0, 2]) - (fCosRoll * pdtRotationMatrix[1, 2]),
+                                      (-fSinRoll * pdtRotationMatrix[0, 1]) + (fCosRoll * pdtRotationMatrix[1, 1]))
+        return pdtEulerAngle
+
+    # 将欧拉角转为旋转矩阵
+    def CvtEulerAngleToRotationMatrix(self, ptrEulerAngle):
+        ptrSinAngle = np.sin(ptrEulerAngle)
+        ptrCosAngle = np.cos(ptrEulerAngle)
+        ptrRotationMatrix = np.zeros((3, 3))
+        ptrRotationMatrix[0, 0] = ptrCosAngle[2] * ptrCosAngle[1]
+        ptrRotationMatrix[0, 1] = ptrCosAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] - ptrSinAngle[2] * ptrCosAngle[0]
+        ptrRotationMatrix[0, 2] = ptrCosAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] + ptrSinAngle[2] * ptrSinAngle[0]
+        ptrRotationMatrix[1, 0] = ptrSinAngle[2] * ptrCosAngle[1]
+        ptrRotationMatrix[1, 1] = ptrSinAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] + ptrCosAngle[2] * ptrCosAngle[0]
+        ptrRotationMatrix[1, 2] = ptrSinAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] - ptrCosAngle[2] * ptrSinAngle[0]
+        ptrRotationMatrix[2, 0] = -ptrSinAngle[1]
+        ptrRotationMatrix[2, 1] = ptrCosAngle[1] * ptrSinAngle[0]
+        ptrRotationMatrix[2, 2] = ptrCosAngle[1] * ptrCosAngle[0]
+        return ptrRotationMatrix
+
+    def wait(self):
+        time.sleep(0.5)
+        while(mc.is_moving() == 1):
+            time.sleep(0.2)
+    
+    def camera_open(self):
+        self.camera.capture()  # 打开摄像头
+
+    # 获取物体坐标(相机系)
+    def calc_markers_base_position(self, corners, ids):
+        if len(corners) == 0:
+            return []
+        rvecs, tvecs = solve_marker_pnp(corners, self.marker_size, self.mtx, self.dist)  # 通过二维码角点获取物体旋转向量和平移向量
+        for i, tvec, rvec in zip(ids, tvecs, rvecs):
+            tvec = tvec.squeeze().tolist()
+            rvec = rvec.squeeze().tolist()
+            rotvector = np.array([[rvec[0], rvec[1], rvec[2]]])
+            Rotation = cv2.Rodrigues(rotvector)[0]  # 将旋转向量转为旋转矩阵
+            Euler = self.CvtRotationMatrixToEulerAngle(Rotation)  # 将旋转矩阵转为欧拉角
+            target_coords = np.array([tvec[0], tvec[1], tvec[2], Euler[0], Euler[1], Euler[2]])  # 物体坐标(相机系)
+        return target_coords
+
+    
+    def eyes_in_hand_calculate(self, pose, tbe, Mc, Mr):
+
+        pose,Mr =  map(np.array, [pose,Mr])
+        # 将角度从度数转换为弧度
+        euler = pose * np.pi / 180
+        Rbe = self.CvtEulerAngleToRotationMatrix(euler)
+        Reb = Rbe.T
+
+        A = np.empty((3, 0))
+        b_comb = np.empty((3, 0))
+        
+        r = tbe.shape[0]
+        
+        for i in range(1, r):
+            A = np.hstack((A, (Mc[i, :].reshape(3, 1) - Mc[0, :].reshape(3, 1))))
+            b_comb = np.hstack((b_comb, (tbe[0, :].reshape(3, 1) - tbe[i, :].reshape(3, 1))))
+        
+        b = Reb @ b_comb
+        U, _, Vt = svd(A @ b.T)
+        Rce = Vt.T @ U.T
+        
+        tbe_sum = np.sum(tbe, axis=0)
+        Mc_sum = np.sum(Mc, axis=0)
+        
+        tce = Reb @ (Mr.reshape(3, 1) - (1/r) * tbe_sum.reshape(3, 1) - (1/r) * (Rbe @ Rce @ Mc_sum.reshape(3, 1)))
+        tce[2] -= self.IDENTIFY_LEN #用于保持识别距离
+
+        EyesInHand_matrix = np.vstack((np.hstack((Rce, tce)), np.array([0, 0, 0, 1])))
+        print("EyesInHand_matrix = ", EyesInHand_matrix)
+        return EyesInHand_matrix
+
+    # 读取Camera坐标（单次）
+    def stag_identify(self):
+        self.camera.update_frame()  # 刷新相机界面
+        frame = self.camera.color_frame()  # 获取当前帧
+        (corners, ids, rejected_corners) = stag.detectMarkers(frame, 11)  # 获取画面中二维码的角度和id
+        # 绘制检测到的标记及其ID
+        stag.drawDetectedMarkers(frame, corners, ids)
+        # 绘制被拒绝的候选区域，颜色设为红色
+        stag.drawDetectedMarkers(frame, rejected_corners, border_color=(255, 0, 0))
+        marker_pos_pack = self.calc_markers_base_position(corners, ids)  # 获取物的坐标(相机系)
+        if(len(marker_pos_pack) == 0):
+             marker_pos_pack, ids = self.stag_identify()
+        # print("Camera coords = ", marker_pos_pack)
+        # cv2.imshow("rrrr", frame)
+        # cv2.waitKey(1)
+        return marker_pos_pack, ids
+    
+    def Matrix_identify(self, ml):
+        ml.send_angles(self.origin_mycbot_level, 50)  # 移动到观测点
+        self.wait()
+        input("make sure camera can observe the stag, enter any key quit")
+        coords = ml.get_coords()
+        pose = coords[3:6]
+        print(pose)
+        # self.camera_open_loop()
+        Mc1,tbe1,pos1 = self.reg_get(ml)
+        ml.send_coord(1, coords[0] + 50, 30)
+        self.wait()
+        Mc2,tbe2,pos2 = self.reg_get(ml)
+        ml.send_coord(3, coords[2] + 20, 30)
+        self.wait()
+        Mc3,tbe3,pos3 = self.reg_get(ml)
+        ml.send_coord(2, coords[1] + 20, 30)
+        self.wait()
+        Mc4,tbe4,pos4 = self.reg_get(ml)
+        ml.send_coord(1, coords[0] + 20, 30)
+        self.wait()
+        Mc5,tbe5,pos5 = self.reg_get(ml)
+        tbe = np.vstack([tbe1, tbe2, tbe3, tbe4, tbe5])
+        Mc = np.vstack([Mc1, Mc2, Mc3, Mc4, Mc5])
+        state = None
+        if self.EyesInHand_matrix is not None:
+            state = True
+            pos = np.vstack([pos1, pos2, pos3, pos4, pos5])
+            r = pos.shape[0]
+            for i in range(1, r):
+                for j in range(3):
+                    err = abs(pos[i][j] - pos[0][j])
+                    if(err > 10):
+                        state = False
+                        # print("matrix error")
+        return pose, tbe, Mc, state
+
+    def Eyes_in_hand_calibration(self, ml):
+        ml.set_end_type(0)
+        pose, tbe, Mc, state = self.Matrix_identify(ml)
+        if(state == True):
+            print("Calibration Complete EyesInHand_matrix = ", self.EyesInHand_matrix)
+            return
+        
+        input("Move the end of the robot arm to the calibration point, press any key to release servo")
+        ml.release_all_servos()
+        input("focus servo and get current coords")
+        ml.power_on()
+        time.sleep(1)
+        coords = ml.get_coords()
+        while len(coords) == 0:
+            coords = ml.get_coords()
+        Mr = coords[0:3]
+        print(Mr)
+
+        self.EyesInHand_matrix = self.eyes_in_hand_calculate(pose, tbe, Mc, Mr)
+        print("EyesInHand_matrix = ", self.EyesInHand_matrix)
+        self.save_matrix()  # Save the matrix to a file after calculating it
+        print("save successe, wait to verify")
+        
+        pose, tbe, Mc, state = self.Matrix_identify(ml)
+        if state != True:
+            self.EyesInHand_matrix = self.eyes_in_hand_calculate(pose, tbe, Mc, Mr)
+
+    def Transformation_matrix(self,coord):
+        """坐标转换为齐次变换矩阵
+
+        Args:
+            coord (_type_): (x,y,z,rx,ry,rz)
+
+        Returns:
+            _type_: _description_
+        """
+        position_robot = coord[:3]
+        pose_robot = coord[3:]
+        # 将欧拉角转为旋转矩阵
+        RBT = self.CvtEulerAngleToRotationMatrix(pose_robot)  
+        PBT = np.array([[position_robot[0]],
+                        [position_robot[1]],
+                        [position_robot[2]]])
+        temp = np.concatenate((RBT, PBT), axis=1)
+        array_1x4 = np.array([[0, 0, 0, 1]])
+        # 将两个数组按行拼接起来
+        matrix = np.concatenate((temp, array_1x4), axis=0)  
+        return matrix
+    
+    def Eyes_in_hand(self, coord, camera, Matrix_TC):
+        Position_Camera = np.transpose(camera[:3])  # 相机坐标
+        Matrix_BT = self.Transformation_matrix(coord)  # 机械臂坐标矩阵
+
+        Position_Camera = np.append(Position_Camera, 1)  # 物体坐标（相机系）
+        Position_B = Matrix_BT @ Matrix_TC @ Position_Camera  # 物体坐标（基坐标系）
+        return Position_B
+    
+    def stag_robot_identify(self, ml):
+        marker_pos_pack,ids = self.stag_identify()
+        target_coords = ml.get_coords() # 获取机械臂当前坐标
+        while (target_coords is None):
+            target_coords = ml.get_coords()
+        # print("current_coords", target_coords)
+        cur_coords = np.array(target_coords.copy())
+        cur_coords[-3:] *= (np.pi / 180)  # 将角度值转为弧度值
+        fact_bcl = self.Eyes_in_hand(cur_coords, marker_pos_pack, self.EyesInHand_matrix)  # 通过矩阵变化将物体坐标(相机系)转成(基坐标系)
+        
+        for i in range(3):
+            target_coords[i] = fact_bcl[i]
+        
+        return target_coords,ids
+    
+    def reg_get(self, ml):
+        target_coords = None
+        for i in range(30):
+            Mc_all,_ = self.stag_identify()
+        if self.EyesInHand_matrix is not None:
+            target_coords,_ = self.stag_robot_identify(ml)
+
+        tbe_all = ml.get_coords() # 获取机械臂当前坐标
+        while (tbe_all is None):
+            tbe_all = ml.get_coords()
+
+        tbe = np.array(tbe_all[0:3])
+        Mc = np.array(Mc_all[0:3])
+        print("tbe = ", tbe)
+        print("Mc = ", Mc)
+        return Mc,tbe,target_coords
+
+
+if __name__ == "__main__":
+    if mc.is_power_on()==0:
+        mc.power_on()
+    camera_params = np.load("camera_params.npz")  # 相机配置文件
+    mtx, dist = camera_params["mtx"], camera_params["dist"]
+    m = camera_detect(0, 32, mtx, dist)
+    # tool_len = 20
+    # mc.set_tool_reference([0, 0, tool_len, 0, 0, 0])
+    # mc.set_end_type(0)
+
+    m.Eyes_in_hand_calibration(mc)  #手眼标定
+    

mycobot_280/mycobot_280arduino/camera_calibration/marker_utils.py

@@ -0,0 +1,59 @@
+import cv2
+import numpy as np
+import typing as T
+from numpy.typing import NDArray, ArrayLike
+
+
+class MarkerInfo(T.TypedDict):
+    corners: np.ndarray
+    tvec: np.ndarray
+    rvec: np.ndarray
+    num_id: int
+
+
+def solve_marker_pnp(corners: NDArray, marker_size: int, mtx: NDArray, dist: NDArray):
+    """
+    This will estimate the rvec and tvec for each of the marker corners detected by:
+       corners, ids, rejectedImgPoints = detector.detectMarkers(image)
+    corners - is an array of detected corners for each detected marker in the image
+    marker_size - is the size of the detected markers
+    mtx - is the camera matrix
+    distortion - is the camera distortion matrix
+    RETURN list of rvecs, tvecs, and trash (so that it corresponds to the old estimatePoseSingleMarkers())
+    """
+    marker_points = np.array(
+        [
+            [-marker_size / 2, marker_size / 2, 0],
+            [marker_size / 2, marker_size / 2, 0],
+            [marker_size / 2, -marker_size / 2, 0],
+            [-marker_size / 2, -marker_size / 2, 0],
+        ],
+        dtype=np.float32,
+    )
+    rvecs = []
+    tvecs = []
+    for corner in corners:
+        retval, rvec, tvec = cv2.solvePnP(
+            marker_points,
+            corner,
+            mtx,
+            dist,
+            flags=cv2.SOLVEPNP_IPPE_SQUARE,
+        )
+        if retval:
+            rvecs.append(rvec)
+            tvecs.append(tvec)
+
+    rvecs = np.array(rvecs)  # type: ignore
+    tvecs = np.array(tvecs)  # type: ignore
+    (rvecs - tvecs).any()  # type: ignore
+    return rvecs, tvecs
+
+
+def draw_marker(frame: np.ndarray, corners, tvecs, rvecs, ids, mtx, dist) -> None:
+    # cv2.aruco.drawDetectedMarkers(frame, corners, None, borderColor=(0, 255, 0))
+    cv2.aruco.drawDetectedMarkers(frame, corners, ids, borderColor=(0, 200, 200))
+    for i in range(len(ids)):
+        corner, tvec, rvec, marker_id = corners[i], tvecs[i], rvecs[i], ids[i]
+        cv2.drawFrameAxes(frame, mtx, dist, rvec, tvec, 60, 2)
+

mycobot_280/mycobot_280arduino/camera_calibration/uvc_camera.py

@@ -0,0 +1,56 @@
+import cv2
+import numpy as np
+import time
+import typing
+
+
+class UVCCamera:
+    def __init__(
+        self,
+        cam_index=0,
+        mtx=None,
+        dist=None,
+        capture_size: typing.Tuple[int, int] = (640, 480),
+    ):
+        super().__init__()
+        self.cam_index = cam_index
+        self.mtx = mtx
+        self.dist = dist
+        self.curr_color_frame: typing.Union[np.ndarray, None] = None
+        self.capture_size = capture_size
+
+    def capture(self):
+        # self.cap = cv2.VideoCapture(self.cam_index, cv2.CAP_DSHOW) #windows
+        self.cap = cv2.VideoCapture(self.cam_index) #linux
+        width, height = self.capture_size
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
+
+    def update_frame(self) -> bool:
+        ret, self.curr_color_frame = self.cap.read()
+        return ret
+
+    def color_frame(self) -> typing.Union[np.ndarray, None]:
+        return self.curr_color_frame
+
+    def release(self):
+        self.cap.release()
+
+
+if __name__ == "__main__":
+    cam = UVCCamera(0)
+    cam.capture()
+    while True:
+        if not cam.update_frame():
+            continue
+
+        frame = cam.color_frame()
+        if frame is None:
+            time.sleep(0.01)
+            continue
+
+        print(frame.shape)
+        window_name = "preview"
+        cv2.imshow(window_name, frame)
+        if cv2.waitKey(1) == ord("q"):
+            break

mycobot_280/mycobot_280arduino/config/EyesInHand_matrix.json

@@ -0,0 +1 @@
+[[0.7978586214708513, 0.59877174890628, -0.06995722161683607, -42.33432105525292], [0.6010697464630348, -0.7990332868561791, 0.016154453957772947, -35.01038302335407], [0.04622531807890511, 0.05493813982585287, 0.9974191800647204, 39.342478235028345], [0.0, 0.0, 0.0, 1.0]]

mycobot_280/mycobot_280arduino/config/mycobot_with_marker.rviz

@@ -126,6 +126,7 @@ Visualization Manager:
           Value: true
         joint6_flange:
           Value: true
+      Marker Alpha: 1
       Marker Scale: 0.300000012
       Name: TF
       Show Arrows: true
@@ -145,7 +146,7 @@ Visualization Manager:
       Value: true
     - Class: rviz/Marker
       Enabled: true
-      Marker Topic: /visualization_marker
+      Marker Topic: cube
       Name: Marker
       Namespaces:
         basic_cube: true

mycobot_280/mycobot_280arduino/launch/detect_marker_with_topic.launch

@@ -0,0 +1,28 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+	<!-- Load file model ，加载文件模型-->
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_280_arduino/mycobot_280_arduino.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280arduino)/config/mycobot_with_marker.rviz" />
+	<arg name="gui" default="false" />
+
+	<arg name="num" default="0" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF ，将值合并到TF-->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+	<!-- mycobot-topics ，mycobot-话题-->
+    <include file="$(find mycobot_communication)/launch/communication_topic.launch">
+		<arg name="port" value="$(arg port)" />	
+		<arg name="baud" value="$(arg baud)" />	
+    </include>
+	<!-- listen and pub the real angles ，监听并发布真实的角度-->
+	<node name="real_listener" pkg="mycobot_280arduino" type="listen_real_of_topic.py" />
+	<include file="$(find mycobot_280arduino)/launch/open_camera.launch" />
+
+</launch>

mycobot_280/mycobot_280arduino/launch/open_camera.launch

@@ -0,0 +1,21 @@
+<launch>
+  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+    <!-- //指定设备文件名，默认是/dev/video0 -->
+    <param name="video_device" value="/dev/video0" />
+    <!-- // 宽和高分辨率	 -->
+    <param name="image_width" value="640" />
+    <param name="image_height" value="480" />
+    <!-- // 像素编码，可选值：mjpeg，yuyv，Quyvy -->
+    <param name="pixel_format" value="yuyv" />
+    <param name="color_format" value="yuv422p" />
+    <!-- // camera坐标系名Q -->
+    <param name="camera_frame_id" value="usb_cam" />
+    <!-- // IO通道，可选值：mmap，read，userptr，大数据量信息一般用mmap -->
+    <param name="io_method" value="mmap"/>
+  </node>
+  <!-- <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen"> -->
+  	<!-- // 指定发出的topic名：/usb_cam/image_raw -->
+    <!-- <remap from="image" to="/usb_cam/image_raw"/> -->
+    <!-- <param name="autosize" value="true" /> -->
+  <!-- </node> -->
+</launch>

mycobot_280/mycobot_280arduino/scripts/detect_stag.py

@@ -0,0 +1,445 @@
+#!/usr/bin/env python
+
+import rospy
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+import numpy as np
+from numpy.typing import NDArray, ArrayLike
+import stag
+import os
+import json
+import time
+import threading
+from mycobot_communication.msg import MycobotAngles, MycobotSetAngles, MycobotCoords, MycobotSetCoords, MycobotSetEndType, MycobotSetFreshMode, MycobotSetToolReference, MycobotSetVisionMode
+from visualization_msgs.msg import Marker
+
+np.set_printoptions(suppress=True, formatter={'float_kind': '{:.2f}'.format})
+
+class STAGRecognizer:
+    def __init__(self):
+        rospy.init_node('stag_recognizer', anonymous=True)
+        self.bridge = CvBridge()
+        self.tool_len = 20
+
+        self.marker_size = 32
+        self.origin_mycbot_horizontal = [-90, -35.85, -52.91, 88.59, 0, 0.0]
+
+        self.EyesInHand_matrix = None
+        # 订阅摄像头话题
+        self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback)
+        
+        # 获取config文件目录并设置相机参数文件路径
+        file_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+        params_file_path = os.path.join(file_dir, "config","camera_params.npz")
+        print(params_file_path)
+        matrix_file_path = os.path.join(file_dir, "config","EyesInHand_matrix.json")
+        self.load_matrix(filename=matrix_file_path)
+        print(matrix_file_path)
+        # 加载相机参数
+        try:
+            camera_params = np.load(params_file_path)
+            self.mtx, self.dist = camera_params["mtx"], camera_params["dist"]
+        except FileNotFoundError:
+            rospy.logerr(f"Camera parameters file not found: {params_file_path}")
+            raise
+        
+        self.current_frame = None
+        
+        # 创建发布者，发布机械臂坐标和角度
+        self.coords_pub = rospy.Publisher('mycobot/coords_goal', MycobotSetCoords, queue_size=5)
+        self.angles_pub = rospy.Publisher('mycobot/angles_goal', MycobotSetAngles, queue_size=5)
+        self.fresh_mode_pub = rospy.Publisher('mycobot/fresh_mode_status', MycobotSetFreshMode, queue_size=5)
+        self.end_type_pub = rospy.Publisher('mycobot/end_type_status', MycobotSetEndType, queue_size=5)
+        self.tool_reference_pub = rospy.Publisher('mycobot/tool_reference_goal', MycobotSetToolReference, queue_size=5)
+        self.vision_mode_pub = rospy.Publisher('mycobot/vision_mode_status', MycobotSetVisionMode, queue_size=5)
+        
+        # 创建订阅者，订阅机械臂的真实坐标和角度
+        rospy.Subscriber('mycobot/coords_real', MycobotCoords, self.coords_callback)
+        
+        self.current_coords = None
+        self.current_angles = None
+        self.lock = threading.Lock()
+        self.set_end_type(0)
+        
+        # init a node and a publisher
+        # rospy.init_node("marker", anonymous=True)
+        self.pub = rospy.Publisher('cube', Marker, queue_size=1)
+        # init a Marker
+        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.0
+        self.marker.color.g = 1.0
+        self.marker.color.r = 1.0
+
+        # 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
+        
+    def load_matrix(self, filename="EyesInHand_matrix.json"):
+        # Load the EyesInHand_matrix from a JSON file, if it exists
+        try:
+            with open(filename, 'r') as f:
+                self.EyesInHand_matrix = np.array(json.load(f))
+        except FileNotFoundError:
+            print("Matrix file not found. EyesInHand_matrix will be initialized later.")
+    
+    # publish marker
+    def pub_marker(self, x, y, z=0.03):
+        self.marker.header.stamp = rospy.Time.now()
+        self.marker.pose.position.x = x
+        self.marker.pose.position.y = y
+        self.marker.pose.position.z = z
+        self.marker.color.g = 0
+        self.pub.publish(self.marker)
+    
+    def coords_callback(self, data):
+        # 获取机械臂当前的坐标，并保留小数点后两位
+        with self.lock:
+            self.current_coords = [round(data.x, 2), round(data.y, 2), round(data.z, 2),
+                                round(data.rx, 2), round(data.ry, 2), round(data.rz, 2)]
+            self.coords_updated = True
+            # rospy.loginfo(f"Current coords11111: {self.current_coords}")
+        
+    def send_angles(self, angles, speed):
+        msg = MycobotSetAngles()
+        msg.joint_1, msg.joint_2, msg.joint_3, msg.joint_4, msg.joint_5, msg.joint_6 = angles
+        msg.speed = speed
+        self.angles_pub.publish(msg)
+        
+    def send_coords(self, coords, speed, model):
+        # 创建 MycobotSetCoords 消息对象
+        msg = MycobotSetCoords()
+        
+        # coords 是一个包含 [x, y, z, rx, ry, rz] 的列表
+        if len(coords) != 6:
+            raise ValueError("coords must be a list of 6 elements")
+        msg.x, msg.y, msg.z, msg.rx, msg.ry, msg.rz = coords
+        msg.speed = speed
+        msg.model = model
+
+        # 发布消息
+        self.coords_pub.publish(msg)
+
+        
+    def get_coords(self):
+        with self.lock:
+            if self.coords_updated:
+                self.coords_updated = False
+                return self.current_coords.copy()
+            return []
+        
+    def set_fresh_mode(self, mode):
+        msg = MycobotSetFreshMode()
+        msg.Status = mode
+        self.fresh_mode_pub.publish(msg)
+        
+    def set_vision_mode(self, mode):
+        msg = MycobotSetVisionMode()
+        msg.Status = mode
+        self.vision_mode_pub.publish(msg)
+        
+    def set_end_type(self, end_type):
+        msg = MycobotSetEndType()
+        msg.Status = end_type
+        self.end_type_pub.publish(msg)
+
+    def set_tool_reference(self, coords):
+        msg = MycobotSetToolReference()
+        msg.x, msg.y, msg.z, msg.rx, msg.ry, msg.rz = coords
+        self.tool_reference_pub.publish(msg)
+
+
+    def solve_marker_pnp(self, corners: NDArray, marker_size: int, mtx: NDArray, dist: NDArray):
+        """
+        This will estimate the rvec and tvec for each of the marker corners detected by:
+        corners, ids, rejectedImgPoints = detector.detectMarkers(image)
+        corners - is an array of detected corners for each detected marker in the image
+        marker_size - is the size of the detected markers
+        mtx - is the camera matrix
+        distortion - is the camera distortion matrix
+        RETURN list of rvecs, tvecs, and trash (so that it corresponds to the old estimatePoseSingleMarkers())
+        """
+        marker_points = np.array(
+            [
+                [-marker_size / 2, marker_size / 2, 0],
+                [marker_size / 2, marker_size / 2, 0],
+                [marker_size / 2, -marker_size / 2, 0],
+                [-marker_size / 2, -marker_size / 2, 0],
+            ],
+            dtype=np.float32,
+        )
+        rvecs = []
+        tvecs = []
+        for corner in corners:
+            retval, rvec, tvec = cv2.solvePnP(
+                marker_points,
+                corner,
+                mtx,
+                dist,
+                flags=cv2.SOLVEPNP_IPPE_SQUARE,
+            )
+            if retval:
+                rvecs.append(rvec)
+                tvecs.append(tvec)
+
+        rvecs = np.array(rvecs)  # type: ignore
+        tvecs = np.array(tvecs)  # type: ignore
+        (rvecs - tvecs).any()  # type: ignore
+        return rvecs, tvecs
+
+    def image_callback(self, data):
+        try:
+            # 将 ROS 图像消息转换为 OpenCV 格式
+            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+            self.current_frame = cv_image
+        except CvBridgeError as e:
+            rospy.logerr(e)
+            return
+        
+        # 应用相机校正
+        # frame_undistorted = cv2.undistort(cv_image, self.mtx, self.dist, None, self.mtx)
+
+        # 检测 STAG 标记
+        corners, ids, rejected_corners = stag.detectMarkers(cv_image, 11)
+        # 绘制检测到的标记及其ID
+        stag.drawDetectedMarkers(cv_image, corners, ids)
+        # 绘制被拒绝的候选区域，颜色设为红色
+        stag.drawDetectedMarkers(cv_image, rejected_corners, border_color=(255, 0, 0))
+        
+        # cv2.imshow("STAG Detection", cv_image)
+        # cv2.waitKey(1)
+        
+    def calc_markers_base_position(self, corners, ids):
+        """获取物体坐标(相机系)
+
+        Args:
+            corners (_type_): _description_
+            ids (_type_): _description_
+
+        Returns:
+            _type_: _description_
+        """
+        if len(corners) == 0:
+            return []
+        # 通过二维码角点获取物体旋转向量和平移向量
+        rvecs, tvecs = self.solve_marker_pnp(corners, self.marker_size, self.mtx, self.dist)
+        for i, tvec, rvec in zip(ids, tvecs, rvecs):
+            tvec = tvec.squeeze().tolist()
+            rvec = rvec.squeeze().tolist()
+            rotvector = np.array([[rvec[0], rvec[1], rvec[2]]])
+            # 将旋转向量转为旋转矩阵
+            Rotation = cv2.Rodrigues(rotvector)[0]  
+            # 将旋转矩阵转为欧拉角
+            Euler = self.CvtRotationMatrixToEulerAngle(Rotation)  
+            # 物体坐标(相机系)
+            target_coords = np.array([tvec[0], tvec[1], tvec[2], Euler[0], Euler[1], Euler[2]])
+        return target_coords
+    
+    def CvtRotationMatrixToEulerAngle(self, pdtRotationMatrix):
+        """将旋转矩阵转为欧拉角
+
+        Args:
+            pdtRotationMatrix (_type_): _description_
+
+        Returns:
+            _type_: _description_
+        """
+        pdtEulerAngle = np.zeros(3)
+        pdtEulerAngle[2] = np.arctan2(pdtRotationMatrix[1, 0], pdtRotationMatrix[0, 0])
+        fCosRoll = np.cos(pdtEulerAngle[2])
+        fSinRoll = np.sin(pdtEulerAngle[2])
+        pdtEulerAngle[1] = np.arctan2(-pdtRotationMatrix[2, 0],
+                                      (fCosRoll * pdtRotationMatrix[0, 0]) + (fSinRoll * pdtRotationMatrix[1, 0]))
+        pdtEulerAngle[0] = np.arctan2((fSinRoll * pdtRotationMatrix[0, 2]) - (fCosRoll * pdtRotationMatrix[1, 2]),
+                                      (-fSinRoll * pdtRotationMatrix[0, 1]) + (fCosRoll * pdtRotationMatrix[1, 1]))
+        return pdtEulerAngle
+    
+    def CvtEulerAngleToRotationMatrix(self, ptrEulerAngle):
+        """将欧拉角转为旋转矩阵
+
+        Args:
+            ptrEulerAngle (_type_): _description_
+
+        Returns:
+            _type_: _description_
+        """
+        ptrSinAngle = np.sin(ptrEulerAngle)
+        ptrCosAngle = np.cos(ptrEulerAngle)
+        ptrRotationMatrix = np.zeros((3, 3))
+        ptrRotationMatrix[0, 0] = ptrCosAngle[2] * ptrCosAngle[1]
+        ptrRotationMatrix[0, 1] = ptrCosAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] - ptrSinAngle[2] * ptrCosAngle[0]
+        ptrRotationMatrix[0, 2] = ptrCosAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] + ptrSinAngle[2] * ptrSinAngle[0]
+        ptrRotationMatrix[1, 0] = ptrSinAngle[2] * ptrCosAngle[1]
+        ptrRotationMatrix[1, 1] = ptrSinAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] + ptrCosAngle[2] * ptrCosAngle[0]
+        ptrRotationMatrix[1, 2] = ptrSinAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] - ptrCosAngle[2] * ptrSinAngle[0]
+        ptrRotationMatrix[2, 0] = -ptrSinAngle[1]
+        ptrRotationMatrix[2, 1] = ptrCosAngle[1] * ptrSinAngle[0]
+        ptrRotationMatrix[2, 2] = ptrCosAngle[1] * ptrCosAngle[0]
+        return ptrRotationMatrix
+    
+    def Transformation_matrix(self,coord):
+        """坐标转换为齐次变换矩阵
+
+        Args:
+            coord (_type_): (x,y,z,rx,ry,rz)
+
+        Returns:
+            _type_: _description_
+        """
+        position_robot = coord[:3]
+        pose_robot = coord[3:]
+        # 将欧拉角转为旋转矩阵
+        RBT = self.CvtEulerAngleToRotationMatrix(pose_robot)  
+        PBT = np.array([[position_robot[0]],
+                        [position_robot[1]],
+                        [position_robot[2]]])
+        temp = np.concatenate((RBT, PBT), axis=1)
+        array_1x4 = np.array([[0, 0, 0, 1]])
+        # 将两个数组按行拼接起来
+        matrix = np.concatenate((temp, array_1x4), axis=0)  
+        return matrix
+
+    def Eyes_in_hand(self, coord, marker_positions, Matrix_TC):
+        # 相机坐标
+        Position_Camera = np.transpose(marker_positions[:3])
+        # 机械臂坐标矩阵
+        Matrix_BT = self.Transformation_matrix(coord)
+        # 物体坐标（相机系）
+        Position_Camera = np.append(Position_Camera, 1)
+
+        # 物体坐标（基坐标系）
+        Position_B = Matrix_BT @ Matrix_TC @ Position_Camera
+        return Position_B
+    
+    def waitl(self, ml):
+        """等待机械臂运行结束
+
+        Args:
+            ml (_type_): _description_
+        """
+        time.sleep(0.2)
+        while ml.is_moving():
+            time.sleep(0.03)
+    
+    def stag_identify(self):
+        """读取Camera坐标（单次）
+
+        Returns:
+            _type_: _description_
+        """
+        try:
+            if self.current_frame is None:
+                rospy.logwarn("No image received yet")
+                return []
+            # 获取当前帧
+            frame = self.current_frame
+            # 获取画面中二维码的角度和id
+            (corners, ids, rejected_corners) = stag.detectMarkers(frame, 11)
+            # 获取物的坐标(相机系)
+            marker_pos_pack = self.calc_markers_base_position(corners, ids)
+            if len(marker_pos_pack) == 0 and not rospy.is_shutdown():
+                # rospy.logwarn("No markers detected")
+                marker_pos_pack, ids = self.stag_identify()  # 递归调用
+
+            # print("Camera coords = ", marker_pos_pack)
+            return marker_pos_pack, ids
+        except RecursionError:
+            # rospy.logerr("Recursion depth exceeded in marker detection")
+            return [0, 0, 0, 0], 0  # 返回默认值
+    
+    def vision_trace(self, mode, ml):
+        sp = 40
+        #水平面抓取
+        if mode == 0:
+            # 移动到观测点
+            ml.send_angles(self.origin_mycbot_horizontal, sp)
+            # 等待机械臂运动结束
+            self.waitl(ml)
+            input("Enter any key to start trace")
+
+            target_coords = self.stag_robot_identify(ml)
+            print(target_coords)
+
+            time.sleep(1)
+            # 机械臂移动到二维码前方
+            ml.send_coords(target_coords, 30)
+            # 等待机械臂运动结束
+            self.waitl(ml)
+            
+    def stag_robot_identify(self):
+        marker_pos_pack, ids = self.stag_identify()
+        # 如果返回的是默认值，直接退出函数，不返回任何数据
+        # if marker_pos_pack == [0, 0, 0, 0]:
+        if np.array_equal(marker_pos_pack, [0, 0, 0, 0]):
+            rospy.logwarn("No markers detected, skipping processing")
+            return None  # 直接返回 None
+        target_coords = self.get_coords()
+        while target_coords is None or len(target_coords) != 6:
+            target_coords = self.get_coords()
+        # print("Current coords:", target_coords)
+        cur_coords = np.array(target_coords.copy())
+        cur_coords[-3:] *= (np.pi / 180)
+        fact_bcl = self.Eyes_in_hand(cur_coords, marker_pos_pack, self.EyesInHand_matrix)
+        
+        for i in range(3):
+            target_coords[i] = fact_bcl[i]
+        return target_coords, ids
+    
+    def coord_limit(self, coords):
+        min_coord = [-350, -350, 300]
+        max_coord = [350, 350, 500]
+        for i in range(3):
+            if(coords[i] < min_coord[i]):
+                coords[i] = min_coord[i]
+
+            if(coords[i] > max_coord[i]):
+                coords[i] = max_coord[i]
+    
+    def vision_trace_loop(self):
+        self.set_fresh_mode(1)
+        time.sleep(1)
+        # self.set_vision_mode(0)
+        # 移动到观测点
+        self.send_angles(self.origin_mycbot_horizontal, 50)  
+        time.sleep(3)
+        origin = self.get_coords()
+        while origin is None:
+            origin = self.get_coords()
+        rate = rospy.Rate(30)
+        while not rospy.is_shutdown():
+            _ ,ids = self.stag_identify()
+            if ids[0] == 0:
+                target_coords,_ = self.stag_robot_identify()
+                # 如果没有返回目标坐标，跳过本次循环
+                self.coord_limit(target_coords)
+                rospy.loginfo('Target Coords: %s', target_coords)
+                for i in range(3):
+                    target_coords[i+3] = origin[i+3]
+                self.pub_marker(target_coords[0]/1000.0, target_coords[1]/1000.0, target_coords[2]/1000.0)
+                self.send_coords(target_coords, 30, 0)  # 机械臂移动到二维码前方
+                rate.sleep()
+            elif ids[0] == 1:
+                self.send_angles(self.origin_mycbot_horizontal, 50) 
+    
+    
+if __name__ == '__main__': 
+    try:
+        sr = STAGRecognizer()
+        sr.vision_trace_loop()
+        rospy.spin()
+    except KeyboardInterrupt:
+        rospy.loginfo("Shutting down...")
+        sr.set_vision_mode(2)
+        cv2.destroyAllWindows()

mycobot_communication/CMakeLists.txt

@@ -59,7 +59,10 @@ add_message_files(FILES
   MycobotSetEndType.msg
   MycobotSetFreshMode.msg
   MycobotSetToolReference.msg
+  MycobotSetVisionMode.msg
+  
   MycobotGetGripperValue.msg
+
 )
 
 ## Generate services in the 'srv' folder
@@ -73,6 +76,7 @@ add_service_files(FILES
   SetEndType.srv
   SetFreshMode.srv
   SetToolReference.srv
+  SetVisionMode.srv
 )
 
 ## Generate added messages and services

mycobot_communication/msg/MycobotSetVisionMode.msg

@@ -0,0 +1 @@
+uint8 Status

mycobot_communication/scripts/mycobot_topics.py

@@ -5,6 +5,7 @@ import os
 import sys
 import signal
 import threading
+import traceback
 
 import rospy
 
@@ -18,6 +19,7 @@ from mycobot_communication.msg import (
     MycobotSetEndType,
     MycobotSetFreshMode,
     MycobotSetToolReference,
+    MycobotSetVisionMode,
     MycobotGetGripperValue,
 )
 from std_msgs.msg import UInt8
@@ -109,6 +111,7 @@ class MycobotTopics(object):
         rospy.loginfo("%s,%s" % (port, baud))
         self.mc = MyCobot280(port, baud)
         self.lock = threading.Lock()
+        self.mc.set_vision_mode(1)
         self.output_robot_message()
 
     def start(self):
@@ -122,6 +125,8 @@ class MycobotTopics(object):
         sfm = threading.Thread(target=self.sub_fresh_mode_status)
         set = threading.Thread(target=self.sub_end_type_status)
         str = threading.Thread(target=self.sub_set_tool_reference)
+        svm = threading.Thread(target=self.sub_vision_mode_status)
+        
         sgv = threading.Thread(target=self.sub_real_gripper_value)
 
         pa.setDaemon(True)
@@ -143,6 +148,9 @@ class MycobotTopics(object):
         set.start()
         str.setDaemon(True)
         str.start()
+        svm.setDaemon(True)
+        svm.start
+        
         sgv.setDaemon(True)
         sgv.start()
 
@@ -156,6 +164,7 @@ class MycobotTopics(object):
         sfm.join()
         set.join()
         str.join()
+        svm.join()
         sgv.join()
 
     def pub_real_angles(self):
@@ -176,7 +185,10 @@ class MycobotTopics(object):
                         ma.joint_5 = angles[4]
                         ma.joint_6 = angles[5]
                         pub.publish(ma)
+                    else:
+                        rospy.logwarn("None or -1")
                 except Exception as e:
+                    e = traceback.format_exc()
                     rospy.logerr(f"SerialException: {e}")
             time.sleep(0.25)
 
@@ -199,7 +211,10 @@ class MycobotTopics(object):
                         ma.ry = coords[4]
                         ma.rz = coords[5]
                         pub.publish(ma)
+                    else:
+                        rospy.logwarn("None or -1")
                 except Exception as e:
+                    e = traceback.format_exc()
                     rospy.logerr(f"SerialException: {e}")
             time.sleep(0.25)
 
@@ -296,6 +311,22 @@ class MycobotTopics(object):
             "mycobot/fresh_mode_status", MycobotSetFreshMode, callback=callback
         )
         rospy.spin()
+    
+    def sub_vision_mode_status(self):
+        """Subscribe to vision mode Status"""
+        """订阅运动模式状态"""
+        def callback(data):
+            if data.Status==1:
+                self.mc.set_vision_mode(1)
+            elif data.Status==2:
+                self.mc.stop()
+            else:
+                self.mc.set_vision_mode(0)
+
+        sub = rospy.Subscriber(
+            "mycobot/vision_mode_status", MycobotSetVisionMode, callback=callback
+        )
+        rospy.spin()
         
     def sub_end_type_status(self):
         """Subscribe to end type Status"""

mycobot_communication/srv/SetVisionMode.srv

@@ -0,0 +1,5 @@
+uint8 Status
+
+---
+
+bool Flag