ArUcoサンプルとピッキング動作の追加

crane_x7_examples/CMakeLists.txt

@@ -17,8 +17,10 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
+find_package(OpenCV REQUIRED COMPONENTS core)
 find_package(rclcpp REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
@@ -29,12 +31,16 @@ set(executable_list
   pose_groupstate
   joint_values
   pick_and_place
+  pick_and_place_tf
+  aruco_detection
 )
 foreach(loop_var IN LISTS executable_list)
   add_executable(${loop_var} src/${loop_var}.cpp)
   ament_target_dependencies(${loop_var}
+    cv_bridge
     geometry_msgs
     moveit_ros_planning_interface
+    OpenCV
     rclcpp
     tf2_geometry_msgs
   )

crane_x7_examples/launch/camera_example.launch.py

@@ -0,0 +1,89 @@
+# Copyright 2022 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from crane_x7_description.robot_description_loader import RobotDescriptionLoader
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import SetParameter
+from launch_ros.actions import Node
+import yaml
+
+
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return file.read()
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    description_loader = RobotDescriptionLoader()
+
+    robot_description_semantic_config = load_file(
+        'crane_x7_moveit_config', 'config/crane_x7.srdf')
+    robot_description_semantic = {
+        'robot_description_semantic': robot_description_semantic_config}
+
+    kinematics_yaml = load_yaml('crane_x7_moveit_config', 'config/kinematics.yaml')
+
+    declare_example_name = DeclareLaunchArgument(
+        'example', default_value='aruco_detection',
+        description=('Set an example executable name: '
+                     '[aruco_detection]')
+    )
+
+    declare_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time', default_value='false',
+        description=('Set true when using the gazebo simulator.')
+    )
+
+    picking_node = Node(name="pick_and_place_tf",
+                        package='crane_x7_examples',
+                        executable='pick_and_place_tf',
+                        output='screen',
+                        parameters=[{'robot_description': description_loader.load()},
+                                    robot_description_semantic,
+                                    kinematics_yaml])
+
+    detection_node = Node(name=[LaunchConfiguration('example'), '_node'],
+                          package='crane_x7_examples',
+                          executable=LaunchConfiguration('example'),
+                          output='screen')
+
+    return LaunchDescription([
+        declare_use_sim_time,
+        SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+        picking_node,
+        detection_node,
+        declare_example_name
+    ])