ramsai description and planning

ramsai/package.xml

@@ -5,7 +5,7 @@
   <version>0.0.1</version>
   <description>Meta-package aggregating the ramsai packages and documentation</description>
   <maintainer email="[email protected]">Laurent Barbé</maintainer>
-  <maintainer email="macbednarczyk@gmail.com">Maciej Bednarczyk</maintainer>
+  <maintainer email="mcbed.robotics@gmail.com">Maciej Bednarczyk</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

ramsai_bringup/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(ramsai_bringup)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+################################################################################
+# Install
+################################################################################
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

ramsai_bringup/launch/ramsai.launch.py

@@ -0,0 +1,298 @@
+# Copyright 2022 ICube Laboratory, University of Strasbourg
+#
+# 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 launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition, UnlessCondition
+from launch.event_handlers import OnProcessExit, OnProcessStart
+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
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'use_sim',
+            default_value='false',
+            description='Start robot in Gazebo simulation.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'use_fake_hardware',
+            default_value='true',
+            description='Start robot with fake hardware mirroring command to its states.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'use_planning',
+            default_value='false',
+            description='Start robot with Moveit2 `move_group` planning \
+                         config for Pilz and OMPL.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'use_servoing',
+            default_value='false',
+            description='Start robot with Moveit2 servoing.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'robot_controller',
+            default_value='iiwa_arm_controller',
+            description='Robot controller to start.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'start_rviz',
+            default_value='true',
+            description='Start RViz2 automatically with this launch file.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'robot_ip',
+            default_value='192.170.10.2',
+            description='Robot IP of FRI interface',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'robot_port',
+            default_value='30200',
+            description='Robot port of FRI interface.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'initial_positions_file',
+            default_value='initial_positions.yaml',
+            description='Configuration file of robot initial positions for simulation.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'command_interface',
+            default_value='position',
+            description='Robot command interface [position|velocity|effort].',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'base_frame_file',
+            default_value='base_frame.yaml',
+            description='Configuration file of robot base frame wrt World.',
+        )
+    )
+
+    # Initialize Arguments
+    use_sim = LaunchConfiguration('use_sim')
+    use_fake_hardware = LaunchConfiguration('use_fake_hardware')
+    use_planning = LaunchConfiguration('use_planning')
+    robot_controller = LaunchConfiguration('robot_controller')
+    start_rviz = LaunchConfiguration('start_rviz')
+    robot_ip = LaunchConfiguration('robot_ip')
+    robot_port = LaunchConfiguration('robot_port')
+    initial_positions_file = LaunchConfiguration('initial_positions_file')
+    command_interface = LaunchConfiguration('command_interface')
+    base_frame_file = LaunchConfiguration('base_frame_file')
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name='xacro')]),
+            ' ',
+            PathJoinSubstitution(
+                [FindPackageShare('ramsai_description'), 'config', 'ramsai.config.xacro']
+            ),
+            ' ',
+            'prefix:=""',
+            ' ',
+            'use_sim:=',
+            use_sim,
+            ' ',
+            'use_fake_hardware:=',
+            use_fake_hardware,
+            ' ',
+            'robot_ip:=',
+            robot_ip,
+            ' ',
+            'robot_port:=',
+            robot_port,
+            ' ',
+            'initial_positions_file:=',
+            initial_positions_file,
+            ' ',
+            'command_interface:=',
+            command_interface,
+            ' ',
+            'base_frame_file:=',
+            base_frame_file,
+        ]
+    )
+
+    robot_description = {'robot_description': robot_description_content}
+
+    # Running with Moveit2 planning
+    iiwa_planning_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            FindPackageShare('ramsai_bringup'),
+            '/launch',
+            '/ramsai_planning.launch.py'
+        ]),
+        launch_arguments={
+            'start_rviz': start_rviz,
+            'base_frame_file': base_frame_file,
+            'use_sim': use_sim,
+        }.items(),
+        condition=IfCondition(use_planning),
+    )
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare('iiwa_description'),
+            'config',
+            'iiwa_controllers.yaml',
+        ]
+    )
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare('iiwa_description'), 'rviz', 'iiwa.rviz']
+    )
+
+    control_node = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[robot_description, robot_controllers],
+        output='both',
+        condition=UnlessCondition(use_sim),
+    )
+    robot_state_pub_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='both',
+        parameters=[robot_description],
+    )
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='log',
+        arguments=['-d', rviz_config_file],
+        parameters=[
+            robot_description,
+        ],
+        condition=UnlessCondition(use_planning),
+    )
+    iiwa_simulation_world = PathJoinSubstitution(
+        [FindPackageShare('iiwa_description'),
+            'gazebo/worlds', 'empty.world']
+    )
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [PathJoinSubstitution(
+                [FindPackageShare('gazebo_ros'),
+                    'launch', 'gazebo.launch.py']
+            )]
+        ),
+        launch_arguments={'verbose': 'false', 'world': iiwa_simulation_world}.items(),
+        condition=IfCondition(use_sim),
+    )
+
+    spawn_entity = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        arguments=['-topic', ['robot_description'], '-entity', ['iiwa14']],
+        output='screen',
+        condition=IfCondition(use_sim),
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager',
+                   ['controller_manager']],
+    )
+
+    external_torque_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['ets_state_broadcaster', '--controller-manager',
+                   ['controller_manager']],
+        condition=UnlessCondition(use_sim),
+    )
+
+    robot_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=[robot_controller, '--controller-manager', ['controller_manager']],
+    )
+
+    # Delay `joint_state_broadcaster` after spawn_entity
+    delay_joint_state_broadcaster_spawner_after_spawn_entity = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=spawn_entity,
+            on_exit=[joint_state_broadcaster_spawner],
+        ),
+        condition=IfCondition(use_sim),
+    )
+
+    # Delay `joint_state_broadcaster` after control_node
+    delay_joint_state_broadcaster_spawner_after_control_node = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=control_node,
+            on_start=[joint_state_broadcaster_spawner],
+        ),
+        condition=UnlessCondition(use_sim),
+    )
+
+    # Delay rviz start after `joint_state_broadcaster`
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[rviz_node],
+        ),
+        condition=IfCondition(start_rviz),
+    )
+
+    # Delay start of robot_controller after `joint_state_broadcaster`
+    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[robot_controller_spawner],
+        )
+    )
+
+    nodes = [
+        gazebo,
+        control_node,
+        iiwa_planning_launch,
+        spawn_entity,
+        robot_state_pub_node,
+        delay_joint_state_broadcaster_spawner_after_control_node,
+        delay_joint_state_broadcaster_spawner_after_spawn_entity,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        external_torque_broadcaster_spawner,
+        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)