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

1

Browse Source
This commit is contained in:
liangyuxuan
2025-12-23 15:13:01 +08:00
parent eec02de5a7
commit d62faaa9cc
2 changed files with 40 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
vision_detect/vision_detect/VisionDetect/node/config_node.py
View File

@@ -7,8 +7,6 @@ from rclpy.node import Node
import numpy as np
import open3d as o3d
from ..utils import read_calibration_mat
share_dir = get_package_share_directory('vision_detect')
@@ -82,7 +80,7 @@ class ConfigBase(Node):
self.detect_mode = self.configs['detect_mode']
self.calculate_mode = self.configs['calculate_mode']
self._read_calibration_mat()
# self._read_calibration_mat()
if self.get_camera_mode == "Service":
self.service_name = self.configs["Service_configs"]["service_name"]
@@ -133,22 +131,22 @@ class ConfigBase(Node):
self.get_logger().info("Get parameters done")
def _read_calibration_mat(self):
eye_in_left_hand_path = os.path.join(share_dir, self.configs["calibration"]["left_hand"])
eye_in_right_hand_path = os.path.join(share_dir, self.configs["calibration"]["right_hand"])
eye_to_hand_path = os.path.join(share_dir, self.configs["calibration"]["head"])
# def _read_calibration_mat(self):
# eye_in_left_hand_path = os.path.join(share_dir, self.configs["calibration"]["left_hand"])
# eye_in_right_hand_path = os.path.join(share_dir, self.configs["calibration"]["right_hand"])
# eye_to_hand_path = os.path.join(share_dir, self.configs["calibration"]["head"])
self.eye_in_left_hand_mat, info, sign = read_calibration_mat(eye_in_left_hand_path)
if not sign:
self.get_logger().warning(f"left_mat: {info}")
# self.eye_in_left_hand_mat, info, sign = read_calibration_mat(eye_in_left_hand_path)
# if not sign:
# self.get_logger().warning(f"left_mat: {info}")
self.eye_in_right_hand_mat, info, sign = read_calibration_mat(eye_in_right_hand_path)
if not sign:
self.get_logger().warning(f"right_mat: {info}")
# self.eye_in_right_hand_mat, info, sign = read_calibration_mat(eye_in_right_hand_path)
# if not sign:
# self.get_logger().warning(f"right_mat: {info}")
self.eye_to_hand_mat, info, sign = read_calibration_mat(eye_to_hand_path)
if not sign:
self.get_logger().warning(f"head_mat: {info}")
# self.eye_to_hand_mat, info, sign = read_calibration_mat(eye_to_hand_path)
# if not sign:
# self.get_logger().warning(f"head_mat: {info}")
self.get_logger().info("Read calibration mat file done")
# self.get_logger().info("Read calibration mat file done")

26
vision_detect/vision_detect/VisionDetect/node/init_node.py
View File

@@ -1,3 +1,4 @@
import os
from ament_index_python.packages import get_package_share_directory
import torch
@@ -12,7 +13,7 @@ from sensor_msgs.msg import Image, CameraInfo
from interfaces.msg import PoseArrayClassAndID, ImgMsg
from interfaces.srv import VisionObjectRecognition
from ..utils import calculate_pose_pca, calculate_pose_icp
from ..utils import calculate_pose_pca, calculate_pose_icp, read_calibration_mat
from .config_node import ConfigBase
share_dir = get_package_share_directory('vision_detect')
@@ -21,6 +22,7 @@ class InitBase(ConfigBase):
def __init__(self, name):
super().__init__(name)
self.future = Future()
self._read_calibration_mat()
if self.get_camera_mode == "Service":
self._init_service()
@@ -145,6 +147,28 @@ class InitBase(ConfigBase):
self.get_logger().info("Initialize subscriber done")
def _read_calibration_mat(self):
eye_in_left_hand_path = os.path.join(share_dir, self.configs["calibration"]["left_hand"])
eye_in_right_hand_path = os.path.join(share_dir, self.configs["calibration"]["right_hand"])
eye_to_hand_path = os.path.join(share_dir, self.configs["calibration"]["head"])
self.eye_in_left_hand_mat, info, sign = read_calibration_mat(eye_in_left_hand_path)
self.get_logger().info(f"left_hand_mat: {self.eye_in_left_hand_mat}")
if not sign:
self.get_logger().warning(f"left_mat: {info}")
self.eye_in_right_hand_mat, info, sign = read_calibration_mat(eye_in_right_hand_path)
self.get_logger().info(f"right_hand_mat: {self.eye_in_right_hand_mat}")
if not sign:
self.get_logger().warning(f"right_mat: {info}")
self.eye_to_hand_mat, info, sign = read_calibration_mat(eye_to_hand_path)
self.get_logger().info(f"head_mat: {self.eye_to_hand_mat}")
if not sign:
self.get_logger().warning(f"head_mat: {info}")
self.get_logger().info("Read calibration mat file done")
def _camera_info_callback(self, msg: CameraInfo):
pass