diff --git a/sciurus17_control/launch/sciurus17_control.launch.py b/sciurus17_control/launch/sciurus17_control.launch.py index 514912f6..0a38e339 100644 --- a/sciurus17_control/launch/sciurus17_control.launch.py +++ b/sciurus17_control/launch/sciurus17_control.launch.py @@ -17,7 +17,6 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from sciurus17_description.robot_description_loader import RobotDescriptionLoader @@ -25,9 +24,7 @@ def generate_launch_description(): config_file_path = os.path.join( - get_package_share_directory('sciurus17_control'), - 'config', - 'manipulator_config.yaml' + get_package_share_directory('sciurus17_control'), 'config', 'manipulator_config.yaml' ) description_loader = RobotDescriptionLoader() @@ -40,73 +37,82 @@ def generate_launch_description(): 'loaded_description', default_value=description_loader.load(), description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in sciurus17_description.' + It is recommended to use RobotDescriptionLoader() in sciurus17_description.', ) sciurus17_controllers = os.path.join( - get_package_share_directory('sciurus17_control'), - 'config', - 'sciurus17_controllers.yaml' - ) + get_package_share_directory('sciurus17_control'), 'config', 'sciurus17_controllers.yaml' + ) controller_manager = Node( package='controller_manager', executable='ros2_control_node', - parameters=[{'robot_description': LaunchConfiguration('loaded_description')}, - sciurus17_controllers], output='screen', - ) - - spawn_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2 run controller_manager spawner joint_state_broadcaster'], - shell=True, - output='screen', - ) - - spawn_right_arm_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner right_arm_controller'], - shell=True, - output='screen', - ) - - spawn_right_gripper_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner right_gripper_controller'], - shell=True, - output='screen', - ) - - spawn_left_arm_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner left_arm_controller'], - shell=True, - output='screen', - ) - - spawn_left_gripper_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner left_gripper_controller'], - shell=True, - output='screen', - ) - - spawn_neck_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner neck_controller'], - shell=True, - output='screen', - ) - - spawn_waist_yaw_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner waist_yaw_controller'], - shell=True, - output='screen', - ) - - return LaunchDescription([ - declare_loaded_description, - controller_manager, - spawn_right_arm_controller, - spawn_right_gripper_controller, - spawn_left_arm_controller, - spawn_left_gripper_controller, - spawn_joint_state_broadcaster, - spawn_neck_controller, - spawn_waist_yaw_controller - ]) + parameters=[ + {'robot_description': LaunchConfiguration('loaded_description')}, + sciurus17_controllers, + ], + ) + + spawn_joint_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['joint_state_broadcaster'], + ) + + spawn_right_arm_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['right_arm_controller'], + ) + + spawn_right_gripper_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['right_gripper_controller'], + ) + + spawn_left_arm_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['left_arm_controller'], + ) + + spawn_left_gripper_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['left_gripper_controller'], + ) + + spawn_neck_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['neck_controller'], + ) + + spawn_waist_yaw_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['waist_yaw_controller'], + ) + + return LaunchDescription( + [ + declare_loaded_description, + controller_manager, + spawn_right_arm_controller, + spawn_right_gripper_controller, + spawn_left_arm_controller, + spawn_left_gripper_controller, + spawn_joint_state_broadcaster, + spawn_neck_controller, + spawn_waist_yaw_controller, + ] + ) diff --git a/sciurus17_examples/launch/camera_example.launch.py b/sciurus17_examples/launch/camera_example.launch.py index 02820a23..68cb1a79 100644 --- a/sciurus17_examples/launch/camera_example.launch.py +++ b/sciurus17_examples/launch/camera_example.launch.py @@ -12,78 +12,56 @@ # 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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_ros.actions import SetParameter +from moveit_configs_utils import MoveItConfigsBuilder from sciurus17_description.robot_description_loader import RobotDescriptionLoader -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( - 'sciurus17_moveit_config', 'config/sciurus17.srdf') - robot_description_semantic = { - 'robot_description_semantic': robot_description_semantic_config} - - kinematics_yaml = load_yaml('sciurus17_moveit_config', 'config/kinematics.yaml') - declare_example_name = DeclareLaunchArgument( - 'example', default_value='point_cloud_detection', - description=('Set an example executable name: ' - '[point_cloud_detection]') + 'example', + default_value='point_cloud_detection', + description=('Set an example executable name: [point_cloud_detection]'), ) declare_use_sim_time = DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description=('Set true when using the gazebo simulator.') + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), ) - picking_node = Node(name='pick_and_place_tf', - package='sciurus17_examples', - executable='pick_and_place_tf', - output='screen', - parameters=[{'robot_description': description_loader.load()}, - robot_description_semantic, - kinematics_yaml]) + description_loader = RobotDescriptionLoader() + + moveit_config = MoveItConfigsBuilder('sciurus17').to_moveit_configs() + moveit_config.robot_description = { + 'robot_description': description_loader.load(), + } + + picking_node = Node( + name='pick_and_place_tf', + package='sciurus17_examples', + executable='pick_and_place_tf', + output='screen', + parameters=[moveit_config.to_dict()], + ) - detection_node = Node(name=[LaunchConfiguration('example'), '_node'], - package='sciurus17_examples', - executable=LaunchConfiguration('example'), - output='screen') + detection_node = Node( + name=[LaunchConfiguration('example'), '_node'], + package='sciurus17_examples', + executable=LaunchConfiguration('example'), + output='screen', + ) - return LaunchDescription([ - declare_use_sim_time, - SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), - picking_node, - declare_example_name, - detection_node - ]) + return LaunchDescription( + [ + declare_example_name, + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + picking_node, + detection_node, + ] + ) diff --git a/sciurus17_examples/launch/chest_camera_tracking.launch.py b/sciurus17_examples/launch/chest_camera_tracking.launch.py index 03d96560..06f6ab6e 100644 --- a/sciurus17_examples/launch/chest_camera_tracking.launch.py +++ b/sciurus17_examples/launch/chest_camera_tracking.launch.py @@ -22,8 +22,9 @@ def generate_launch_description(): declare_use_sim_time = DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description=('Set true when using the gazebo simulator.') + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), ) container = ComposableNodeContainer( @@ -31,40 +32,38 @@ def generate_launch_description(): namespace='head_camera_tracking', package='rclcpp_components', executable='component_container', + output='screen', composable_node_descriptions=[ ComposableNode( - package='sciurus17_examples', - plugin='sciurus17_examples::ColorDetection', name='color_detection', namespace='chest_camera_tracking', - remappings=[ - ('/image_raw', '/chest_camera/image_raw') - ], - extra_arguments=[{'use_intra_process_comms': True}] - ), - ComposableNode( package='sciurus17_examples', - plugin='sciurus17_examples::ObjectTracker', + plugin='sciurus17_examples::ColorDetection', + remappings=[('/image_raw', '/chest_camera/image_raw')], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ComposableNode( name='object_tracker', namespace='chest_camera_tracking', - remappings=[ - ('/controller_state', '/waist_yaw_controller/controller_state') - ], - extra_arguments=[{'use_intra_process_comms': True}] - ), - ComposableNode( package='sciurus17_examples', - plugin='sciurus17_examples::WaistJtControl', + plugin='sciurus17_examples::ObjectTracker', + remappings=[('/controller_state', '/waist_yaw_controller/controller_state')], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ComposableNode( name='waist_jt_control', namespace='chest_camera_tracking', - extra_arguments=[{'use_intra_process_comms': True}] - ), + package='sciurus17_examples', + plugin='sciurus17_examples::WaistJtControl', + extra_arguments=[{'use_intra_process_comms': True}], + ), ], - output='screen', ) - return LaunchDescription([ - declare_use_sim_time, - SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), - container - ]) + return LaunchDescription( + [ + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + container, + ] + ) diff --git a/sciurus17_examples/launch/demo.launch.py b/sciurus17_examples/launch/demo.launch.py index 59d1f903..625ecdc5 100644 --- a/sciurus17_examples/launch/demo.launch.py +++ b/sciurus17_examples/launch/demo.launch.py @@ -26,39 +26,27 @@ def generate_launch_description(): declare_port_name = DeclareLaunchArgument( - 'port_name', - default_value='/dev/sciurus17spine', - description='Set port name.' + 'port_name', default_value='/dev/sciurus17spine', description='Set port name.' ) declare_baudrate = DeclareLaunchArgument( - 'baudrate', - default_value='3000000', - description='Set baudrate.' + 'baudrate', default_value='3000000', description='Set baudrate.' ) declare_use_head_camera = DeclareLaunchArgument( - 'use_head_camera', - default_value='true', - description='Use head camera.' + '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.' + 'use_chest_camera', default_value='true', description='Use chest camera.' ) config_file_path = os.path.join( - get_package_share_directory('sciurus17_control'), - 'config', - 'manipulator_config.yaml' + get_package_share_directory('sciurus17_control'), 'config', 'manipulator_config.yaml' ) declare_use_mock_components = DeclareLaunchArgument( - 'use_mock_components', - default_value='false', - description='Use mock_components or not.' + 'use_mock_components', default_value='false', description='Use mock_components or not.' ) description_loader = RobotDescriptionLoader() @@ -67,47 +55,52 @@ def generate_launch_description(): description_loader.timeout_seconds = '1.0' description_loader.manipulator_config_file_path = config_file_path description_loader.use_mock_components = LaunchConfiguration('use_mock_components') - description = description_loader.load() move_group = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ + PythonLaunchDescriptionSource( + [ get_package_share_directory('sciurus17_moveit_config'), - '/launch/run_move_group.launch.py']), - launch_arguments={ - 'loaded_description': description - }.items() - ) + '/launch/run_move_group.launch.py', + ] + ), + launch_arguments={'loaded_description': description}.items(), + ) control_node = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ + PythonLaunchDescriptionSource( + [ get_package_share_directory('sciurus17_control'), - '/launch/sciurus17_control.launch.py']), - launch_arguments={'loaded_description': description}.items() - ) + '/launch/sciurus17_control.launch.py', + ] + ), + 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')), - ) + 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')), - ) + 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, - declare_use_mock_components, - move_group, - control_node, - head_camera_node, - chest_camera_node - ]) + return LaunchDescription( + [ + declare_port_name, + declare_baudrate, + declare_use_head_camera, + declare_use_chest_camera, + declare_use_mock_components, + move_group, + control_node, + head_camera_node, + chest_camera_node, + ] + ) diff --git a/sciurus17_examples/launch/example.launch.py b/sciurus17_examples/launch/example.launch.py index b2fd1206..c0097cd5 100644 --- a/sciurus17_examples/launch/example.launch.py +++ b/sciurus17_examples/launch/example.launch.py @@ -12,73 +12,52 @@ # 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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_ros.actions import SetParameter +from moveit_configs_utils import MoveItConfigsBuilder from sciurus17_description.robot_description_loader import RobotDescriptionLoader -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( - 'sciurus17_moveit_config', 'config/sciurus17.srdf') - robot_description_semantic = { - 'robot_description_semantic': robot_description_semantic_config} - - kinematics_yaml = load_yaml('sciurus17_moveit_config', 'config/kinematics.yaml') - declare_example_name = DeclareLaunchArgument( - 'example', default_value='gripper_control', - description=('Set an example executable name: ' - '[gripper_control, neck_control, waist_control,' - 'pick_and_place_right_arm_waist, pick_and_place_left_arm]') + 'example', + default_value='gripper_control', + description=( + 'Set an example executable name: ' + '[gripper_control, neck_control, waist_control,' + 'pick_and_place_right_arm_waist, pick_and_place_left_arm]' + ), ) declare_use_sim_time = DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description=('Set true when using the gazebo simulator.') + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), ) - example_node = Node(name=[LaunchConfiguration('example'), '_node'], - package='sciurus17_examples', - executable=LaunchConfiguration('example'), - output='screen', - parameters=[{'robot_description': description_loader.load()}, - robot_description_semantic, - kinematics_yaml]) + description_loader = RobotDescriptionLoader() - return LaunchDescription([ - declare_use_sim_time, - SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), - declare_example_name, - example_node - ]) + moveit_config = MoveItConfigsBuilder('sciurus17').to_moveit_configs() + moveit_config.robot_description = { + 'robot_description': description_loader.load(), + } + + example_node = Node( + name=[LaunchConfiguration('example'), '_node'], + package='sciurus17_examples', + executable=LaunchConfiguration('example'), + output='screen', + parameters=[moveit_config.to_dict()], + ) + + return LaunchDescription( + [ + declare_example_name, + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + example_node, + ] + ) diff --git a/sciurus17_examples/launch/head_camera_tracking.launch.py b/sciurus17_examples/launch/head_camera_tracking.launch.py index 4ca5c6b8..bf7bca29 100644 --- a/sciurus17_examples/launch/head_camera_tracking.launch.py +++ b/sciurus17_examples/launch/head_camera_tracking.launch.py @@ -22,8 +22,9 @@ def generate_launch_description(): declare_use_sim_time = DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description=('Set true when using the gazebo simulator.') + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), ) container = ComposableNodeContainer( @@ -31,40 +32,38 @@ def generate_launch_description(): namespace='head_camera_tracking', package='rclcpp_components', executable='component_container', + output='screen', composable_node_descriptions=[ ComposableNode( - package='sciurus17_examples', - plugin='sciurus17_examples::ColorDetection', name='color_detection', namespace='head_camera_tracking', - remappings=[ - ('/image_raw', '/head_camera/color/image_raw') - ], - extra_arguments=[{'use_intra_process_comms': True}] - ), - ComposableNode( package='sciurus17_examples', - plugin='sciurus17_examples::ObjectTracker', + plugin='sciurus17_examples::ColorDetection', + remappings=[('/image_raw', '/head_camera/color/image_raw')], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ComposableNode( name='object_tracker', namespace='head_camera_tracking', - remappings=[ - ('/controller_state', '/neck_controller/controller_state') - ], - extra_arguments=[{'use_intra_process_comms': True}] - ), - ComposableNode( package='sciurus17_examples', - plugin='sciurus17_examples::NeckJtControl', + plugin='sciurus17_examples::ObjectTracker', + remappings=[('/controller_state', '/neck_controller/controller_state')], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ComposableNode( name='neck_jt_control', namespace='head_camera_tracking', - extra_arguments=[{'use_intra_process_comms': True}] - ), + package='sciurus17_examples', + plugin='sciurus17_examples::NeckJtControl', + extra_arguments=[{'use_intra_process_comms': True}], + ), ], - output='screen', ) - return LaunchDescription([ - declare_use_sim_time, - SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), - container - ]) + return LaunchDescription( + [ + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + container, + ] + ) diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py index 4c560243..e2419397 100644 --- a/sciurus17_examples_py/launch/example.launch.py +++ b/sciurus17_examples_py/launch/example.launch.py @@ -22,6 +22,21 @@ def generate_launch_description(): + declare_example_name = DeclareLaunchArgument( + 'example', + default_value='gripper_control', + description=( + 'Set an example executable name: ' + '[gripper_control, neck_control, waist_control, ' + 'pick_and_place_right_arm_waist, pick_and_place_left_arm]' + ), + ) + + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), + ) description_loader = RobotDescriptionLoader() declare_loaded_description = DeclareLaunchArgument( @@ -29,32 +44,26 @@ def generate_launch_description(): default_value=description_loader.load(), description='Set robot_description text. \ It is recommended to use RobotDescriptionLoader() \ - in sciurus17_description.') - - moveit_config = (MoveItConfigsBuilder('sciurus17').planning_scene_monitor( - publish_robot_description=True, - publish_robot_description_semantic=True, - ).moveit_cpp(file_path=get_package_share_directory('sciurus17_examples_py') + - '/config/sciurus17_moveit_py_examples.yaml').to_moveit_configs()) + in sciurus17_description.', + ) + moveit_config = ( + MoveItConfigsBuilder('sciurus17') + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .moveit_cpp( + file_path=get_package_share_directory('sciurus17_examples_py') + + '/config/sciurus17_moveit_py_examples.yaml' + ) + .to_moveit_configs() + ) moveit_config.robot_description = { 'robot_description': LaunchConfiguration('loaded_description') } - moveit_config.move_group_capabilities = {'capabilities': ''} - declare_example_name = DeclareLaunchArgument( - 'example', - default_value='gripper_control', - description=('Set an example executable name: ' - '[gripper_control, neck_control, waist_control, ' - 'pick_and_place_right_arm_waist, pick_and_place_left_arm]')) - - declare_use_sim_time = DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description=('Set true when using the gazebo simulator.') - ) - # 下記Issue対応のためここでパラメータを設定する # https://github.com/moveit/moveit2/issues/2940#issuecomment-2401302214 config_dict = moveit_config.to_dict() @@ -68,9 +77,11 @@ def generate_launch_description(): parameters=[config_dict], ) - return LaunchDescription([ - declare_loaded_description, - declare_use_sim_time, - declare_example_name, - example_node - ]) + return LaunchDescription( + [ + declare_use_sim_time, + declare_example_name, + declare_loaded_description, + example_node, + ] + ) diff --git a/sciurus17_gazebo/launch/sciurus17_with_table.launch.py b/sciurus17_gazebo/launch/sciurus17_with_table.launch.py index ad1a5568..e7468ea2 100644 --- a/sciurus17_gazebo/launch/sciurus17_with_table.launch.py +++ b/sciurus17_gazebo/launch/sciurus17_with_table.launch.py @@ -28,41 +28,46 @@ def generate_launch_description(): declare_use_head_camera = DeclareLaunchArgument( - 'use_head_camera', - default_value='true', - description='Use head camera.' + '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.' + 'use_chest_camera', default_value='true', description='Use chest camera.' ) # PATHを追加で通さないとSTLファイルが読み込まれない - env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], - 'GZ_SIM_RESOURCE_PATH': os.path.dirname( - get_package_share_directory('sciurus17_description'))} + env = { + 'GZ_SIM_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], + 'GZ_SIM_RESOURCE_PATH': os.path.dirname( + get_package_share_directory('sciurus17_description') + ), + } world_file = os.path.join( - get_package_share_directory('sciurus17_gazebo'), 'worlds', 'table.sdf') - gui_config = os.path.join( - get_package_share_directory('sciurus17_gazebo'), 'gui', 'gui.config') + get_package_share_directory('sciurus17_gazebo'), 'worlds', 'table.sdf' + ) + gui_config = os.path.join(get_package_share_directory('sciurus17_gazebo'), 'gui', 'gui.config') # -r オプションで起動時にシミュレーションをスタートしないと、コントローラが起動しない gz_sim = ExecuteProcess( - cmd=['gz sim -r', world_file, '--gui-config', gui_config], - output='screen', - additional_env=env, - shell=True - ) + cmd=['gz sim -r', world_file, '--gui-config', gui_config], + output='screen', + additional_env=env, + shell=True, + ) gz_sim_spawn_entity = Node( package='ros_gz_sim', executable='create', output='screen', - arguments=['-topic', '/robot_description', - '-name', 'sciurus17', - '-z', '1.02', - '-allow_renaming', 'true'], + arguments=[ + '-topic', + '/robot_description', + '-name', + 'sciurus17', + '-z', + '1.02', + '-allow_renaming', + 'true', + ], ) description_loader = RobotDescriptionLoader() @@ -74,77 +79,90 @@ def generate_launch_description(): description = description_loader.load() move_group = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ + PythonLaunchDescriptionSource( + [ get_package_share_directory('sciurus17_moveit_config'), - '/launch/run_move_group.launch.py']), - launch_arguments={'loaded_description': description}.items() - ) - - spawn_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2 run controller_manager spawner joint_state_broadcaster'], - shell=True, - output='screen', - ) - - spawn_right_arm_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner right_arm_controller'], - shell=True, - output='screen', - ) - - spawn_right_gripper_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner right_gripper_controller'], - shell=True, - output='screen', - ) - - spawn_left_arm_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner left_arm_controller'], - shell=True, - output='screen', - ) - - spawn_left_gripper_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner left_gripper_controller'], - shell=True, - output='screen', - ) - - spawn_neck_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner neck_controller'], - shell=True, - output='screen', - ) - - spawn_waist_yaw_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner waist_yaw_controller'], - shell=True, - output='screen', - ) + '/launch/run_move_group.launch.py', + ] + ), + launch_arguments={'loaded_description': description}.items(), + ) + + spawn_joint_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['joint_state_broadcaster'], + ) + + spawn_right_arm_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['right_arm_controller'], + ) + + spawn_right_gripper_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['right_gripper_controller'], + ) + + spawn_left_arm_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['left_arm_controller'], + ) + + spawn_left_gripper_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['left_gripper_controller'], + ) + + spawn_neck_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['neck_controller'], + ) + + spawn_waist_yaw_controller = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments=['waist_yaw_controller'], + ) bridge_file = os.path.join( - get_package_share_directory('sciurus17_gazebo'), 'config', 'bridge.yaml') + get_package_share_directory('sciurus17_gazebo'), 'config', 'bridge.yaml' + ) bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - parameters=[{'config_file': bridge_file}], - output='screen' - ) - - return LaunchDescription([ - SetParameter(name='use_sim_time', value=True), - declare_use_head_camera, - declare_use_chest_camera, - gz_sim, - gz_sim_spawn_entity, - spawn_joint_state_broadcaster, - spawn_right_arm_controller, - spawn_right_gripper_controller, - spawn_left_arm_controller, - spawn_left_gripper_controller, - spawn_neck_controller, - spawn_waist_yaw_controller, - move_group, - bridge - ]) + package='ros_gz_bridge', + executable='parameter_bridge', + output='screen', + parameters=[{'config_file': bridge_file}], + ) + + return LaunchDescription( + [ + SetParameter(name='use_sim_time', value=True), + declare_use_head_camera, + declare_use_chest_camera, + gz_sim, + gz_sim_spawn_entity, + move_group, + spawn_joint_state_broadcaster, + spawn_right_arm_controller, + spawn_right_gripper_controller, + spawn_left_arm_controller, + spawn_left_gripper_controller, + spawn_neck_controller, + spawn_waist_yaw_controller, + bridge, + ] + ) diff --git a/sciurus17_moveit_config/launch/run_move_group.launch.py b/sciurus17_moveit_config/launch/run_move_group.launch.py index 69e91055..79a1309f 100644 --- a/sciurus17_moveit_config/launch/run_move_group.launch.py +++ b/sciurus17_moveit_config/launch/run_move_group.launch.py @@ -1,4 +1,3 @@ -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -11,27 +10,14 @@ def generate_launch_description(): - ld = LaunchDescription() - description_loader = RobotDescriptionLoader() - ld.add_action( - DeclareLaunchArgument( - 'loaded_description', - default_value=description_loader.load(), - description='Set robot_description text. \ + declare_robot_description = DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ It is recommended to use RobotDescriptionLoader() \ - in sciurus17_description.' - ) - ) - - ld.add_action( - DeclareLaunchArgument( - 'rviz_config', - default_value=get_package_share_directory( - 'sciurus17_moveit_config') + '/config/moveit.rviz', - description='Set the path to rviz configuration file.' - ) + in sciurus17_description.', ) moveit_config = ( @@ -42,22 +28,18 @@ def generate_launch_description(): ) .planning_pipelines(pipelines=['ompl']) .to_moveit_configs() - ) + ) moveit_config.robot_description = { 'robot_description': LaunchConfiguration('loaded_description') - } - - # Move group - ld.add_entity(generate_move_group_launch(moveit_config)) - - # RViz - ld.add_entity(generate_moveit_rviz_launch(moveit_config)) - - # Static TF - ld.add_entity(generate_static_virtual_joint_tfs_launch(moveit_config)) - - # Publish TF - ld.add_entity(generate_rsp_launch(moveit_config)) - - return ld + } + + return LaunchDescription( + [ + declare_robot_description, + generate_move_group_launch(moveit_config), + generate_moveit_rviz_launch(moveit_config), + generate_static_virtual_joint_tfs_launch(moveit_config), + generate_rsp_launch(moveit_config), + ] + ) diff --git a/sciurus17_moveit_config/launch/setup_assistant.launch.py b/sciurus17_moveit_config/launch/setup_assistant.launch.py index 55c82e28..ba5b30d1 100644 --- a/sciurus17_moveit_config/launch/setup_assistant.launch.py +++ b/sciurus17_moveit_config/launch/setup_assistant.launch.py @@ -4,5 +4,6 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder( - 'sciurus17', package_name='sciurus17_moveit_config').to_moveit_configs() + 'sciurus17', package_name='sciurus17_moveit_config' + ).to_moveit_configs() return generate_setup_assistant_launch(moveit_config) diff --git a/sciurus17_vision/launch/chest_camera.launch.py b/sciurus17_vision/launch/chest_camera.launch.py index 8c2bb1e4..1862e37f 100644 --- a/sciurus17_vision/launch/chest_camera.launch.py +++ b/sciurus17_vision/launch/chest_camera.launch.py @@ -20,24 +20,29 @@ def generate_launch_description(): - camera_info_file = 'file://' + get_package_share_directory( - 'sciurus17_vision') + '/config/chest_camera_info.yaml' + 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': os.path.realpath('/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'} - ], - ) + namespace='chest_camera', + package='usb_cam', + executable='usb_cam_node_exe', + parameters=[ + {'video_device': os.path.realpath('/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 - ]) + return LaunchDescription( + [ + usb_cam_node, + ] + ) diff --git a/sciurus17_vision/launch/head_camera.launch.py b/sciurus17_vision/launch/head_camera.launch.py index a7cc0c8a..307e553b 100644 --- a/sciurus17_vision/launch/head_camera.launch.py +++ b/sciurus17_vision/launch/head_camera.launch.py @@ -20,20 +20,22 @@ def generate_launch_description(): head_camera_node = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - get_package_share_directory('realsense2_camera'), - '/launch/rs_launch.py']), - launch_arguments={ - 'camera_namespace': '', - 'camera_name': 'head_camera', - 'device_type': 'd415', - 'rgb_camera.color_profile': '640x360x30', - 'depth_module.depth_profile': '640x360x30', - 'pointcloud.enable': 'true', - 'align_depth.enable': 'true', - }.items() - ) + PythonLaunchDescriptionSource( + [get_package_share_directory('realsense2_camera'), '/launch/rs_launch.py'] + ), + launch_arguments={ + 'camera_namespace': '', + 'camera_name': 'head_camera', + 'device_type': 'd415', + 'rgb_camera.color_profile': '640x360x30', + 'depth_module.depth_profile': '640x360x30', + 'pointcloud.enable': 'true', + 'align_depth.enable': 'true', + }.items(), + ) - return LaunchDescription([ - head_camera_node - ]) + return LaunchDescription( + [ + head_camera_node, + ] + )