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

简化配置方式，修正部分错误

Browse Source
This commit is contained in:
liangyuxuan
2025-12-04 16:19:40 +08:00
parent f433d4428f
commit 927f3c728c
11 changed files with 114 additions and 318 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
vision_detect/configs/launch_configs/bottle_detect_service_icp.json
View File

@@ -2,28 +2,31 @@
"node_name": "bottle_detect_service",
"output_boxes": "True",
"output_masks": "False",
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json",
"calibration": {
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json"
},
"get_camera_mode": "Service",
"Service_configs": {
"service_name": "/vision_object_recognition"
},
"detect_mode": "Detect",
"Detect_configs": {
"checkpoint_path": "checkpoints/yolo11s-seg.pt",
"confidence": 0.50,
"classes": [39]
},
"calculate_mode": "PCA",
"ICP_configs": {
"complete_model_path": "pointclouds/bottle_model.pcd",
"depth_scale": 1000.0,
"depth_trunc": 2.0,
"nb_points": 10,
"radius": 0.1,
"nb_neighbors": 20,
"std_ratio": 3.0,
"voxel_size": 0.010,
"ransac_voxel_size": 0.005,
"icp_voxel_radius": [0.004, 0.002, 0.001],
"icp_max_iter": [50, 30, 14]

12
vision_detect/configs/launch_configs/bottle_detect_service_pca.json
View File

@@ -2,19 +2,25 @@
"node_name": "bottle_detect_service",
"output_boxes": "True",
"output_masks": "False",
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json",
"calibration": {
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json"
},
"get_camera_mode": "Service",
"Service_configs": {
"service_name": "/vision_object_recognition"
},
"detect_mode": "Detect",
"Detect_configs": {
"checkpoint_path": "checkpoints/yolo11s-seg.pt",
"confidence": 0.50,
"classes": [39]
},
"calculate_mode": "PCA",
"PCA_configs": {
"depth_scale": 1000.0,

12
vision_detect/configs/launch_configs/crossboard_topic_pca.json
View File

@@ -2,9 +2,13 @@
"node_name": "crossboard_topic",
"output_boxes": "True",
"output_masks": "False",
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json",
"calibration": {
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json"
},
"get_camera_mode": "Topic",
"Topic_configs": {
"position": "right",
@@ -12,10 +16,12 @@
"depth_image_topic_name": "/camera/depth/image_raw",
"camera_info_topic_name": "/camera/color/camera_info"
},
"detect_mode": "Crossboard",
"Crossboard_configs": {
"pattern_size": [8, 5]
},
"calculate_mode": "PCA",
"PCA_configs": {
"depth_scale": 1000.0,

17
vision_detect/configs/launch_configs/default_config.json
View File

@@ -2,9 +2,13 @@
"node_name": "default_config_detect_service",
"output_boxes": "True",
"output_masks": "False",
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json",
"calibration": {
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json"
},
"get_camera_mode": "Service",
"Service_configs": {
"service_name": "/vision_object_recognition"
@@ -15,6 +19,7 @@
"depth_image_topic_name": "/camera/depth/image_raw",
"camera_info_topic_name": "/camera/color/camera_info"
},
"detect_mode": "Detect",
"Detect_configs": {
"checkpoint_path": "checkpoints/yolo11s-seg.pt",
@@ -28,6 +33,7 @@
"Crossboard_configs": {
"pattern_size": [8, 5]
},
"calculate_mode": "PCA",
"PCA_configs": {
"depth_scale": 1000.0,
@@ -38,10 +44,7 @@
"complete_model_path": "pointclouds/bottle_model.pcd",
"depth_scale": 1000.0,
"depth_trunc": 2.0,
"nb_points": 10,
"radius": 0.1,
"nb_neighbors": 20,
"std_ratio": 3.0,
"voxel_size": 0.010,
"ransac_voxel_size": 0.005,
"icp_voxel_radius": [0.004, 0.002, 0.001],
"icp_max_iter": [50, 30, 14]

12
vision_detect/configs/launch_configs/detect_service_pca.json
View File

@@ -2,19 +2,25 @@
"node_name": "detect_service",
"output_boxes": "True",
"output_masks": "False",
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json",
"calibration": {
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_hand": "configs/hand_eye_mat/eye_in_right_hand.json",
"head": "configs/hand_eye_mat/eye_to_hand.json"
},
"get_camera_mode": "Service",
"Service_configs": {
"service_name": "/vision_object_recognition"
},
"detect_mode": "Detect",
"Detect_configs": {
"checkpoint_path": "checkpoints/yolo11s-seg.pt",
"confidence": 0.50,
"classes": []
},
"calculate_mode": "PCA",
"PCA_configs": {
"depth_scale": 1000.0,

42
vision_detect/launch/bottle_detect_service_icp.launch.py
View File

@@ -15,35 +15,11 @@ with open(config_dir, "r") as f:
def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('left_hand', default_value=configs['left_hand']),
DeclareLaunchArgument('right_hand', default_value=configs['right_hand']),
DeclareLaunchArgument('head', default_value=configs['eye_to_handhead_mat_path']),
DeclareLaunchArgument('get_camera_mode', default_value=configs['get_camera_mode']),
DeclareLaunchArgument('detect_mode', default_value=configs['detect_mode']),
DeclareLaunchArgument('calculate_mode', default_value=configs['calculate_mode']),
DeclareLaunchArgument("Service_configs", default_value=json.dumps(configs['Service_configs'])),
DeclareLaunchArgument("Detect_configs", default_value=json.dumps(configs['Detect_configs'])),
DeclareLaunchArgument("ICP_configs", default_value=json.dumps(configs['ICP_configs'])),
DeclareLaunchArgument('configs_path', default_value=config_dir),
]
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
left_hand = LaunchConfiguration('left_hand').perform(context)
right_hand = LaunchConfiguration('right_hand').perform(context)
head = LaunchConfiguration('head').perform(context)
get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
detect_mode = LaunchConfiguration('detect_mode').perform(context)
calculate_mode = LaunchConfiguration('calculate_mode').perform(context)
Service_configs = LaunchConfiguration('Service_configs').perform(context)
Detect_configs = LaunchConfiguration('Detect_configs').perform(context)
ICP_configs = LaunchConfiguration('ICP_configs').perform(context)
configs_path = LaunchConfiguration('configs_path').perform(context)
return [
Node(
@@ -51,19 +27,7 @@ def generate_launch_description():
executable='detect_node',
name=configs['node_name'],
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'left_hand': left_hand,
'right_hand': right_hand,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,
'calculate_mode': calculate_mode,
'Service_configs': Service_configs,
'Detect_configs': Detect_configs,
'ICP_configs': ICP_configs,
'configs_path': configs_path,
}]
)
]

42
vision_detect/launch/bottle_detect_service_pca.launch.py
View File

@@ -15,35 +15,11 @@ with open(config_dir, "r") as f:
def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('left_hand', default_value=configs['left_hand']),
DeclareLaunchArgument('right_hand', default_value=configs['right_hand']),
DeclareLaunchArgument('head', default_value=configs['head']),
DeclareLaunchArgument('get_camera_mode', default_value=configs['get_camera_mode']),
DeclareLaunchArgument('detect_mode', default_value=configs['detect_mode']),
DeclareLaunchArgument('calculate_mode', default_value=configs['calculate_mode']),
DeclareLaunchArgument("Service_configs", default_value=json.dumps(configs['Service_configs'])),
DeclareLaunchArgument("Detect_configs", default_value=json.dumps(configs['Detect_configs'])),
DeclareLaunchArgument("PCA_configs", default_value=json.dumps(configs['PCA_configs'])),
DeclareLaunchArgument('configs_path', default_value=config_dir),
]
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
left_hand = LaunchConfiguration('left_hand').perform(context)
right_hand = LaunchConfiguration('right_hand').perform(context)
head = LaunchConfiguration('head').perform(context)
get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
detect_mode = LaunchConfiguration('detect_mode').perform(context)
calculate_mode = LaunchConfiguration('calculate_mode').perform(context)
Service_configs = LaunchConfiguration('Service_configs').perform(context)
Detect_configs = LaunchConfiguration('Detect_configs').perform(context)
PCA_configs = LaunchConfiguration('PCA_configs').perform(context)
configs_path = LaunchConfiguration('configs_path').perform(context)
return [
Node(
@@ -51,19 +27,7 @@ def generate_launch_description():
executable='detect_node',
name=configs['node_name'],
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'left_hand': left_hand,
'right_hand': right_hand,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,
'calculate_mode': calculate_mode,
'Service_configs': Service_configs,
'Detect_configs': Detect_configs,
'PCA_configs': PCA_configs,
'configs_path': configs_path,
}]
)
]

