Refactor MoveIt Launch files (#162)

* Remove use of MoveIt_config_builder launchers Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai> Co-authored-by: Alex Moriarty <alex.moriarty@picknik.ai>
Kinovarobotics · Aug 2, 2023 · 62a602f · 62a602f
1 parent 97d94d0
commit 62a602f
Show file tree

Hide file tree

Showing 5 changed files with 371 additions and 54 deletions.
diff --git a/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro b/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro
@@ -20,6 +20,9 @@
     session_inactivity_timeout_ms:=6000
     connection_inactivity_timeout_ms:=2000
     use_internal_bus_gripper_comm:=false
+    gripper_joint_name
+    gripper_max_velocity:=100.0
+    gripper_max_force:=100.0
     use_fake_hardware:=false
     fake_sensor_commands:=false
     sim_gazebo:=false
@@ -46,7 +49,10 @@
       port_realtime="${port_realtime}"
       session_inactivity_timeout_ms="${session_inactivity_timeout_ms}"
       connection_inactivity_timeout_ms="${connection_inactivity_timeout_ms}"
-      use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}" />
+      use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"
+      gripper_max_velocity="${gripper_max_velocity}"
+      gripper_max_force="${gripper_max_force}"
+      gripper_joint_name="${gripper_joint_name}"/>
 
     <joint name="${prefix}base_joint" type="fixed">
       <xacro:insert_block name="origin" />

diff --git a/...x_moveit_config/kinova_gen3_6dof_robotiq_2f_85_moveit_config/config/ros2_controllers.yaml b/...x_moveit_config/kinova_gen3_6dof_robotiq_2f_85_moveit_config/config/ros2_controllers.yaml
@@ -8,7 +8,7 @@ controller_manager:
     joint_trajectory_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
-    streaming_controller:
+    twist_controller:
       type: picknik_twist_controller/PicknikTwistController
 
     robotiq_gripper_controller:
@@ -39,7 +39,7 @@ joint_trajectory_controller:
       stopped_velocity_tolerance: 0.0
       goal_time: 0.0
 
-streaming_controller:
+twist_controller:
   ros__parameters:
     joint: tcp
     interface_names:
@@ -53,5 +53,5 @@ streaming_controller:
 robotiq_gripper_controller:
   ros__parameters:
     default: true
-    joint: finger_joint
+    joint: robotiq_85_left_knuckle_joint
     allow_stalling: true
diff --git a/kortex_moveit_config/kinova_gen3_6dof_robotiq_2f_85_moveit_config/launch/robot.launch.py b/kortex_moveit_config/kinova_gen3_6dof_robotiq_2f_85_moveit_config/launch/robot.launch.py
@@ -12,11 +12,171 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
 
+from launch import LaunchDescription
 from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument
+from launch.actions import (
+    DeclareLaunchArgument,
+    OpaqueFunction,
+    RegisterEventHandler,
+)
+from launch.event_handlers import OnProcessExit
+from launch.conditions import IfCondition
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
 from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_demo_launch
