Merge pull request #4 from robomaster-oss/feature/update-config

Feature/update config
robomaster-oss · Apr 16, 2023 · cba6e72 · cba6e72
2 parents 1c24ca7 + eef5c6b
commit cba6e72
Show file tree

Hide file tree

Showing 3 changed files with 70 additions and 14 deletions.
diff --git a/config/cam_param.yaml b/config/cam_param.yaml
@@ -1,8 +1,13 @@
 /mindvision_camera:
     ros__parameters:
-        camera_name: 'mv_camera'
+        camera_name: 'front_camera'
+        frame_id: 'front_camera'
+        camera_info_url: 'package://rmoss_cam/resource/image_cam_calibration.yaml'
         sn: '048021610077'
-        config_path: './rmoss_core/rmoss_entity_cam/config/048021610077.Config'
+        config_path: 'package://rmoss_mindvision_driver/config/048021610077.Config'
         camera_k: [2032.263030, 0.000000, 625.248326, 0.000000, 2033.755589, 507.872755, 0.000000, 0.000000, 1.000000]
         camera_d: [-0.065000, 0.116275, -0.000281, 0.001765, 0.000000]
-        fps: 200
+        fps: 200
+        autostart: true
+        use_sensor_data_qos: false
+        use_image_transport_camera_publisher: true
diff --git a/launch/mindvision_cam.launch.py b/launch/mindvision_cam.launch.py
@@ -16,25 +16,73 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
+
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import UnlessCondition
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import LoadComposableNodes, Node
+from launch_ros.descriptions import ComposableNode
 
 
 def generate_launch_description():
+    package_path = get_package_share_directory('rmoss_mindvision_driver')
+    default_config_file_path = os.path.join(
+        package_path, 'config', 'cam_param.yaml')
+
+    # Declare the launch config
+    namespace = LaunchConfiguration('namespace')
+    config_file = LaunchConfiguration('config_file')
+    container_name = LaunchConfiguration('container_name')
+    use_eternal_container = LaunchConfiguration('use_eternal_container')
+    use_sim_time = LaunchConfiguration('use_sim_time')
 
-    param_path = os.path.join(
-        get_package_share_directory('rmoss_mindvision_driver'),
-        'config/cam_param.yaml')
+    # Declare the launch arguments
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace', default_value='', description='Namespace')
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name', default_value='mv_camera_container', description='Container name')
+    declare_config_file_cmd = DeclareLaunchArgument(
+        'config_file', default_value=default_config_file_path, description='Config file path')
+    declare_use_eternal_container_cmd = DeclareLaunchArgument(
+        'use_eternal_container', default_value='false', description='Use eternal container')
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time', default_value='false', description='Use sim time')
 
-    mv_cam_node = Node(
-        package='rmoss_mindvision_driver',
-        executable='mindvision_cam',
-        name='mindvision_camera',
-        parameters=[param_path],
-        output='screen'
+    # Create container node
+    container_node = Node(
+        name=container_name,
+        package='rclcpp_components',
+        executable='component_container_isolated',
+        output='screen',
+        condition=UnlessCondition(use_eternal_container),
+    )
+
+    # Create armor detector node
+    load_detector = LoadComposableNodes(
+        target_container=container_name,
+        composable_node_descriptions=[
+            ComposableNode(
+                package='rmoss_mindvision_driver',
+                plugin='rmoss_mindvision_driver::MindVisionCamNode',
+                name='mindvision_camera',
+                parameters=[config_file,
+                            {'use_sim_time': use_sim_time}],
+                namespace=namespace
+            )
+        ]
     )
 
     ld = LaunchDescription()
-    ld.add_action(mv_cam_node)
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_config_file_cmd)
+    ld.add_action(declare_use_eternal_container_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+
+    # Add nodes
+    ld.add_action(container_node)
+    ld.add_action(load_detector)
 
     return ld
diff --git a/src/mindvision_cam.cpp b/src/mindvision_cam.cpp
@@ -173,6 +173,7 @@ bool MindVisionCam::grab_image(cv::Mat & image)
     RCLCPP_WARN(
       node_->get_logger(),
       "ERROR: [%d] - Trigger failed!", status);
+    return false;
   }
 
   // 读取帧 超时1000ms
@@ -182,6 +183,7 @@ bool MindVisionCam::grab_image(cv::Mat & image)
     RCLCPP_WARN(
       node_->get_logger(),
       "ERROR [%d] - Frame read failed!", status);
+    return false;
   }
 
   image = cv::Mat(header.iHeight, header.iWidth, CV_8UC3);
@@ -190,6 +192,7 @@ bool MindVisionCam::grab_image(cv::Mat & image)
   status = CameraImageProcess(hCamera_, pFrameBuffer_, image.data, &header);
   if (status != CAMERA_STATUS_SUCCESS) {
     RCLCPP_WARN(node_->get_logger(), "Image process failed!");
+    return false;
   }
 
   // 释放缓存