43
vision_detect/launch/crossboard_topic_pca.launch.py
View File

@@ -15,36 +15,11 @@ with open(config_dir, "r") as f:
def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('left_hand', default_value=configs['left_hand']),
DeclareLaunchArgument('right_hand', default_value=configs['right_hand']),
DeclareLaunchArgument('head', default_value=configs['head']),
DeclareLaunchArgument('get_camera_mode', default_value=configs['get_camera_mode']),
DeclareLaunchArgument('detect_mode', default_value=configs['detect_mode']),
DeclareLaunchArgument('calculate_mode', default_value=configs['calculate_mode']),
DeclareLaunchArgument("Topic_configs", default_value=json.dumps(configs['Topic_configs'])),
DeclareLaunchArgument("Crossboard_configs", default_value=json.dumps(configs['Crossboard_configs'])),
DeclareLaunchArgument("PCA_configs", default_value=json.dumps(configs['PCA_configs'])),
DeclareLaunchArgument('configs_path', default_value=config_dir),
]
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
left_hand = LaunchConfiguration('left_hand').perform(context)
right_hand = LaunchConfiguration('right_hand').perform(context)
head = LaunchConfiguration('head').perform(context)
get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
detect_mode = LaunchConfiguration('detect_mode').perform(context)
calculate_mode = LaunchConfiguration('calculate_mode').perform(context)
Topic_configs = LaunchConfiguration('Topic_configs').perform(context)
Crossboard_configs = LaunchConfiguration('Crossboard_configs').perform(context)
PCA_configs = LaunchConfiguration('PCA_configs').perform(context)
configs_path = LaunchConfiguration('configs_path').perform(context)
return [
Node(
@@ -52,19 +27,7 @@ def generate_launch_description():
executable='detect_node',
name=configs['node_name'],
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'left_hand': left_hand,
'right_hand': right_hand,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,
'calculate_mode': calculate_mode,
'Topic_configs': Topic_configs,
'Crossboard_configs': Crossboard_configs,
'PCA_configs': PCA_configs,
'configs_path': configs_path,
}]
)
]

42
vision_detect/launch/detect_service_pca.launch.py
View File

@@ -15,35 +15,11 @@ with open(config_dir, "r") as f:
def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('left_hand', default_value=configs['left_hand']),
DeclareLaunchArgument('right_hand', default_value=configs['right_hand']),
DeclareLaunchArgument('head', default_value=configs['head']),
DeclareLaunchArgument('get_camera_mode', default_value=configs['get_camera_mode']),
DeclareLaunchArgument('detect_mode', default_value=configs['detect_mode']),
DeclareLaunchArgument('calculate_mode', default_value=configs['calculate_mode']),
DeclareLaunchArgument("Service_configs", default_value=json.dumps(configs['Service_configs'])),
DeclareLaunchArgument("Detect_configs", default_value=json.dumps(configs['Detect_configs'])),
DeclareLaunchArgument("PCA_configs", default_value=json.dumps(configs['PCA_configs'])),
DeclareLaunchArgument('configs_path', default_value=config_dir),
]
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
left_hand = LaunchConfiguration('left_hand').perform(context)
right_hand = LaunchConfiguration('right_hand').perform(context)
head = LaunchConfiguration('head').perform(context)
get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
detect_mode = LaunchConfiguration('detect_mode').perform(context)
calculate_mode = LaunchConfiguration('calculate_mode').perform(context)
Service_configs = LaunchConfiguration('Service_configs').perform(context)
Detect_configs = LaunchConfiguration('Detect_configs').perform(context)
PCA_configs = LaunchConfiguration('PCA_configs').perform(context)
configs_path = LaunchConfiguration('configs_path').perform(context)
return [
Node(
@@ -51,19 +27,7 @@ def generate_launch_description():
executable='detect_node',
name=configs['node_name'],
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'left_hand': left_hand,
'right_hand': right_hand,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,
'calculate_mode': calculate_mode,
'Service_configs': Service_configs,
'Detect_configs': Detect_configs,
'PCA_configs': PCA_configs,
'configs_path': configs_path,
}]
)
]