+
+
+def launch_setup(context, *args, **kwargs):
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    gripper_max_velocity = LaunchConfiguration("gripper_max_velocity")
+    gripper_max_force = LaunchConfiguration("gripper_max_force")
+    launch_rviz = LaunchConfiguration("launch_rviz")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    use_internal_bus_gripper_comm = LaunchConfiguration("use_internal_bus_gripper_comm")
+
+    launch_arguments = {
+        "robot_ip": robot_ip,
+        "use_fake_hardware": use_fake_hardware,
+        "gripper": "robotiq_2f_85",
+        "gripper_joint_name": "robotiq_85_left_knuckle_joint",
+        "dof": "6",
+        "gripper_max_velocity": gripper_max_velocity,
+        "gripper_max_force": gripper_max_force,
+        "use_internal_bus_gripper_comm": use_internal_bus_gripper_comm,
+    }
+
+    moveit_config = (
+        MoveItConfigsBuilder("gen3", package_name="kinova_gen3_6dof_robotiq_2f_85_moveit_config")
+        .robot_description(mappings=launch_arguments)
+        .planning_pipelines(pipelines=["ompl"])
+        .to_moveit_configs()
+    )
+
+    moveit_config.moveit_cpp.update({"use_sim_time": use_sim_time.perform(context) == "true"})
+
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            moveit_config.to_dict(),
+        ],
+    )
+
+    # Static TF
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"],
+    )
+
+    # Publish TF
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[
+            moveit_config.robot_description,
+        ],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("kinova_gen3_6dof_robotiq_2f_85_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[moveit_config.to_dict(), ros2_controllers_path],
+        output="both",
+    )
+
+    robot_traj_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_trajectory_controller", "-c", "/controller_manager"],
+    )
+
+    robot_pos_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["twist_controller", "--inactive", "-c", "/controller_manager"],
+    )
+
+    robot_hand_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
+        # condition=IfCondition(use_internal_bus_gripper_comm),
+    )
+
+    fault_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["fault_controller", "-c", "/controller_manager"],
+    )
+
+    # rviz with moveit configuration
+    rviz_config_file = (
+        get_package_share_directory("kinova_gen3_6dof_robotiq_2f_85_moveit_config")
+        + "/config/moveit.rviz"
+    )
+    rviz_node = Node(
+        package="rviz2",
+        condition=IfCondition(launch_rviz),
+        executable="rviz2",
+        name="rviz2_moveit",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+        ],
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    # 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(launch_rviz),
+    )
+
+    nodes_to_start = [
+        ros2_control_node,
+        robot_state_publisher,
+        joint_state_broadcaster_spawner,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        robot_traj_controller_spawner,
+        robot_pos_controller_spawner,
+        robot_hand_controller_spawner,
+        fault_controller_spawner,
+        move_group_node,
+        static_tf,
+    ]
+
+    return nodes_to_start
 
 
 def generate_launch_description():
@@ -35,6 +195,28 @@ def generate_launch_description():
             description="Start robot with fake hardware mirroring command to its states.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gripper_max_velocity",
+            default_value="100.0",
+            description="Max velocity for gripper commands",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gripper_max_force",
+            default_value="100.0",
+            description="Max force for gripper commands",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_internal_bus_gripper_comm",
+            default_value="true",
+            description="Use arm's internal gripper connection",
+        )
+    )
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_sim_time",
@@ -43,22 +225,8 @@ def generate_launch_description():
         )
     )
 
-    # Initialize Arguments
-    robot_ip = LaunchConfiguration("robot_ip")
-    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
-
-    launch_arguments = {
-        "robot_ip": robot_ip,
-        "use_fake_hardware": use_fake_hardware,
-        "gripper": "robotiq_2f_85",
-        "dof": "6",
-    }
-
-    moveit_config = (
-        MoveItConfigsBuilder("gen3", package_name="kinova_gen3_6dof_robotiq_2f_85_moveit_config")
-        .robot_description(mappings=launch_arguments)
-        .to_moveit_configs()
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
     )
 
-    moveit_config.moveit_cpp.update({"use_sim_time": LaunchConfiguration("use_sim_time")})
-    return generate_demo_launch(moveit_config)
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
diff --git a/...x_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ros2_controllers.yaml b/...x_moveit_config/kinova_gen3_7dof_robotiq_2f_85_moveit_config/config/ros2_controllers.yaml
@@ -8,7 +8,7 @@ controller_manager:
     joint_trajectory_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
-    streaming_controller:
+    twist_controller:
       type: picknik_twist_controller/PicknikTwistController
 
     robotiq_gripper_controller:
@@ -40,7 +40,7 @@ joint_trajectory_controller:
       stopped_velocity_tolerance: 0.0
       goal_time: 0.0
 
-streaming_controller:
+twist_controller:
   ros__parameters:
     joint: tcp
     interface_names: