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

add tools

Browse Source
This commit is contained in:
liangyuxuan
2025-11-26 10:30:55 +08:00
parent e99fa8758f
commit ea1a985b44
25 changed files with 331 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
vision_detect/vision_detect/VisionDetect/tools/calibration_tools.py → tools/calibration_tools.py
View File

0
vision_detect/vision_detect/VisionDetect/tools/data_tools.py → tools/data_tools.py
View File

0
vision_detect/vision_detect/VisionDetect/tools/error_calculation_tool.py → tools/error_calculation_tool.py
View File

23
tools/pt2engiene.py Normal file
View File

@@ -0,0 +1,23 @@
import argparse
from ultralytics import YOLO
def main(checkpoint_path):
model = YOLO(checkpoint_path)
model.export(
format="engine",
imgsz=(640, 480),
dynamic=True,
simplify=True,
half=True,
workspace=0.8,
batch=1,
device=0
)
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("checkpoint_path", type=str)
args = parser.parse_args()
main(args.checkpoint_path)

0
vision_detect/vision_detect/VisionDetect/tools/read_stl.py → tools/read_stl.py
View File

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

@@ -2,9 +2,9 @@
"node_name": "bottle_detect_service",
"output_boxes": "True",
"output_masks": "False",
"eye_in_left_hand_mat_path": "configs/hand_eye_mat/eye_in_left_hand.json",
"eye_in_right_hand_mat_path": "configs/hand_eye_mat/eye_in_right_hand.json",
"eye_to_hand_mat_path": "configs/hand_eye_mat/eye_to_hand.json",
"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"

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

@@ -2,9 +2,9 @@
"node_name": "bottle_detect_service",
"output_boxes": "True",
"output_masks": "False",
"eye_in_left_hand_mat_path": "configs/hand_eye_mat/eye_in_left_hand.json",
"eye_in_right_hand_mat_path": "configs/hand_eye_mat/eye_in_right_hand.json",
"eye_to_hand_mat_path": "configs/hand_eye_mat/eye_to_hand.json",
"left_hand": "configs/hand_eye_mat/eye_in_left_hand.json",
"right_handh": "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"

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

@@ -2,9 +2,9 @@
"node_name": "crossboard_topic",
"output_boxes": "True",
"output_masks": "False",
"eye_in_left_hand_mat_path": "configs/hand_eye_mat/eye_in_left_hand.json",
"eye_in_right_hand_mat_path": "configs/hand_eye_mat/eye_in_right_hand.json",
"eye_to_hand_mat_path": "configs/hand_eye_mat/eye_to_hand.json",
"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",

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

@@ -2,9 +2,9 @@
"node_name": "default_config_detect_service",
"output_boxes": "True",
"output_masks": "False",
"eye_in_left_hand_mat_path": "configs/hand_eye_mat/eye_in_left_hand.json",
"eye_in_right_hand_mat_path": "configs/hand_eye_mat/eye_in_right_hand.json",
"eye_to_hand_mat_path": "configs/hand_eye_mat/eye_to_hand.json",
"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"

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

@@ -2,9 +2,9 @@
"node_name": "detect_service",
"output_boxes": "True",
"output_masks": "False",
"eye_in_left_hand_mat_path": "configs/hand_eye_mat/eye_in_left_hand.json",
"eye_in_right_hand_mat_path": "configs/hand_eye_mat/eye_in_right_hand.json",
"eye_to_hand_mat_path": "configs/hand_eye_mat/eye_to_hand.json",
"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"

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

@@ -17,9 +17,9 @@ def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('eye_in_left_hand_mat_path', default_value=configs['eye_in_left_hand_mat_path']),
DeclareLaunchArgument('eye_in_right_hand_mat_path', default_value=configs['eye_in_right_hand_mat_path']),
DeclareLaunchArgument('eye_to_hand_mat_path', default_value=configs['eye_to_hand_mat_path']),
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']),
@@ -33,9 +33,9 @@ def generate_launch_description():
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
eye_in_left_hand_mat_path = LaunchConfiguration('eye_in_left_hand_mat_path').perform(context)
eye_in_right_hand_mat_path = LaunchConfiguration('eye_in_right_hand_mat_path').perform(context)
eye_to_hand_mat_path = LaunchConfiguration('eye_to_hand_mat_path').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)
@@ -53,9 +53,9 @@ def generate_launch_description():
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'eye_in_left_hand_mat_path': eye_in_left_hand_mat_path,
'eye_in_right_hand_mat_path': eye_in_right_hand_mat_path,
'eye_to_hand_mat_path': eye_to_hand_mat_path,
'left_hand': left_hand,
'right_hand': right_hand,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,

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