83
vision_detect/launch/example_configs.launch.py
View File

@@ -13,74 +13,14 @@ config_dir = os.path.join(share_dir, 'configs/launch_configs/default_config.json
with open(config_dir, "r") as f:
configs = json.load(f)
# def get_param_configs():
# param_configs = {}
# if configs["get_camera_mode"] == "Service":
# param_configs.update(configs["Service_configs"])
# elif configs["get_camera_mode"] == "Topic":
# param_configs.update(configs["Topic_configs"])
# else:
# param_configs.update(configs["Service_configs"])
#
# if configs["detect_mode"] == "Detect":
# param_configs.update(configs["Detect_configs"])
# elif configs["detect_mode"] == "Color":
# param_configs.update(configs["Color_configs"])
# elif configs["detect_mode"] == "Crossboard":
# param_configs.update(configs["Crossboard_configs"])
# else:
# param_configs.update(configs["Detect_configs"])
#
# if configs["calculate_mode"] == "PCA":
# param_configs.update(configs["PCA_configs"])
# elif configs["calculate_mode"] == "ICP":
# param_configs.update(configs["ICP_configs"])
# else:
# param_configs.update(configs["PCA_configs"])
# return param_configs
def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('left_hand', default_value=configs['left_hand']),
DeclareLaunchArgument('right_hand', default_value=configs['right_hand']),
DeclareLaunchArgument('head', default_value=configs['head']),
DeclareLaunchArgument('get_camera_mode', default_value=configs['get_camera_mode']),
DeclareLaunchArgument('detect_mode', default_value=configs['detect_mode']),
DeclareLaunchArgument('calculate_mode', default_value=configs['calculate_mode']),
# DeclareLaunchArgument("param_configs", default_value=json.dumps(get_param_configs())),
DeclareLaunchArgument("Service_configs", default_value=json.dumps(configs['Service_configs'])),
DeclareLaunchArgument("Topic_configs", default_value=json.dumps(configs['Topic_configs'])),
DeclareLaunchArgument("Detect_configs", default_value=json.dumps(configs['Detect_configs'])),
DeclareLaunchArgument("Color_configs", default_value=json.dumps(configs['Color_configs'])),
DeclareLaunchArgument("Crossboard_configs", default_value=json.dumps(configs['Crossboard_configs'])),
DeclareLaunchArgument("PCA_configs", default_value=json.dumps(configs['PCA_configs'])),
DeclareLaunchArgument("ICP_configs", default_value=json.dumps(configs['ICP_configs'])),
DeclareLaunchArgument('configs_path', default_value=config_dir),
]
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
left_hand = LaunchConfiguration('left_hand').perform(context)
right_hand = LaunchConfiguration('right_hand').perform(context)
head = LaunchConfiguration('head').perform(context)
get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
detect_mode = LaunchConfiguration('detect_mode').perform(context)
calculate_mode = LaunchConfiguration('calculate_mode').perform(context)
# param_configs = LaunchConfiguration('param_configs').perform(context)
Service_configs = LaunchConfiguration('Service_configs').perform(context)
Topic_configs = LaunchConfiguration('Topic_configs').perform(context)
Detect_configs = LaunchConfiguration('Detect_configs').perform(context)
Color_configs = LaunchConfiguration('Color_configs').perform(context)
Crossboard_configs = LaunchConfiguration('Crossboard_configs').perform(context)
PCA_configs = LaunchConfiguration('PCA_configs').perform(context)
ICP_configs = LaunchConfiguration('ICP_configs').perform(context)
configs_path = LaunchConfiguration('configs_path').perform(context)
return [
Node(
@@ -88,24 +28,7 @@ def generate_launch_description():
executable='detect_node',
name=configs['node_name'],
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'left_hand': left_hand,
'right_hand': right_hand,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,
'calculate_mode': calculate_mode,
# 'param_configs': param_configs,
'Service_configs': Service_configs,
'Topic_configs': Topic_configs,
'Detect_configs': Detect_configs,
'Color_configs': Color_configs,
'Crossboard_configs': Crossboard_configs,
'PCA_configs': PCA_configs,
'ICP_configs': ICP_configs,
'configs_path': configs_path,
}]
)
]

