HiveCoreRD/hivecore_robot_os
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加检测标定板的测试节点，修改手眼标定节点

Browse Source
This commit is contained in:
liangyuxuan
2025-08-22 10:34:01 +08:00
parent e7ce19ab8a
commit 7a0091ca59
3 changed files with 342 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

189
HiveCoreR0/src/robot_vision/detect_part/detect_part/detect.py
View File

@@ -38,13 +38,13 @@ def calculate_coordinate(mask, depth_img: np.ndarray, camera_info, u, v):
u = int(round(u))
v = int(round(v))
_cx, _cy, _fx, _fy = camera_info
_cx, _cy, _fx, _fy = camera_info[2], camera_info[5], camera_info[0], camera_info[4]
if not (0 <= v < depth_img.shape[0] and 0 <= u < depth_img.shape[1]):
print(f'Calculate coordinate error: u={u}, v={v}')
return 0.0, 0.0, 0.0
window_size = 25
window_size = 15
half = window_size // 2
"""获取mask质心patch"""
@@ -86,8 +86,6 @@ def calculate_pose(mask, orig_shape, depth_img: np.ndarray, camera_info):
else:
raise ValueError("Mask Is Empty")
# center = (box[0]+box[1]+box[2]+box[3]) / 4
mask = cv2.resize(mask.astype(np.uint8), orig_shape[::-1], interpolation=cv2.INTER_LINEAR)
x, y, z = calculate_coordinate(mask, depth_img, camera_info, center_x, center_y)
if len1 >= len2:
@@ -116,25 +114,41 @@ def draw_box(set_confidence, rgb_img, result, post_list):
class_ids = result.boxes.cls.cpu().numpy()
labels = result.names
for i, box in enumerate(boxes):
j = 0
if confidences[i] < set_confidence:
continue
else:
if len(post_list):
for i, box in enumerate(boxes):
j = 0
if confidences[i] < set_confidence:
continue
else:
x_center, y_center, width, height = box[:4]
x_center, y_center, width, height = box[:4]
p1 = [int((x_center - width / 2)), int((y_center - height / 2))]
p2 = [int((x_center + width / 2)), int((y_center + height / 2))]
cv2.rectangle(rgb_img, p1, p2, (255, 255, 0), 2)
p1 = [int((x_center - width / 2)), int((y_center - height / 2))]
p2 = [int((x_center + width / 2)), int((y_center + height / 2))]
cv2.rectangle(rgb_img, p1, p2, (255, 255, 0), 2)
cv2.putText(rgb_img, f'{labels[class_ids[i]]}: {confidences[i]*100:.2f}', (p1[0], p1[1] - 10),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
cv2.putText(rgb_img, f'{labels[class_ids[i]]}: {confidences[i]*100:.2f}', (p1[0], p1[1] - 10),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
cv2.putText(rgb_img, f'cs: {post_list[j][0]:.3f} {post_list[j][1]:.3f} {post_list[j][2]:.3f}', (p1[0], p1[1] - 35)
,cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
cv2.putText(rgb_img, f'rpy: {post_list[j][3]:.3f} {post_list[j][4]:.3f} {post_list[j][5]:.3f}', (p1[0], p1[1] - 60)
,cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
j += 1
cv2.putText(rgb_img, f'cs: {post_list[j][0]:.3f} {post_list[j][1]:.3f} {post_list[j][2]:.3f}', (p1[0], p1[1] - 35)
,cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
cv2.putText(rgb_img, f'rpy: {post_list[j][3]:.3f} {post_list[j][4]:.3f} {post_list[j][5]:.3f}', (p1[0], p1[1] - 60)
,cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
j += 1
else:
for i, box in enumerate(boxes):
j = 0
if confidences[i] < set_confidence:
continue
else:
x_center, y_center, width, height = box[:4]
p1 = [int((x_center - width / 2)), int((y_center - height / 2))]
p2 = [int((x_center + width / 2)), int((y_center + height / 2))]
cv2.rectangle(rgb_img, p1, p2, (255, 255, 0), 2)
cv2.putText(rgb_img, f'{labels[class_ids[i]]}: {confidences[i] * 100:.2f}', (p1[0], p1[1] - 10),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
j += 1
def draw_mask(set_confidence, rgb_img, result):
@@ -151,8 +165,18 @@ def draw_mask(set_confidence, rgb_img, result):
rgb_img[mask > 0] = rgb_img[mask > 0] * 0.5 + np.array([0, 0, 255]) * 0.5
def distortion_correction(color_image, depth_image, map1, map2):
undistorted_color = cv2.remap(color_image, map1, map2, cv2.INTER_LINEAR)
undistorted_color = undistorted_color.astype(color_image.dtype)
undistorted_depth = cv2.remap(depth_image, map1, map2, cv2.INTER_NEAREST)
undistorted_depth = undistorted_depth.astype(depth_image.dtype)
return undistorted_color, undistorted_depth
class DetectNode(Node):
def __init__(self, name):
def __init__(self, name, mode):
super().__init__(name)
self.device = None
self.checkpoint_path = None
@@ -160,17 +184,27 @@ class DetectNode(Node):
self.function = None
self.output_boxes = None
self.output_masks = None
self.K = self.D = self.R = self.P = None
self.map1 = self.map2 = None
self.mode = mode
self.camera_info = []
self.camera_size = []
self.cv_bridge = CvBridge()
'''init'''
self._init_param()
self._init_model()
if mode == 'detect':
self._init_model()
elif mode == 'test':
self.function = self._test_image
else:
self.function = None
self.get_logger().error('Error: Mode Unknown')
self._init_publisher()
self._init_subscriber()
self.cv_bridge = CvBridge()
def _init_param(self):
"""init parameter"""
@@ -234,44 +268,64 @@ class DetectNode(Node):
def _camera_info_callback(self, msg: CameraInfo):
"""Get camera info"""
self.K = msg.k
self.D = msg.d
self.R = msg.r
self.P = msg.p
self.camera_size.append(msg.width)
self.camera_size.append(msg.height)
self.camera_info.append(msg.k[2])
self.camera_info.append(msg.k[5])
self.camera_info.append(msg.k[0])
self.camera_info.append(msg.k[4])
self.destroy_subscription(self.sub_camera_info)
self.map1, self.map2, self.K = self._get_map(msg.k)
# self.get_logger().info(f'K: {self.K}')
# self.get_logger().info(f'D: {self.D}')
def _get_map(self, K):
h, w = self.camera_size[::-1]
K = np.array(K).reshape(3, 3)
D = np.array(self.D)
new_K, _ = cv2.getOptimalNewCameraMatrix(K, D, (w, h), 1, (w, h))
map1, map2 = cv2.initUndistortRectifyMap(K, D, None, new_K, (w, h), cv2.CV_32FC1)
return map1, map2, new_K.flatten()
def _sync_callback(self, color_img_ros, depth_img_ros):
"""同步回调函数"""
if not self.camera_info:
if self.K is None:
self.get_logger().warn('Camera intrinsics not yet received. Waiting...')
return
color_img_cv = self.cv_bridge.imgmsg_to_cv2(color_img_ros, "bgr8")
depth_img_cv = self.cv_bridge.imgmsg_to_cv2(depth_img_ros, '16UC1')
img, pose_dict = self.function(color_img_cv, depth_img_cv)
if img is not None:
self.pub_detect_image.publish(img)
color_img_cv_new, depth_img_cv_new = distortion_correction(color_img_cv, depth_img_cv, self.map1, self.map2)
# diff = cv2.absdiff(color_img_cv, color_img_cv_new)
# cv2.imshow('diff', diff)
# cv2.waitKey(0)
img, pose_dict = self.function(color_img_cv_new, depth_img_cv_new)
"""masks为空，结束这一帧"""
if img is None and pose_dict is None:
return
img = self.cv_bridge.cv2_to_imgmsg(color_img_cv_new, "bgr8")
pose_list_all = PoseWithIDArray()
for (class_id, class_name), pose_list in pose_dict.items():
pose_list_all.objects.append(
PoseWithID(
class_name = class_name,
class_id = class_id,
pose_list = pose_list
if img is not None and (self.output_boxes or self.output_masks):
self.pub_detect_image.publish(img)
if pose_dict is not None:
pose_list_all = PoseWithIDArray()
for (class_id, class_name), pose_list in pose_dict.items():
pose_list_all.objects.append(
PoseWithID(
class_name = class_name,
class_id = class_id,
pose_list = pose_list
)
)
)
self.pub_pose_list.publish(pose_list_all)
self.pub_pose_list.publish(pose_list_all)
def _seg_image(self, rgb_img: np.ndarray, depth_img: np.ndarray):
"""Use segmentation model"""
@@ -310,8 +364,7 @@ class DetectNode(Node):
and border_y <= y_center <= rgb_img.shape[:2][0] - border_y):
continue
x, y, z, roll, pitch, yaw = calculate_pose(mask, orig_shape, depth_img, self.camera_info)
x, y, z, roll, pitch, yaw = calculate_pose(mask, orig_shape, depth_img, self.K)
if (x, y, z) != (0.0, 0.0, 0.0):
pose = Pose()
pose.position = Point(x=x, y=y, z=z)
@@ -333,10 +386,52 @@ class DetectNode(Node):
else:
return None, pose_dict
def _test_image(self, rgb_img, depth_img):
rgb_img_gray = cv2.cvtColor(rgb_img, cv2.COLOR_BGR2GRAY)
pattern_size = (11, 8)
ret, corners = cv2.findChessboardCorners(rgb_img_gray, pattern_size)
if ret:
# 角点亚像素精确化（提高标定精度）
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners_subpix = cv2.cornerSubPix(rgb_img_gray, corners, (11, 11), (-1, -1), criteria)
corners_subpix = corners_subpix.reshape(pattern_size[1], pattern_size[0], 2)
mask = np.zeros(rgb_img_gray.shape, dtype=np.uint8)
for i in [0, pattern_size[1] - 1]:
for j in [0, pattern_size[0] - 1]:
pts = np.array([
corners_subpix[i, j],
corners_subpix[i, j + 1],
corners_subpix[i + 1, j + 1],
corners_subpix[i + 1, j]
], dtype=np.int32)
cv2.fillConvexPoly(mask, pts, 1)
rgb_img[mask > 0] = rgb_img[mask > 0] * 0.5 + np.array([0, 0, 255]) * 0.5
orig_shape = rgb_img_gray.shape
x, y, z, roll, pitch, yaw = calculate_pose(mask, orig_shape, depth_img, self.K)
cv2.putText(rgb_img, f'cs: x: {x:.2f}, y: {y:.2f}, z: {z:.2f}', (0, 0 + 15),cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
return self.cv_bridge.cv2_to_imgmsg(rgb_img, "bgr8"), None
else:
return self.cv_bridge.cv2_to_imgmsg(rgb_img, "bgr8"), None
def main(args=None):
rclpy.init(args=args)
node = DetectNode('detect')
node = DetectNode('detect', 'detect')
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
def test_node(args=None):
rclpy.init(args=args)
node = DetectNode('detect', 'test')
try:
rclpy.spin(node)
except KeyboardInterrupt:

251
HiveCoreR0/src/robot_vision/detect_part/detect_part/hand_eye_calibration.py
View File

@@ -3,6 +3,7 @@
import transforms3d as tfs
import numpy as np
import math
import json
import rclpy
from rclpy.node import Node
@@ -10,19 +11,55 @@ from geometry_msgs.msg import Pose
from message_filters import ApproximateTimeSynchronizer, Subscriber
def get_matrix_eular_radu(x, y, z, rx, ry, rz):
""""""
def get_matrix_quat(x, y, z, rx, ry, rz, rw):
"""从单位四元数构建齐次变换矩阵"""
'''构造旋转矩阵'''
rmat = tfs.euler.euler2mat(
rx, ry, rz,
)
q = [rw, rx, ry, rz]
rmat = tfs.quaternions.quat2mat(q)
"""构造齐次变换矩阵"""
rmat = tfs.affines.compose(
np.squeeze(np.asarray((x, y, z))),
rmat,
[1, 1, 1]
)
return rmat
def get_matrix_eular_radu(x, y, z, rx, ry, rz):
"""从欧拉角构建齐次变换矩阵"""
'''构造旋转矩阵'''
rmat = tfs.euler.euler2mat(
math.radians(rx), math.radians(ry), math.radians(rz)
# rx, ry, rz
)
"""构造齐次变换矩阵"""
rmat = tfs.affines.compose(
np.squeeze(np.asarray((x, y, z))),
rmat,
[1, 1, 1]
)
return rmat
def get_matrix_rotvector(x, y, z, rx, ry, rz):
"""从旋转向量构建齐次变换矩阵"""
'''构造旋转矩阵'''
rvec = np.array([rx, ry, rz])
theta = np.linalg.norm(rvec)
if theta < 1e-8:
rmat = np.eye(3) # 小角度直接返回单位矩阵
else:
axis = rvec / theta
rmat = tfs.axangles.axangle2mat(axis, theta)
"""构造齐次变换矩阵"""
rmat = tfs.affines.compose(
np.squeeze(np.asarray((x, y, z))),
rmat,
[1, 1, 1]
)
return rmat
@@ -32,17 +69,20 @@ def skew(v):
[-v[1], v[0], 0]])
def rot2quat_minimal(m):
"""旋转矩阵 --> 最小四元数"""
quat = tfs.quaternions.mat2quat(m[0:3, 0:3])
return quat[1:]
def R2P(R):
"""旋转矩阵 --> 修正罗德里格斯向量"""
axis, angle= tfs.axangles.mat2axangle(R[0:3, 0:3])
P = 2 * math.sin(angle / 2) * axis
return P
def quat_minimal2rot(q):
"""最小四元数 --> 旋转矩阵"""
p = np.dot(q.T, q)
w = np.sqrt(np.subtract(1, p[0][0]))
return tfs.quaternions.quat2mat([w, q[0], q[1], q[2]])
def P2R(P):
"""修正罗德里格斯向量 --> 旋转矩阵"""
P2 = np.dot(P.T, P)
part_1 = (1 - P2 / 2) * np.eye(3)
part_2 = np.add(np.dot(P, P.T), np.sqrt(4- P2) * skew(P.flatten().T))
R = np.add(part_1, np.divide(part_2, 2))
return R
def calculate(Hgs, Hcs):
@@ -58,22 +98,22 @@ def calculate(Hgs, Hcs):
size += 1
Hgij = np.dot(np.linalg.inv(Hgs[j]), Hgs[i])
Hgijs.append(Hgij)
Pgij = np.dot(2, rot2quat_minimal(Hgij))
Pgij = np.dot(2, R2P(Hgij))
Hcij = np.dot(Hcs[j], np.linalg.inv(Hcs[i]))
Hcijs.append(Hcij)
Pcij = np.dot(2, rot2quat_minimal(Hcij))
Pcij = np.dot(2, R2P(Hcij))
A.append(skew(np.add(Pgij, Pcij)))
B.append(np.subtract(Pcij, Pgij))
B.append(np.subtract(Pcij, Pgij).reshape(3, 1))
MA = np.asarray(A).reshape(size * 3, 3)
MB = np.asarray(B).reshape(size * 3, 1)
MA = np.vstack(A)
MB = np.vstack(B)
Pcg_ = np.dot(np.linalg.pinv(MA), MB)
# pcg_norm = np.dot(np.conjugate(Pcg_).T, Pcg_)
pcg = np.sqrt(np.add(1, np.dot(Pcg_.T, Pcg_)))
pcg = np.dot(np.dot(2, Pcg_), np.linalg.inv(pcg))
Rcg = quat_minimal2rot(np.divide(pcg, 2)).reshape(3, 3)
Pcg = np.dot(np.dot(2, Pcg_), np.linalg.inv(pcg))
Rcg = P2R(Pcg).reshape(3, 3)
A = []
B = []
@@ -103,18 +143,31 @@ class Calibration(Node):
self.declare_parameter('start_collect_once', False)
self.collect = self.get_parameter('start_collect_once').value
self.declare_parameter('start_calculate', False)
self.calculate = self.get_parameter('start_calculate').value
self.declare_parameter('mode', 'eye_in_hand')
self.mode = self.get_parameter('mode').value.lower()
if self.mode not in ['eye_in_hand', 'eye_to_hand']:
raise ValueError("mode must be 'eye_in_hand' or 'eye_to_hand'")
self.declare_parameter('input', 'rotvertor')
self.input = self.get_parameter('input').value.lower()
if self.input == 'rotvertor':
self.function = get_matrix_rotvector
elif self.input == 'eular':
self.function = get_matrix_eular_radu
elif self.input == 'quat':
self.function = get_matrix_quat
else:
raise ValueError("INPUT ERROR")
self.done = False
self._init_sub()
def _init_sub(self):
self.sub_hand_pose = Subscriber(self, Pose, '/calibration/camera/pose')
self.sub_camera_pose = Subscriber(self, Pose, '/calibration/hand/pose')
self.sub_hand_pose = Subscriber(self, Pose, '/calibration/hand/pose')
self.sub_camera_pose = Subscriber(self, Pose, '/calibration/camera/pose')
self.sync_subscriber = ApproximateTimeSynchronizer(
[self.sub_hand_pose, self.sub_camera_pose],
@@ -145,7 +198,7 @@ class Calibration(Node):
self.camera.extend(_camera)
self.collect = False
if len(self.hand) >= 25 * 6 and len(self.camera) >= 25 * 6:
if self.calculate:
self.sync_subscriber.registerCallback(None)
self.sub_hand_pose = None
self.sub_camera_pose = None
@@ -157,26 +210,49 @@ class Calibration(Node):
self.done = True
def calculate_data(self):
for i in range(0, len(self.hand), 6):
self.Hgs.append(
np.linalg.inv(
get_matrix_eular_radu(
if self.input == 'quat':
for i in range(0, len(self.hand), 7):
self.Hgs.append(
np.linalg.inv(
self.function(
self.hand[i], self.hand[i + 1], self.hand[i + 2],
self.hand[i + 3], self.hand[i + 4], self.hand[i + 5], self.hand[i + 6]
)
)
if self.mode == 'eye_to_hand' else
self.function(
self.hand[i], self.hand[i + 1], self.hand[i + 2],
self.hand[i + 3], self.hand[i + 4], self.hand[i + 5], self.hand[i + 6]
)
)
self.Hcs.append(
self.function(
self.camera[i], self.camera[i + 1], self.camera[i + 2],
self.camera[i + 3], self.camera[i + 4], self.camera[i + 5], self.camera[i + 6]
)
)
else:
for i in range(0, len(self.hand), 6):
self.Hgs.append(
np.linalg.inv(
self.function(
self.hand[i], self.hand[i + 1], self.hand[i + 2],
self.hand[i + 3], self.hand[i + 4], self.hand[i + 5]
)
)
if self.mode == 'eye_to_hand' else
self.function(
self.hand[i], self.hand[i + 1], self.hand[i + 2],
self.hand[i + 3], self.hand[i + 4], self.hand[i + 5]
)
)
if self.mode == 'eye_to_hand' else
get_matrix_eular_radu(
self.hand[i], self.hand[i + 1], self.hand[i + 2],
self.hand[i + 3], self.hand[i + 4], self.hand[i + 5]
self.Hcs.append(
self.function(
self.camera[i], self.camera[i + 1], self.camera[i + 2],
self.camera[i + 3], self.camera[i + 4], self.camera[i + 5]
)
)
)
self.Hcs.append(
get_matrix_eular_radu(
self.camera[i], self.camera[i + 1], self.camera[i + 2],
self.camera[i + 3], self.camera[i + 4], self.camera[i + 5]
)
)
def calculate_calibration(self):
self.calculate_data()
@@ -186,6 +262,13 @@ class Calibration(Node):
self.hand, self.camera = hand, camera
self.calculate_calibration()
print(self.calibration_matrix)
hand_eye_result = {
"T": self.calibration_matrix.tolist()
}
with open("calibration_matrix.json", "w") as f:
json.dump(hand_eye_result, f, indent=4)
self.done = True
@@ -203,18 +286,82 @@ def main(args=None):
if __name__ == '__main__':
hand = [1.1988093940033604, -0.42405585264804424, 0.18828251788562061, math.radians(151.3390418721659), math.radians(-18.612399542280507),
math.radians(153.05074895025035),
1.1684831621733476, -0.183273375514656, 0.12744868246620855, math.radians(-161.57083804238462), math.radians(9.07159838346732),
math.radians(89.1641128844487),
1.1508343174145468, -0.22694301453461405, 0.26625166858469146, math.radians(177.8815855486261), math.radians(0.8991159570568988),
math.radians(77.67286224959672)]
camera = [-0.16249272227287292, -0.047310635447502136, 0.4077761471271515, math.radians(-56.98037030812389), math.radians(-6.16739631361851),
math.radians(-115.84333735802369),
0.03955405578017235, -0.013497642241418362, 0.33975949883461, math.radians(-100.87129330834215), math.radians(-17.192685528625265),
math.radians(-173.07354634882094),
-0.08517949283123016, 0.00957852229475975, 0.46546608209609985, math.radians(-90.85270962096058), math.radians(0.9315977976503153),
math.radians(175.2059707654342)]
# hand = [
# 1.1988093940033604, -0.42405585264804424, 0.18828251788562061, 151.3390418721659, -18.612399542280507,
# 153.05074895025035,
# 1.1684831621733476, -0.183273375514656, 0.12744868246620855, -161.57083804238462, 9.07159838346732,
# 89.1641128844487,
# 1.1508343174145468, -0.22694301453461405, 0.26625166858469146, 177.8815855486261, 0.8991159570568988,
# 77.67286224959672
# ]
# camera = [
# -0.16249272227287292, -0.047310635447502136, 0.4077761471271515, -56.98037030812389, -6.16739631361851,
# -115.84333735802369,
# 0.03955405578017235, -0.013497642241418362, 0.33975949883461, -100.87129330834215, -17.192685528625265,
# -173.07354634882094,
# -0.08517949283123016, 0.00957852229475975, 0.46546608209609985, -90.85270962096058, 0.9315977976503153,
# 175.2059707654342
# ]
hand = [
0.400422, -0.0262847, 0.365594, 3.12734, -0.0857978, -0.0168582,
0.423347, -0.16857, 0.340184, -2.73844, 0.089013, 0.123284,
0.42540, 0.19543, 0.29062, 2.44351, -0.1777, -0.0722,
0.58088, -0.0541, 0.32633, -2.9303, 0.06957, 0.98985,
0.2760, -0.033, 0.3301, 3.0813, 0.0724, 0.6077,
0.54011, 0.09071, 0.34623, 2.62279, 0.75876, -0.6927,
0.57732, -0.1346, 0.37990, -2.6764, 1.04868, 0.82177,
0.27844, -0.1371, 0.28799, -0.8233, 2.26319, 0.36110
]
camera = [
0.11725, 0.05736, 0.32176, 0.02860, -0.1078, 3.06934,
0.07959, 0.06930, 0.29829, 0.66126, -0.2127, 2.99042,
0.14434, 0.03227, 0.40377, -0.9465, -0.1744, 2.80668,
0.11008, 0.05605, 0.35730, 0.12422, -1.0665, 2.87604,
0.1131, 0.0438171, 0.299624, -0.058278, -0.514154, -3.06309,
0.057039, 0.109464, 0.415275, 0.414777, 0.750109, -2.49495,
0.13702, 0.00520, 0.38190, 0.26431, -0.7554, 2.26885,
- 0.03670, -0.04433, 0.237292, 0.807501, -0.27347, 0.614594
]
# hand = [
# 0.00036945715011716274, -0.8839821223981196, 0.5523193840441242,
# 177.27956424635218, 0.23589536602025146, 107.82169371956397,
# 0.11896749459717415, -0.8718906380613968, 0.5523143801701358,
# -157.77906314717302, -8.024173057288115, 106.21079406179818,
# 0.1189648210427768, -0.814346693785594, 0.5523175381728943,
# 168.53772599122402, 1.9258975408714558, 108.31277860632433,
# ]
# camera = [
# -0.02678103931248188, 0.016525425016880035, 0.4080054759979248,
# -92.63997644065388, 0.2218862615062928, 114.13933860507045,
# 0.027912182733416557, 0.0497259683907032, 0.4265890121459961,
# -66.88363156672801, -1.4961574658157981, 113.78281426313359,
# -0.21827486157417297, 0.03443644195795059, 0.37940269708633423,
# -101.57441674818881, 3.3466880152822167, 114.46663884305094,
# ]
# hand = [
# 0.4285232296, 0.1977596461, -0.5595823047, 0.4738016082, 0.3209333657, -0.2268795901, -0.7880605703,
# 0.2265712272, 0.0261943502, -0.6661910850, 0.5222343809, 0.0461668455, -0.1315038186, -0.8413362107,
# 0.1058885008, 0.0527681997, -0.7419267756, 0.5081638565, -0.0263721340, -0.1696570449, -0.8439730402,
# 0.1127224767, -0.0420359377, -0.7413071786, 0.5101706595, -0.0322252464, -0.1864402870, -0.8390038445,
# 0.2592932751, -0.0329068529, -0.7162865014, 0.5101641882, 0.1265739325, -0.0153077050, -0.8505746380,
# -0.1724239544, 0.0084144761, -0.6998332592, 0.4989369998, -0.0973210400, -0.1244194561, -0.8521210503,
# -0.0534271258, 0.0771779706, -0.6845820453, 0.5195499916, 0.0511217081, -0.1691595167, -0.8359661686,
# 0.2598500029, 0.0933230213, -0.6638257382, 0.5673912325, 0.1686973705, -0.1427337629, -0.7932436318,
# ]
# camera = [
# 0.4277459616, -0.0267754900, 0.0822384718, 0.3555198802, -0.2313832954, -0.6738796808, -0.6049409568,
# 0.4610891450, -0.0089776615, 0.0394863697, -0.4168581229, 0.4389318994, 0.4953132086, 0.6230833961,
# 0.4159190432, 0.0112235849, 0.0112504715, -0.3559197420, 0.4898053987, 0.4542453721, 0.6535081871,
# 0.4143974465, 0.0094036803, 0.1073298638, -0.3438949365, 0.4875942762, 0.4507613906, 0.6639294113,
# 0.3473267442, -0.1866610227, 0.0651366494, 0.4966101920, -0.4291949558, -0.5361047392, -0.5308123169,
# 0.3883165305, -0.2249779584, 0.0093256602, -0.3595692001, 0.5529429756, 0.4090559739, 0.6305848604,
# 0.4309364801, -0.2354261800, -0.0925819097, -0.3831460128, 0.4244707870, 0.5043032275, 0.6470718186,
# 0.4362292324, -0.0778107597, -0.1007970399, 0.4568889439, -0.3144470665, -0.5302104578, -0.6412896427,
# ]
rclpy.init(args=None)
node = Calibration('Calibration')

1
HiveCoreR0/src/robot_vision/detect_part/setup.py
View File

@@ -28,6 +28,7 @@ setup(
entry_points={
'console_scripts': [
'detect_node = detect_part.detect:main',
'test_node = detect_part.detect:test_node',
'sub_pose_node = detect_part.sub_pose:main',
'calibration_node = detect_part.hand_eye_calibration:main',
],