@@ -17,9 +17,9 @@ def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('eye_in_left_hand_mat_path', default_value=configs['eye_in_left_hand_mat_path']),
DeclareLaunchArgument('eye_in_right_hand_mat_path', default_value=configs['eye_in_right_hand_mat_path']),
DeclareLaunchArgument('eye_to_hand_mat_path', default_value=configs['eye_to_hand_mat_path']),
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']),
@@ -33,9 +33,9 @@ def generate_launch_description():
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
eye_in_left_hand_mat_path = LaunchConfiguration('eye_in_left_hand_mat_path').perform(context)
eye_in_right_hand_mat_path = LaunchConfiguration('eye_in_right_hand_mat_path').perform(context)
eye_to_hand_mat_path = LaunchConfiguration('eye_to_hand_mat_path').perform(context)
left_hand = LaunchConfiguration('left_hand').perform(context)
right_head = LaunchConfiguration('right_head').perform(context)
head = LaunchConfiguration('head').perform(context)
get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
detect_mode = LaunchConfiguration('detect_mode').perform(context)
@@ -53,9 +53,9 @@ def generate_launch_description():
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'eye_in_left_hand_mat_path': eye_in_left_hand_mat_path,
'eye_in_right_hand_mat_path': eye_in_right_hand_mat_path,
'eye_to_hand_mat_path': eye_to_hand_mat_path,
'left_hand': left_hand,
'right_head': right_head,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,

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

@@ -17,9 +17,10 @@ def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('eye_in_left_hand_mat_path', default_value=configs['eye_in_left_hand_mat_path']),
DeclareLaunchArgument('eye_in_right_hand_mat_path', default_value=configs['eye_in_right_hand_mat_path']),
DeclareLaunchArgument('eye_to_hand_mat_path', default_value=configs['eye_to_hand_mat_path']),
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']),
@@ -33,9 +34,9 @@ def generate_launch_description():
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
eye_in_left_hand_mat_path = LaunchConfiguration('eye_in_left_hand_mat_path').perform(context)
eye_in_right_hand_mat_path = LaunchConfiguration('eye_in_right_hand_mat_path').perform(context)
eye_to_hand_mat_path = LaunchConfiguration('eye_to_hand_mat_path').perform(context)
left_hand = LaunchConfiguration('left_hand').perform(context)
right_head = LaunchConfiguration('right_head').perform(context)
head = LaunchConfiguration('head').perform(context)
get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
detect_mode = LaunchConfiguration('detect_mode').perform(context)
@@ -53,9 +54,9 @@ def generate_launch_description():
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'eye_in_left_hand_mat_path': eye_in_left_hand_mat_path,
'eye_in_right_hand_mat_path': eye_in_right_hand_mat_path,
'eye_to_hand_mat_path': eye_to_hand_mat_path,
'left_hand': left_hand,
'right_head': right_head,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,

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

@@ -17,9 +17,9 @@ def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('eye_in_left_hand_mat_path', default_value=configs['eye_in_left_hand_mat_path']),
DeclareLaunchArgument('eye_in_right_hand_mat_path', default_value=configs['eye_in_right_hand_mat_path']),
DeclareLaunchArgument('eye_to_hand_mat_path', default_value=configs['eye_to_hand_mat_path']),
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']),
@@ -33,9 +33,9 @@ def generate_launch_description():
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
eye_in_left_hand_mat_path = LaunchConfiguration('eye_in_left_hand_mat_path').perform(context)
eye_in_right_hand_mat_path = LaunchConfiguration('eye_in_right_hand_mat_path').perform(context)
eye_to_hand_mat_path = LaunchConfiguration('eye_to_hand_mat_path').perform(context)
left_hand = LaunchConfiguration('left_hand').perform(context)
right_head = LaunchConfiguration('right_head').perform(context)
head = LaunchConfiguration('head').perform(context)
get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
detect_mode = LaunchConfiguration('detect_mode').perform(context)
@@ -53,9 +53,9 @@ def generate_launch_description():
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'eye_in_left_hand_mat_path': eye_in_left_hand_mat_path,
'eye_in_right_hand_mat_path': eye_in_right_hand_mat_path,
'eye_to_hand_mat_path': eye_to_hand_mat_path,
'left_hand': left_hand,
'right_head': right_head,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,

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