110
vision_detect/vision_detect/VisionDetect/node/detect_node.py
View File

@@ -32,6 +32,8 @@ class DetectNode(Node):
def __init__(self, name):
super().__init__(name)
self.device = None
self.configs = None
self.calculate_configs = None
self.checkpoint_path = None
self.output_boxes = None
self.output_masks = None
@@ -81,44 +83,32 @@ class DetectNode(Node):
def _init_param(self):
"""init parameter"""
self.declare_parameter('output_boxes', True)
self.declare_parameter('output_masks', False)
self.declare_parameter(
'configs_path',
os.path.join(share_dir, "configs/launch_configs/default_config.json")
)
configs_path = self.get_parameter('configs_path').value
with open(configs_path, 'r') as f:
self.configs = json.load(f)
self.output_boxes = self.get_parameter('output_boxes').value
self.output_masks = self.get_parameter('output_masks').value
self.output_boxes = self.configs['output_boxes'].lower() == "true"
self.output_masks = self.configs['output_masks'].lower() == "true"
self.declare_parameter('get_camera_mode', 'service')
self.declare_parameter('detect_mode', 'detect')
self.declare_parameter('calculate_mode', 'pca')
self.get_camera_mode = self.get_parameter('get_camera_mode').value
self.detect_mode = self.get_parameter('detect_mode').value
self.calculate_mode = self.get_parameter("calculate_mode").value
self.declare_parameter('left_hand', 'configs/hand_eye_mat/hand_in_left_hand.json')
self.declare_parameter('right_hand', 'configs/hand_eye_mat/hand_in_right_hand.json')
self.declare_parameter('head', 'configs/hand_eye_mat/hand_to_hand.json')
self.declare_parameter('Service_configs', '{}')
self.declare_parameter('Topic_configs', '{}')
self.declare_parameter('Detect_configs', '{}')
self.declare_parameter('Color_configs', '{}')
self.declare_parameter('Crossboard_configs', '{}')
self.declare_parameter('PCA_configs', '{}')
self.declare_parameter('ICA_configs', '{}')
self.get_camera_mode = self.configs['get_camera_mode']
self.detect_mode = self.configs['detect_mode']
self.calculate_mode = self.configs['calculate_mode']
self._init_json_file()
if self.get_camera_mode == "Service":
self.service_name = json.loads(self.get_parameter('Service_configs').value)["service_name"]
self.service_name = self.configs["Service_configs"]["service_name"]
self._init_service()
elif self.get_camera_mode == "Topic":
self.declare_parameter('Topic_configs', '{}')
self.color_image_topic_name = json.loads(self.get_parameter('Topic_configs').value)["color_image_topic_name"]
self.depth_image_topic_name = json.loads(self.get_parameter('Topic_configs').value)["depth_image_topic_name"]
self.camera_info_topic_name = json.loads(self.get_parameter('Topic_configs').value)["camera_info_topic_name"]
self.position = json.loads(self.get_parameter('Topic_configs').value)["position"]
topic_configs = self.configs['Topic_configs']
self.color_image_topic_name = topic_configs["color_image_topic_name"]
self.depth_image_topic_name = topic_configs["depth_image_topic_name"]
self.camera_info_topic_name = topic_configs["camera_info_topic_name"]
self.position = topic_configs["position"]
if self.position == 'left':
self.hand_eye_mat = self.eye_in_left_hand_mat
elif self.position == 'right':
@@ -126,61 +116,58 @@ class DetectNode(Node):
else:
self.hand_eye_mat = self.eye_to_hand_mat
else:
self.service_name = json.loads(self.get_parameter('Service_configs').value)["service_name"]
self.service_name = self.configs["Service"]["service_name"]
if self.detect_mode == 'Detect':
self.function = self._seg_image
self.confidence = json.loads(self.get_parameter('Detect_configs').value)["confidence"]
self.classes = json.loads(self.get_parameter('Detect_configs').value)["classes"]
detect_configs = self.configs['Detect_configs']
self.confidence = detect_configs["confidence"]
self.classes = detect_configs["classes"]
if not self.classes:
self.classes = None
self.checkpoint_path = os.path.join(share_dir, json.loads(self.get_parameter('Detect_configs').value)["checkpoint_path"])
self.checkpoint_path = os.path.join(share_dir, detect_configs["checkpoint_path"])
self._init_model()
elif self.detect_mode == 'Color':
self.function = self._seg_color
self.color_range = json.loads(self.get_parameter('Color_configs').value)["color_range"]
self.color_range = self.configs["Color_configs"]["color_range"]
self.distance = self.configs["Color_configs"]["distance"]
self.color_range = [[np.array(lower), np.array(upper)] for lower, upper in self.color_range]
elif self.detect_mode == 'Crossboard':
self.function = self._seg_crossboard
self.pattern_size = json.loads(self.get_parameter('Crossboard_configs').value)["pattern_size"]
self.pattern_size = self.configs["Crossboard_configs"]["pattern_size"]
else:
self.get_logger().warning("Unknown mode, use detect")
self.function = self._seg_image
self.confidence = json.loads(self.get_parameter('Detect_configs').value)["confidence"]
self.classes = json.loads(self.get_parameter('Detect_configs').value)["classes"]
detect_configs = self.configs['Detect_configs']
self.confidence = detect_configs["confidence"]
self.classes = detect_configs["classes"]
if not self.classes:
self.classes = None
self.checkpoint_path = json.loads(self.get_parameter('Detect_configs').value)["checkpoint_path"]
self.checkpoint_path = detect_configs["checkpoint_path"]
self._init_model()
if self.calculate_mode == "PCA":
self.configs = json.loads(self.get_parameter('PCA_configs').value)
self.calculate_configs = self.configs['PCA_configs']
self.calculate_function = calculate_pose_pca
elif self.calculate_mode == "ICP":
self.configs = json.loads(self.get_parameter('ICA_configs').value)
self.calculate_configs = self.configs['ICA_configs']
source = o3d.io.read_point_cloud(
os.path.join(share_dir, self.configs['complete_model_path'])
os.path.join(share_dir, self.calculate_configs['complete_model_path'])
)
self.configs["source"] = source
self.calculate_configs["source"] = source
self.calculate_function = calculate_pose_icp
else:
self.get_logger().warning("Unknown calculate_mode, use PCA")
self.configs = json.loads(self.get_parameter('PCA_configs').value)
self.calculate_configs = self.configs['PCA_configs']
self.calculate_function = calculate_pose_pca
self.get_logger().info("Initialize parameters done")
def _init_json_file(self):
self.eye_in_left_hand_path = os.path.join(
share_dir, self.get_parameter('left_hand').value
)
self.eye_in_right_hand_path = os.path.join(
share_dir, self.get_parameter('right_hand').value
)
self.eye_to_hand_path = os.path.join(
share_dir, self.get_parameter('head').value
)
self.eye_in_left_hand_path = os.path.join(share_dir, self.configs["calibration"]["left_hand"])
self.eye_in_right_hand_path = os.path.join(share_dir, self.configs["calibration"]["right_hand"])
self.eye_to_hand_path = os.path.join(share_dir, self.configs["calibration"]["head"])
self.eye_in_left_hand_mat, info, sign = read_calibration_mat(self.eye_in_left_hand_path)
if not sign:
@@ -450,7 +437,7 @@ class DetectNode(Node):
mask_crop,
depth_crop,
intrinsics,
**self.configs
**self.calculate_configs
)
if rmat is None:
self.get_logger().warning("Object point cloud have too many noise")
@@ -539,8 +526,11 @@ class DetectNode(Node):
mask_crop,
depth_crop,
intrinsics,
**self.configs
**self.calculate_configs
)
if rmat is None:
self.get_logger().warning("Color Area point cloud have too many noise")
return None, None
rmat = self.hand_eye_mat @ rmat
@@ -604,9 +594,13 @@ class DetectNode(Node):
mask_crop,
depth_crop,
intrinsics,
**self.configs
**self.calculate_configs
)
if rmat is None:
self.get_logger().warning("Corssboard point cloud have too many noise")
return None, None
x, y, z, rw, rx, ry, rz = rmat2quat(rmat)
self.get_logger().info(f"{x}, {y}, {z}, {rw}, {rx}, {ry}, {rz}")