カメラ起動用launchファイル追加 (#151)

* 不要なファイルを削除 * ROS 2対応 * 頭部カメラlaunchファイル追加 * 胸部カメラlaunchファイル追加 * カメラパラメータファイル追加 * カメラ起動オプション追加 * 使用していないライブラリのimportを削除 * 依存パッケージ追加 * RViz設定変更 * camera_name修正 * カメラキャリブレーションパラメータ修正 * README更新
rt-net · Nov 24, 2023 · 111829a · 111829a
1 parent 7f337f7
commit 111829a
Show file tree

Hide file tree

Showing 14 changed files with 166 additions and 208 deletions.
diff --git a/README.en.md b/README.en.md
@@ -109,7 +109,7 @@ Please refer to [./sciurus17_examples/README.md](./sciurus17_examples/README.md)
   - This package includes option tools for Sciurus17.
 - sciurus17_vision
   - This package includes launch files for camera nodes.
-  - This package does not support ROS 2 yet.
+  - [chest_camera_info.yaml](./sciurus17_vision/config/chest_camera_info.yaml) is the chest camera calibration parameter.
 - sciurus17_description (external package)
   - [README](https://github.com/rt-net/sciurus17_description/blob/ros2/README.en.md)
   - This package includes a model data (xacro) of Sciurus17.

diff --git a/README.md b/README.md
@@ -109,7 +109,7 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control'
   - Sciurus17を活用するためのオプションツールをまとめたパッケージです
 - sciurus17_vision
   - カメラのlaunchファイルや画像認識を行うノードを定義するパッケージです
-  - ※現在ROS 2未対応のため使用できません。随時対応してまいります。
+  - 胸部カメラのキャリブレーションパラメータファイルは[chest_camera_info.yaml](./sciurus17_vision/config/chest_camera_info.yaml)です
 - sciurus17_description (外部パッケージ)
   - [README](https://github.com/rt-net/sciurus17_description/blob/ros2/README.md)
   - Sciurus17のモデルデータ（xacro）を定義するパッケージです

diff --git a/sciurus17_examples/launch/demo.launch.py b/sciurus17_examples/launch/demo.launch.py
@@ -19,6 +19,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.actions import IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 
@@ -36,6 +37,18 @@ def generate_launch_description():
         description='Set baudrate.'
     )
 
+    declare_use_head_camera = DeclareLaunchArgument(
+        'use_head_camera',
+        default_value='true',
+        description='Use head camera.'
+    )
+
+    declare_use_chest_camera = DeclareLaunchArgument(
+        'use_chest_camera',
+        default_value='true',
+        description='Use chest camera.'
+    )
+
     config_file_path = os.path.join(
         get_package_share_directory('sciurus17_control'),
         'config',
@@ -66,9 +79,27 @@ def generate_launch_description():
             launch_arguments={'loaded_description': description}.items()
         )
 
+    head_camera_node = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                get_package_share_directory('sciurus17_vision'),
+                '/launch/head_camera.launch.py']),
+            condition=IfCondition(LaunchConfiguration('use_head_camera')),
+        )
+
+    chest_camera_node = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                get_package_share_directory('sciurus17_vision'),
+                '/launch/chest_camera.launch.py']),
+            condition=IfCondition(LaunchConfiguration('use_chest_camera')),
+        )
+
     return LaunchDescription([
         declare_port_name,
         declare_baudrate,
+        declare_use_head_camera,
+        declare_use_chest_camera,
         move_group,
-        control_node
+        control_node,
+        head_camera_node,
+        chest_camera_node
     ])
diff --git a/sciurus17_moveit_config/config/moveit.rviz b/sciurus17_moveit_config/config/moveit.rviz
@@ -79,7 +79,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-            Value: true
           dummy_mimic_fix_l:
             Alpha: 1
             Show Axes: false
@@ -88,19 +87,10 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          head_camera_color_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          head_camera_color_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           head_camera_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-            Value: true
           l_gripperA_link:
             Alpha: 1
             Show Axes: false
@@ -275,7 +265,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-            Value: true
           dummy_mimic_fix_l:
             Alpha: 1
             Show Axes: false
@@ -284,19 +273,10 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          head_camera_color_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          head_camera_color_optical_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           head_camera_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-            Value: true
           l_gripperA_link:
             Alpha: 1
             Show Axes: false
