|
| 1 | +# Copyright 2022 RT Corporation |
| 2 | +# |
| 3 | +# Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +# you may not use this file except in compliance with the License. |
| 5 | +# You may obtain a copy of the License at |
| 6 | +# |
| 7 | +# http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +# |
| 9 | +# Unless required by applicable law or agreed to in writing, software |
| 10 | +# distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +# See the License for the specific language governing permissions and |
| 13 | +# limitations under the License. |
| 14 | + |
| 15 | +import os |
| 16 | + |
| 17 | +from ament_index_python.packages import get_package_share_directory |
| 18 | +from crane_x7_description.robot_description_loader import RobotDescriptionLoader |
| 19 | +from launch import LaunchDescription |
| 20 | +from launch.actions import DeclareLaunchArgument |
| 21 | +from launch.substitutions import LaunchConfiguration |
| 22 | +from launch_ros.actions import SetParameter |
| 23 | +from launch_ros.actions import Node |
| 24 | +import yaml |
| 25 | + |
| 26 | + |
| 27 | +def load_file(package_name, file_path): |
| 28 | + package_path = get_package_share_directory(package_name) |
| 29 | + absolute_file_path = os.path.join(package_path, file_path) |
| 30 | + |
| 31 | + try: |
| 32 | + with open(absolute_file_path, 'r') as file: |
| 33 | + return file.read() |
| 34 | + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available |
| 35 | + return None |
| 36 | + |
| 37 | + |
| 38 | +def load_yaml(package_name, file_path): |
| 39 | + package_path = get_package_share_directory(package_name) |
| 40 | + absolute_file_path = os.path.join(package_path, file_path) |
| 41 | + |
| 42 | + try: |
| 43 | + with open(absolute_file_path, 'r') as file: |
| 44 | + return yaml.safe_load(file) |
| 45 | + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available |
| 46 | + return None |
| 47 | + |
| 48 | + |
| 49 | +def generate_launch_description(): |
| 50 | + description_loader = RobotDescriptionLoader() |
| 51 | + |
| 52 | + robot_description_semantic_config = load_file( |
| 53 | + 'crane_x7_moveit_config', 'config/crane_x7.srdf') |
| 54 | + robot_description_semantic = { |
| 55 | + 'robot_description_semantic': robot_description_semantic_config} |
| 56 | + |
| 57 | + kinematics_yaml = load_yaml('crane_x7_moveit_config', 'config/kinematics.yaml') |
| 58 | + |
| 59 | + declare_example_name = DeclareLaunchArgument( |
| 60 | + 'example', default_value='aruco_detection', |
| 61 | + description=('Set an example executable name: ' |
| 62 | + '[aruco_detection]') |
| 63 | + ) |
| 64 | + |
| 65 | + declare_use_sim_time = DeclareLaunchArgument( |
| 66 | + 'use_sim_time', default_value='false', |
| 67 | + description=('Set true when using the gazebo simulator.') |
| 68 | + ) |
| 69 | + |
| 70 | + picking_node = Node(name="pick_and_place_tf", |
| 71 | + package='crane_x7_examples', |
| 72 | + executable='pick_and_place_tf', |
| 73 | + output='screen', |
| 74 | + parameters=[{'robot_description': description_loader.load()}, |
| 75 | + robot_description_semantic, |
| 76 | + kinematics_yaml]) |
| 77 | + |
| 78 | + detection_node = Node(name=[LaunchConfiguration('example'), '_node'], |
| 79 | + package='crane_x7_examples', |
| 80 | + executable=LaunchConfiguration('example'), |
| 81 | + output='screen') |
| 82 | + |
| 83 | + return LaunchDescription([ |
| 84 | + declare_use_sim_time, |
| 85 | + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), |
| 86 | + picking_node, |
| 87 | + detection_node, |
| 88 | + declare_example_name |
| 89 | + ]) |
0 commit comments