added moveit_interface

launch/moveit_panda.launch.py

@@ -0,0 +1,238 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  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.
+
+# This file is an adapted version of
+# https://github.com/ros-planning/moveit_resources/blob/ca3f7930c630581b5504f3b22c40b4f82ee6369d/panda_moveit_config/launch/demo.launch.py
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription,
+                            Shutdown)
+from launch.conditions import UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import yaml
+
+
+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():
+    robot_ip_parameter_name = 'robot_ip'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+
+    # Command-line arguments
+
+    db_arg = DeclareLaunchArgument(
+        'db', default_value='False', description='Database flag'
+    )
+    log_level_arg = DeclareLaunchArgument(name='log_level', default_value='info')
+
+    # planning_context
+    franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots',
+                                     'panda_arm.urdf.xacro')
+    robot_description_config = Command(
+        [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=false',
+         ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
+         ' fake_sensor_commands:=', fake_sensor_commands])
+
+    robot_description = {'robot_description': robot_description_config}
+
+    franka_semantic_xacro_file = os.path.join(get_package_share_directory('franka_moveit_config'),
+                                              'srdf',
+                                              'panda_arm.srdf.xacro')
+    robot_description_semantic_config = Command(
+        [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=false']
+    )
+    robot_description_semantic = {
+        'robot_description_semantic': robot_description_semantic_config
+    }
+
+    kinematics_yaml = load_yaml(
+        'franka_moveit_config', 'config/kinematics.yaml'
+    )
+
+    # Planning Functionality
+    ompl_planning_pipeline_config = {
+        'move_group': {
+            'planning_plugin': 'ompl_interface/OMPLPlanner',
+            'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization '
+                                'default_planner_request_adapters/ResolveConstraintFrames '
+                                'default_planner_request_adapters/FixWorkspaceBounds '
+                                'default_planner_request_adapters/FixStartStateBounds '
+                                'default_planner_request_adapters/FixStartStateCollision '
+                                'default_planner_request_adapters/FixStartStatePathConstraints',
+            'start_state_max_bounds_error': 0.1,
+        }
+    }
+    ompl_planning_yaml = load_yaml(
+        'franka_moveit_config', 'config/ompl_planning.yaml'
+    )
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+    # Trajectory Execution Functionality
+    moveit_simple_controllers_yaml = load_yaml(
+        'franka_moveit_config', 'config/panda_controllers.yaml'
+    )
+    moveit_controllers = {
+        'moveit_simple_controller_manager': moveit_simple_controllers_yaml,
+        'moveit_controller_manager': 'moveit_simple_controller_manager'
+                                     '/MoveItSimpleControllerManager',
+    }
+
+    trajectory_execution = {
+        'moveit_manage_controllers': True,
+        'trajectory_execution.allowed_execution_duration_scaling': 1.2,
+        'trajectory_execution.allowed_goal_duration_margin': 0.5,
+        'trajectory_execution.allowed_start_tolerance': 0.01,
+    }
+
+    planning_scene_monitor_parameters = {
+        'publish_planning_scene': True,
+        'publish_geometry_updates': True,
+        'publish_state_updates': True,
+        'publish_transforms_updates': True,
+    }
+
+    # Start the actual move_group node/action server
+    run_move_group_node = Node(
+        package='moveit_ros_move_group',
+        executable='move_group',
+        output='screen',
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+        ],
+    )
+
+    # RViz
+    rviz_base = os.path.join(get_package_share_directory('franka_moveit_config'), 'rviz')
+    rviz_full_config = os.path.join(rviz_base, 'moveit.rviz')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='log',
+        arguments=['-d', rviz_full_config],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            ompl_planning_pipeline_config,
+            kinematics_yaml,
+        ],
+    )
+
+    # Publish TF
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='both',
+        parameters=[robot_description],
+    )
+
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory('franka_moveit_config'),
+        'config',
+        'panda_ros_controllers.yaml',
+    )
+    ros2_control_node = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[robot_description, ros2_controllers_path],
+        remappings=[('joint_states', 'franka/joint_states')],
+        output={
+            'stdout': 'screen',
+            'stderr': 'screen',
+        },
+        on_exit=Shutdown(),
+    )
+
+    # Load controllers
+    load_controllers = []
+    for controller in ['panda_arm_controller', 'joint_state_broadcaster']:
+        load_controllers += [
+            ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner {}'.format(controller)],
+                shell=True,
+                output='screen',
+            )
+        ]
+
+    joint_state_publisher = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        parameters=[
+            {'source_list': ['franka/joint_states'], 'rate': 30}],
+    )
+
+    franka_robot_state_broadcaster = Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['franka_robot_state_broadcaster'],
+            output='screen',
+            condition=UnlessCondition(use_fake_hardware),
+    )
+
+    robot_arg = DeclareLaunchArgument(
+        robot_ip_parameter_name,
+        description='Hostname or IP address of the robot.')
+
+    use_fake_hardware_arg = DeclareLaunchArgument(
+        use_fake_hardware_parameter_name,
+        default_value='true',
+        description='Use fake hardware')
+    fake_sensor_commands_arg = DeclareLaunchArgument(
+        fake_sensor_commands_parameter_name,
+        default_value='false',
+        description="Fake sensor commands. Only valid when '{}' is true".format(
+            use_fake_hardware_parameter_name))
+    return LaunchDescription(
+        [robot_arg,
+         use_fake_hardware_arg,
+         fake_sensor_commands_arg,
+         db_arg,
+         rviz_node,
+         robot_state_publisher,
+         run_move_group_node,
+         ros2_control_node,
+         joint_state_publisher,
+         franka_robot_state_broadcaster
+         ]
+        + load_controllers
+    )

setup.py

@@ -1,4 +1,6 @@
 from setuptools import find_packages, setup
+import os
+from glob import glob
 
 package_name = 'franka_iml_experiment'
 
@@ -10,6 +12,8 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/config', ['config/kinematics.yaml']),
+        (os.path.join('share', package_name), glob('launch/*.launch.py'))
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -20,6 +24,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'yaml_t = franka_iml_experiment.yaml_loader:main'
         ],
     },
 )