@@ -44,9 +44,9 @@ def generate_launch_description():
args_detect = [
DeclareLaunchArgument('output_boxes', default_value=configs['output_boxes']),
DeclareLaunchArgument('output_masks', default_value=configs['output_masks']),
DeclareLaunchArgument('eye_in_left_hand_mat_path', default_value=configs['eye_in_left_hand_mat_path']),
DeclareLaunchArgument('eye_in_right_hand_mat_path', default_value=configs['eye_in_right_hand_mat_path']),
DeclareLaunchArgument('eye_to_hand_mat_path', default_value=configs['eye_to_hand_mat_path']),
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']),
@@ -65,9 +65,9 @@ def generate_launch_description():
def create_detect_node(context):
output_boxes = LaunchConfiguration('output_boxes').perform(context)
output_masks = LaunchConfiguration('output_masks').perform(context)
eye_in_left_hand_mat_path = LaunchConfiguration('eye_in_left_hand_mat_path').perform(context)
eye_in_right_hand_mat_path = LaunchConfiguration('eye_in_right_hand_mat_path').perform(context)
eye_to_hand_mat_path = LaunchConfiguration('eye_to_hand_mat_path').perform(context)
left_hand = LaunchConfiguration('left_hand').perform(context)
right_head = LaunchConfiguration('right_head').perform(context)
head = LaunchConfiguration('head').perform(context)
get_camera_mode = LaunchConfiguration('get_camera_mode').perform(context)
detect_mode = LaunchConfiguration('detect_mode').perform(context)
@@ -90,9 +90,9 @@ def generate_launch_description():
parameters=[{
'output_boxes': output_boxes.lower() == 'true',
'output_masks': output_masks.lower() == 'true',
'eye_in_left_hand_mat_path': eye_in_left_hand_mat_path,
'eye_in_right_hand_mat_path': eye_in_right_hand_mat_path,
'eye_to_hand_mat_path': eye_to_hand_mat_path,
'left_hand': left_hand,
'right_head': right_head,
'head': head,
'get_camera_mode': get_camera_mode,
'detect_mode': detect_mode,

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

@@ -72,9 +72,9 @@ class DetectNode(Node):
self.declare_parameter('get_camera_mode', 'service')
self.declare_parameter('detect_mode', 'detect')
self.declare_parameter('calculate_mode', 'pca')
self.declare_parameter('eye_in_left_hand_mat_path', 'configs/hand_eye_mat/hand_in_left_hand.json')
self.declare_parameter('eye_in_right_hand_mat_path', 'configs/hand_eye_mat/hand_in_right_hand.json')
self.declare_parameter('eye_to_hand_mat_path', 'configs/hand_eye_mat/hand_to_hand.json')
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.output_boxes = self.get_parameter('output_boxes').value
self.output_masks = self.get_parameter('output_masks').value
@@ -150,9 +150,9 @@ class DetectNode(Node):
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('eye_in_left_hand_mat_path').value)
self.eye_in_right_hand_path = os.path.join(share_dir, self.get_parameter('eye_in_right_hand_mat_path').value)
self.eye_to_hand_path = os.path.join(share_dir, self.get_parameter('eye_to_hand_mat_path').value)
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)
if not os.path.exists(self.eye_in_left_hand_path):
self.get_logger().warning(f"{self.eye_in_left_hand_path} not found, use E(4, 4)")

0
vision_detect/vision_detect/VisionDetect/tools/__init__.py
View File

28
vision_test/calibration_mats/eye_in_left_hand.json Normal file
View File

@@ -0,0 +1,28 @@
{
"T": [
[
1.0,
0.0,
0.0,
0.0
],
[
0.0,
1.0,
0.0,
0.0
],
[
0.0,
0.0,
1.0,
0.0
],
[
0.0,
0.0,
0.0,
1.0
]
]
}

28
vision_test/calibration_mats/eye_in_right_hand.json Normal file
View File

@@ -0,0 +1,28 @@
{
"T": [
[
1.0,
0.0,
0.0,
0.0
],
[
0.0,
1.0,
0.0,
0.0
],
[
0.0,
0.0,
1.0,
0.0
],
[
0.0,
0.0,
0.0,
1.0
]
]
}

28
vision_test/calibration_mats/eye_to_hand.json Normal file
View File

@@ -0,0 +1,28 @@
{
"T": [
[
1.0,
0.0,
0.0,
0.0
],
[
0.0,
1.0,
0.0,
0.0
],
[
0.0,
0.0,
1.0,
0.0
],
[
0.0,
0.0,
0.0,
1.0
]
]
}

55
vision_test/configs/default_config.json Normal file
View File

@@ -0,0 +1,55 @@
{
"node_name": "default_config_detect_service",
"output_boxes": "True",
"output_masks": "False",
"calibrition_path": {
"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"
},
"Topic_configs": {
"position": "right",
"color_image_topic_name": "/camera/color/image_raw",
"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",
"confidence": 0.25,
"classes": []
},
"Color_configs": {
"distance": 1500,
"color_range": [[[0, 120, 70], [10, 255, 255]], [[170, 120, 70], [180, 255, 255]]]
},
"Crossboard_configs": {
"pattern_size": [8, 5]
},
"calculate_mode": "PCA",
"PCA_configs": {
"depth_scale": 1000.0,
"depth_trunc": 3.0,
"voxel_size": 0.020,
"nb_points": 10,
"radius": 0.1,
"nb_neighbors": 20,
"std_ratio": 3.0
},
"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,
"ransac_voxel_size": 0.005,
"icp_voxel_radius": [0.004, 0.002, 0.001],
"icp_max_iter": [50, 30, 14]
}
}