@@ -438,14 +418,14 @@ Visualization Manager:
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.0010000000474974513
+      Size (m): 0.009999999776482582
       Style: Flat Squares
       Topic:
         Depth: 5
         Durability Policy: Volatile
         Filter size: 10
         History Policy: Keep Last
-        Reliability Policy: Reliable
+        Reliability Policy: Best Effort
         Value: /head_camera/depth/color/points
       Use Fixed Frame: true
       Use rainbow: true
@@ -520,5 +500,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1200
-  X: 170
-  Y: 27
+  X: 156
+  Y: 29
diff --git a/sciurus17_tools/scripts/get_chest_camera_product_id b/sciurus17_tools/scripts/get_chest_camera_product_id
diff --git a/sciurus17_tools/scripts/get_chest_camera_vendor_id b/sciurus17_tools/scripts/get_chest_camera_vendor_id
diff --git a/sciurus17_vision/CMakeLists.txt b/sciurus17_vision/CMakeLists.txt
@@ -1,35 +1,21 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(sciurus17_vision)
 
-add_compile_options(-std=c++11)
+find_package(ament_cmake REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS
-  cv_bridge
-  geometry_msgs
-  image_geometry
-  image_transport
-  pcl_ros
-  roscpp
-  rospy
-  sensor_msgs
-  std_msgs
-  tf
-  tf2
-  tf2_ros
-  sciurus17_msgs
-  dynamic_reconfigure
-)
-
-catkin_package(
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
 )
 
 include_directories(
-  include
   ${catkin_INCLUDE_DIRS}
 )
 
-set(MESSAGE_DEPENDS sciurus17_msgs_generate_messages)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
 
-install(DIRECTORY launch/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-)
+ament_package()
diff --git a/sciurus17_vision/COLCON_IGNORE b/sciurus17_vision/COLCON_IGNORE
diff --git a/sciurus17_vision/config/chest_camera_info.yaml b/sciurus17_vision/config/chest_camera_info.yaml
@@ -0,0 +1,26 @@
+image_width: 1280
+image_height: 720
+camera_name: chest_camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [520.89227,  -0.0822 , 628.05406,
+           0.     , 521.19138, 363.34233,
+           0.     ,   0.     ,   1.     ]
+distortion_model: equidistant
+distortion_coefficients:
+  rows: 1
+  cols: 4
+  data: [-0.221837, 1.448854, -1.560465, 0.357714]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1., 0., 0.,
+         0., 1., 0.,
+         0., 0., 1.]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [520.89227,  -0.0822 , 628.05406,   0.     ,
+           0.     , 521.19138, 363.34233,   0.     ,
+           0.     ,   0.     ,   1.     ,   0.     ]
diff --git a/sciurus17_vision/launch/chest_camera.launch b/sciurus17_vision/launch/chest_camera.launch
diff --git a/sciurus17_vision/launch/chest_camera.launch.py b/sciurus17_vision/launch/chest_camera.launch.py
@@ -0,0 +1,41 @@
+# Copyright 2023 RT Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    camera_info_file = 'file://' + get_package_share_directory(
+        'sciurus17_vision') + '/config/chest_camera_info.yaml'
+    usb_cam_node = Node(
+            package='usb_cam',
+            executable='usb_cam_node_exe',
+            namespace='chest_camera',
+            parameters=[
+                {'video_device': '/dev/chestcamera'},
+                {'frame_id': 'chest_camera_link'},
+                {'image_width': 1280},
+                {'image_height': 720},
+                {'framerate': 30.0},
+                {'camera_name': 'chest_camera'},
+                {'camera_info_url': camera_info_file},
+                {'pixel_format': 'mjpeg2rgb'}
+            ],
+        )
+
+    return LaunchDescription([
+        usb_cam_node
+    ])
diff --git a/sciurus17_vision/launch/head_camera.launch.py b/sciurus17_vision/launch/head_camera.launch.py
@@ -0,0 +1,38 @@
+# Copyright 2023 RT Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    head_camera_node = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                get_package_share_directory('realsense2_camera'),
+                '/launch/rs_launch.py']),
+            launch_arguments={
+                'camera_name': 'head_camera',
+                'device_type': 'd415',
+                'rgb_camera.profile': '640x360x30',
+                'depth_module.profile': '640x360x30',
+                'pointcloud.enable': 'true',
+                'align_depth.enable': 'true',
+            }.items()
+        )
+
+    return LaunchDescription([
+        head_camera_node
+    ])