13
vision_test/include/vision_test/VisionDetectNode.hpp
View File

@@ -1,8 +1,11 @@
#include <map>
#include <string>
#include <nlohmann/json.hpp>
#include <Eigen/Dense>
#include "rclcpp/rclcpp.hpp"
#include <message_filters/subscriber.hpp>
#include <message_filters/sync_policies/approximate_time.hpp>
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
@@ -11,6 +14,7 @@
#include "interfaces/msg/pose_array_class_and_id.hpp"
using json = nlohmann::json;
using SyncPolicy=message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image>;
class VisionDetectNode: public rclcpp::Node {
@@ -20,16 +24,21 @@ class VisionDetectNode: public rclcpp::Node {
bool output_box, output_mask;
std::string GetCameraMode, DetectMode, CalculateMode;
std::string cfg_str;
json calibrition_path;
json GetCameraConfigs, DetectConfigs, CalculateConfigs;
Eigen::Matrix4d left_hand_mat, right_hand_mat, head_mat;
void (VisionDetectNode::*detect_function)();
void (VisionDetectNode::*calculate_function)();
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_detect_img;
rclcpp::Publisher<interfaces::msg::PoseArrayClassAndID>::SharedPtr pub_pose_list;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr color_img;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_img;
message_filters::Subscriber<sensor_msgs::msg::Image> color_img_sub;
message_filters::Subscriber<sensor_msgs::msg::Image> depth_img_sub;
std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_sub;
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info;
rclcpp::Service<interfaces::srv::VisionObjectRecognition>::SharedPtr detect_server;

6
vision_test/include/vision_test/utils.hpp Normal file
View File

@@ -0,0 +1,6 @@
#include <string>
#include <Eigen/Dense>
Eigen::Matrix4d read_data_from_json(const std::string& file_path, std::string& info);

53
vision_test/src/VisionDetectNode.cpp
View File

@@ -5,6 +5,7 @@
#include "rclcpp/rclcpp.hpp"
#include "vision_test/VisionDetectNode.hpp"
#include "vision_test/utils.hpp"
using json = nlohmann::json;
@@ -33,6 +34,7 @@ VisionDetectNode::VisionDetectNode(const std::string& name): Node(name) {
}
if (this->DetectMode == "Detect") {
this->init_model();
this->DetectConfigs = json::parse(this->get_parameter("DetectConfigs").as_string());
this->detect_function = &VisionDetectNode::seg_img;
}
@@ -54,31 +56,37 @@ VisionDetectNode::VisionDetectNode(const std::string& name): Node(name) {
void VisionDetectNode::init_param() {
/*Init Param*/
this->declare_parameter("output_box", true);
this->declare_parameter("output_mask", false);
this->declare_parameter<bool>("output_box", true);
this->declare_parameter<bool>("output_mask", false);
this->output_box = this->get_parameter("output_box").as_bool();
this->output_mask = this->get_parameter("output_mask").as_bool();
this->declare_parameter("GetCameraMode", "Service");
this->declare_parameter("DetectMode", "Detect");
this->declare_parameter("CalculateMode", "PCA");
this->declare_parameter<std::string>("GetCameraMode", "Service");
this->declare_parameter<std::string>("DetectMode", "Detect");
this->declare_parameter<std::string>("CalculateMode", "PCA");
this->GetCameraMode = this->get_parameter("GetCameraMode").as_string();
this->DetectMode = this->get_parameter("DetectMode").as_string();
this->CalculateMode = this->get_parameter("CalculateMode").as_string();
this->declare_parameter("ServiceConfigs", "{}");
this->declare_parameter("TopicConfigs", "{}");
this->declare_parameter<std::string>("calibrition_path", "{}");
this->declare_parameter("DetectConfigs", "{}");
this->declare_parameter("ColorConfigs", "{}");
this->declare_parameter("CrossboardConfigs", "{}");
this->calibrition_path = json::parse(this->get_parameter("calibrition_path").as_string());
this->declare_parameter("PCAConfigs", "{}");
this->declare_parameter("ICPConfigs", "{}");
this->declare_parameter<std::string>("ServiceConfigs", "{}");
this->declare_parameter<std::string>("TopicConfigs", "{}");
this->declare_parameter<std::string>("DetectConfigs", "{}");
this->declare_parameter<std::string>("ColorConfigs", "{}");
this->declare_parameter<std::string>("CrossboardConfigs", "{}");
this->declare_parameter<std::string>("PCAConfigs", "{}");
this->declare_parameter<std::string>("ICPConfigs", "{}");
RCLCPP_INFO(this->get_logger(), "Init param done");
// Done
}
@@ -90,14 +98,29 @@ void VisionDetectNode::init_model() {
void VisionDetectNode::init_calibration_mat() {
/*Init Calibration Mat*/
std::string info;
info = "left";
this->left_hand_mat = read_data_from_json(this->calibrition_path["left_hand"].get<std::string>(), info);
RCLCPP_INFO(this->get_logger(), info.c_str());
info = "right";
this->right_hand_mat = read_data_from_json(this->calibrition_path["right_hand"].get<std::string>(), info);
RCLCPP_INFO(this->get_logger(), info.c_str());
info = "head";
this->head_mat = read_data_from_json(this->calibrition_path["head"].get<std::string>(), info);
RCLCPP_INFO(this->get_logger(), info.c_str());
RCLCPP_INFO(this->get_logger(), "Init calibration mat done");
// Done
}
void VisionDetectNode::init_service() {
/*Init_service*/
detect_server = this->create_service<interfaces::srv::VisionObjectRecognition>(
std::string(GetCameraConfigs["service_name"]),
GetCameraConfigs["service_name"].get<std::string>(),
std::bind(
&VisionDetectNode::service_callback,
this,
@@ -107,11 +130,15 @@ void VisionDetectNode::init_service() {
);
RCLCPP_INFO(this->get_logger(), "Init service done");
// Done
}
void VisionDetectNode::init_topic() {
/*Init Topic*/
color_img_sub.subscribe(this, GetCameraConfigs["color_image_topic_name"].get<std::string>());
depth_img_sub.subscribe(this, GetCameraConfigs["depth_image_topic_name"].get<std::string>());
RCLCPP_INFO(this->get_logger(), "Init topic done");
}

45
vision_test/src/utils.cpp Normal file
View File

@@ -0,0 +1,45 @@
#include <string>
#include <fstream>
#include <Eigen/Dense>
#include <nlohmann/json.hpp>
Eigen::Matrix4d read_data_from_json(const std::string& file_path, std::string& info) {
Eigen::Matrix4d mat;
nlohmann::json jfile;
std::ifstream file(file_path);
if(!file.is_open()) {
if (info == "right") {
info = "Cannot open right_hand file! use E(4, 4)";
}
else if (info == "left") {
info = "Cannot open left_hand file! use E(4, 4)";
}
else {
info = "Cannot open head file! use E(4, 4)";
}
mat = Eigen::Matrix4d::Identity();
}
else {
file >> jfile;
std::vector<std::vector<double>> data = jfile["T"];
for(int i = 0; i < 4; i++) {
for(int j = 0; j < 4; j++) {
mat(i, j) = data[i][j];
}
}
if (info == "right") {
info = "Success read right_hand data";
}
else if (info == "left") {
info = "Success read left_hand data";
}
else {
info = "Success read head data";
}
}
return mat;
}