From ebe87b6bdf7633fade81eccf079254258d8e3205 Mon Sep 17 00:00:00 2001
From: Stephanie Eng <stephanie.eng@picknik.ai>
Date: Fri, 11 Mar 2022 16:27:39 -0500
Subject: [PATCH 1/6] Port of Fanuc launch file to ROS2

---
 .../config/chomp_planning.yaml                |   8 +
 .../config/fanuc.ros2_control.xacro           |  55 ++
 fanuc_moveit_config/config/fanuc.urdf.xacro   |  12 +
 .../config/fanuc_controllers.yaml             |  23 +
 .../config/fanuc_ros2_controllers.yaml        |  27 +
 .../config/initial_positions.yaml             |   8 +
 fanuc_moveit_config/config/ompl_planning.yaml |   9 +
 fanuc_moveit_config/launch/demo.launch.py     | 124 +++
 fanuc_moveit_config/launch/moveit.rviz        | 769 ++++--------------
 9 files changed, 432 insertions(+), 603 deletions(-)
 create mode 100644 fanuc_moveit_config/config/fanuc.ros2_control.xacro
 create mode 100644 fanuc_moveit_config/config/fanuc.urdf.xacro
 create mode 100644 fanuc_moveit_config/config/fanuc_controllers.yaml
 create mode 100644 fanuc_moveit_config/config/fanuc_ros2_controllers.yaml
 create mode 100644 fanuc_moveit_config/config/initial_positions.yaml
 create mode 100644 fanuc_moveit_config/launch/demo.launch.py

diff --git a/fanuc_moveit_config/config/chomp_planning.yaml b/fanuc_moveit_config/config/chomp_planning.yaml
index f98e1c53..96950bc6 100644
--- a/fanuc_moveit_config/config/chomp_planning.yaml
+++ b/fanuc_moveit_config/config/chomp_planning.yaml
@@ -1,3 +1,11 @@
+planning_plugin: chomp_interface/CHOMPPlanner
+request_adapters: >-
+  default_planner_request_adapters/AddTimeParameterization
+  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
 planning_time_limit: 10.0
 max_iterations: 200
 max_iterations_after_collision_free: 5
diff --git a/fanuc_moveit_config/config/fanuc.ros2_control.xacro b/fanuc_moveit_config/config/fanuc.ros2_control.xacro
new file mode 100644
index 00000000..f9c2c58b
--- /dev/null
+++ b/fanuc_moveit_config/config/fanuc.ros2_control.xacro
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="fanuc_ros2_control" params="name initial_positions_file">
+        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
+
+        <ros2_control name="${name}" type="system">
+            <hardware>
+                <plugin>fake_components/GenericSystem</plugin>
+            </hardware>
+            <joint name="joint_1">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_1']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_2">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_2']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_3">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_3']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_4">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_4']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_5">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_5']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_6">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_6']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+        </ros2_control>
+    </xacro:macro>
+</robot>
diff --git a/fanuc_moveit_config/config/fanuc.urdf.xacro b/fanuc_moveit_config/config/fanuc.urdf.xacro
new file mode 100644
index 00000000..c2077df3
--- /dev/null
+++ b/fanuc_moveit_config/config/fanuc.urdf.xacro
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fanuc">
+    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
+
+    <!-- Import panda urdf file -->
+    <xacro:include filename="$(find moveit_resources_fanuc_description)/urdf/fanuc.urdf" />
+
+    <!-- Import fanuc ros2_control description -->
+    <xacro:include filename="fanuc.ros2_control.xacro" />
+
+    <xacro:fanuc_ros2_control name="FanucFakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
+</robot>
diff --git a/fanuc_moveit_config/config/fanuc_controllers.yaml b/fanuc_moveit_config/config/fanuc_controllers.yaml
new file mode 100644
index 00000000..96e83ac3
--- /dev/null
+++ b/fanuc_moveit_config/config/fanuc_controllers.yaml
@@ -0,0 +1,23 @@
+# MoveIt uses this configuration for controller management
+trajectory_execution:
+  allowed_execution_duration_scaling: 1.2
+  allowed_goal_duration_margin: 0.5
+  allowed_start_tolerance: 0.01
+
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+  controller_names:
+    - fanuc_controller
+
+  fanuc_controller:
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
diff --git a/fanuc_moveit_config/config/fanuc_ros2_controllers.yaml b/fanuc_moveit_config/config/fanuc_ros2_controllers.yaml
new file mode 100644
index 00000000..5bd9a148
--- /dev/null
+++ b/fanuc_moveit_config/config/fanuc_ros2_controllers.yaml
@@ -0,0 +1,27 @@
+# This config file is used by ros2_control
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    fanuc_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+
+fanuc_controller:
+  ros__parameters:
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+
diff --git a/fanuc_moveit_config/config/initial_positions.yaml b/fanuc_moveit_config/config/initial_positions.yaml
new file mode 100644
index 00000000..84038bb3
--- /dev/null
+++ b/fanuc_moveit_config/config/initial_positions.yaml
@@ -0,0 +1,8 @@
+# Default initial positions for the panda arm's ros2_control fake system
+initial_positions:
+  joint_1: 0.0
+  joint_2: 0.0
+  joint_3: 0.0
+  joint_4: 0.0
+  joint_5: 0.0
+  joint_6: 0.0
\ No newline at end of file
diff --git a/fanuc_moveit_config/config/ompl_planning.yaml b/fanuc_moveit_config/config/ompl_planning.yaml
index 96d846b6..9076dd64 100644
--- a/fanuc_moveit_config/config/ompl_planning.yaml
+++ b/fanuc_moveit_config/config/ompl_planning.yaml
@@ -1,3 +1,12 @@
+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
 planner_configs:
   AnytimePathShortening:
     type: geometric::AnytimePathShortening
diff --git a/fanuc_moveit_config/launch/demo.launch.py b/fanuc_moveit_config/launch/demo.launch.py
new file mode 100644
index 00000000..f78e56dc
--- /dev/null
+++ b/fanuc_moveit_config/launch/demo.launch.py
@@ -0,0 +1,124 @@
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+from ament_index_python.packages import get_package_share_directory
+from moveit_configs_utils import MoveItConfigsBuilder
+
+def generate_launch_description():
+
+    # Command-line arguments
+    db_arg = DeclareLaunchArgument(
+        "db", default_value="False", description="Database flag"
+    )
+
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_fanuc")
+        .robot_description(file_path="config/fanuc.urdf.xacro")
+        .robot_description_semantic(file_path="config/fanuc.srdf")
+        .trajectory_execution(file_path="config/fanuc_controllers.yaml")
+        .to_moveit_configs()
+    )
+
+    # Start the actual move_group node/action server
+    run_move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            moveit_config.to_dict()
+        ],
+    )
+
+    # RViz
+    rviz_base = os.path.join(get_package_share_directory("moveit_resources_fanuc_moveit_config"), "launch")
+    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=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.planning_pipelines,
+            moveit_config.robot_description_kinematics,
+        ],
+    )
+
+    # 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("moveit_resources_fanuc_moveit_config"),
+        "config",
+        "fanuc_ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        output={
+            "stdout": "screen",
+            "stderr": "screen",
+        },
+    )
+
+    # Load controllers
+    load_controllers = []
+    for controller in ["fanuc_controller", "joint_state_broadcaster"]:
+        load_controllers += [
+            ExecuteProcess(
+                cmd=["ros2 run controller_manager spawner {}".format(controller)],
+                shell=True,
+                output="screen",
+            )
+        ]
+
+    # Warehouse mongodb server
+    db_config = LaunchConfiguration("db")
+    mongodb_server_node = Node(
+        package="warehouse_ros_mongo",
+        executable="mongo_wrapper_ros.py",
+        parameters=[
+            {"warehouse_port": 33829},
+            {"warehouse_host": "localhost"},
+            {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"},
+        ],
+        output="screen",
+        condition=IfCondition(db_config)
+    )
+
+    return LaunchDescription(
+        [
+            db_arg,
+            rviz_node,
+            static_tf,
+            robot_state_publisher,
+            run_move_group_node,
+            ros2_control_node,
+            mongodb_server_node,
+        ]
+        + load_controllers
+    )
diff --git a/fanuc_moveit_config/launch/moveit.rviz b/fanuc_moveit_config/launch/moveit.rviz
index d411f27b..50468619 100644
--- a/fanuc_moveit_config/launch/moveit.rviz
+++ b/fanuc_moveit_config/launch/moveit.rviz
@@ -1,15 +1,20 @@
 Panels:
-  - Class: rviz/Displays
-    Help Height: 84
+  - Class: rviz_common/Displays
+    Help Height: 78
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /MotionPlanning1
-      Splitter Ratio: 0.74256
-    Tree Height: 664
-  - Class: rviz/Help
-    Name: Help
-  - Class: rviz/Views
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 802
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
     Expanded:
       - /Current View1
     Name: Views
@@ -19,11 +24,11 @@ Visualization Manager:
   Displays:
     - Alpha: 0.5
       Cell Size: 1
-      Class: rviz/Grid
+      Class: rviz_default_plugins/Grid
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -35,655 +40,213 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Class: moveit_rviz_plugin/MotionPlanning
+    - Class: rviz_default_plugins/MarkerArray
       Enabled: true
-      MoveIt_Goal_Tolerance: 0
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Constraint_Aware_IK: true
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
-      Name: MotionPlanning
-      Planned Path:
-        Links:
-          base_bellow_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          double_stereo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_ir_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_rgb_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_prosilica_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_plate_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_tilt_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          laser_tilt_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          sensor_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          torso_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Loop Animation: true
-        Robot Alpha: 0.5
-        Show Robot Collision: false
-        Show Robot Visual: true
-        Show Trail: false
-        State Display Time: 0.05 s
-        Trajectory Topic: /display_planned_path
-      Planning Metrics:
-        Payload: 1
-        Show Joint Torques: false
-        Show Manipulability: false
-        Show Manipulability Index: false
-        Show Weight Limit: false
-      Planning Request:
-        Colliding Link Color: 255; 0; 0
-        Goal State Alpha: 1
-        Goal State Color: 250; 128; 0
-        Interactive Marker Size: 0
-        Joint Violation Color: 255; 0; 255
-        Planning Group: left_arm
-        Query Goal State: true
-        Query Start State: false
-        Show Workspace: false
-        Start State Alpha: 1
-        Start State Color: 0; 255; 0
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 100
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rviz_visual_tools
+      Value: true
+    - Class: moveit_rviz_plugin/Trajectory
+      Color Enabled: false
+      Enabled: true
+      Interrupt Display: false
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Loop Animation: false
+      Name: Trajectory
+      Robot Alpha: 0.5
+      Robot Color: 150; 50; 150
+      Robot Description: robot_description
+      Show Robot Collision: false
+      Show Robot Visual: true
+      Show Trail: false
+      State Display Time: 0.05 s
+      Trail Step Size: 1
+      Trajectory Topic: /display_planned_path
+      Value: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
       Planning Scene Topic: /monitored_planning_scene
       Robot Description: robot_description
       Scene Geometry:
-        Scene Alpha: 1
+        Scene Alpha: 0.8999999761581421
         Scene Color: 50; 230; 50
-        Scene Display Time: 0.2
+        Scene Display Time: 0.009999999776482582
         Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
       Scene Robot:
         Attached Body Color: 150; 50; 150
         Links:
-          base_bellow_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
           base_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          bl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          double_stereo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_ir_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_rgb_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_prosilica_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_plate_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_tilt_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_motor_accelerometer_link:
+          link_1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_gripper_palm_link:
+          link_2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_gripper_r_finger_link:
+          link_3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_gripper_r_finger_tip_link:
+          link_4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_shoulder_lift_link:
+          link_5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_shoulder_pan_link:
+          link_6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          l_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          laser_tilt_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          sensor_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          torso_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Robot Alpha: 0.5
-        Show Scene Robot: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Fixed Frame: base_link
+    Frame Rate: 30
   Name: root
   Tools:
-    - Class: rviz/Interact
+    - Class: rviz_default_plugins/Interact
       Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
   Value: true
   Views:
     Current:
-      Class: rviz/XYOrbit
-      Distance: 2.9965
+      Class: rviz_default_plugins/Orbit
+      Distance: 3.119211196899414
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
       Focal Point:
-        X: 0.113567
-        Y: 0.10592
-        Z: 2.23518e-07
+        X: 0.02386285550892353
+        Y: 0.15478567779064178
+        Z: 0.039489321410655975
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.509797
-      Target Frame: /base_link
-      Value: XYOrbit (rviz)
-      Yaw: 5.65995
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5953981876373291
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.958578109741211
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1337
-  Help:
-    collapsed: false
+  Height: 1025
   Hide Left Dock: false
-  Hide Right Dock: false
-  Motion Planning:
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003abfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003ab000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005e1000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Views:
+  Tool Properties:
+    collapsed: false
+  Trajectory - Trajectory Slider:
     collapsed: false
-  Width: 1828
-  X: 459
-  Y: -243
+  Views:
+    collapsed: true
+  Width: 1853
+  X: 67
+  Y: 27

From f8856f941bb8c60a5b5c43b4ff6a7c4df4b99985 Mon Sep 17 00:00:00 2001
From: Stephanie Eng <stephanie.eng@picknik.ai>
Date: Fri, 11 Mar 2022 16:46:23 -0500
Subject: [PATCH 2/6] Cleanup of old XML files

---
 .../config/fake_controllers.yaml              | 12 ---
 .../config/fanuc_ros2_controllers.yaml        |  1 -
 .../config/initial_positions.yaml             |  4 +-
 .../config/ros_controllers.yaml               | 35 --------
 .../launch/chomp_planning_pipeline.launch.xml | 10 ---
 .../launch/default_warehouse_db.launch        | 15 ----
 fanuc_moveit_config/launch/demo.launch        | 62 --------------
 fanuc_moveit_config/launch/demo_chomp.launch  | 48 -----------
 fanuc_moveit_config/launch/demo_gazebo.launch | 70 ----------------
 .../fake_moveit_controller_manager.launch.xml |  9 --
 ...fanuc_moveit_controller_manager.launch.xml | 10 ---
 .../fanuc_moveit_sensor_manager.launch.xml    |  3 -
 fanuc_moveit_config/launch/gazebo.launch      | 23 ------
 .../launch/joystick_control.launch            | 17 ----
 fanuc_moveit_config/launch/move_group.launch  | 82 -------------------
 fanuc_moveit_config/launch/moveit_rviz.launch | 15 ----
 .../launch/ompl_planning_pipeline.launch.xml  | 25 ------
 .../launch/planning_context.launch            | 25 ------
 .../launch/planning_pipeline.launch.xml       | 10 ---
 .../launch/ros_controllers.launch             | 11 ---
 .../launch/run_benchmark_ompl.launch          | 21 -----
 .../launch/sensor_manager.launch.xml          | 17 ----
 .../launch/setup_assistant.launch             | 15 ----
 .../launch/test_environment.launch            |  6 --
 .../launch/trajectory_execution.launch.xml    | 20 -----
 fanuc_moveit_config/launch/warehouse.launch   | 15 ----
 .../launch/warehouse_settings.launch.xml      | 16 ----
 27 files changed, 2 insertions(+), 595 deletions(-)
 delete mode 100644 fanuc_moveit_config/config/fake_controllers.yaml
 delete mode 100644 fanuc_moveit_config/config/ros_controllers.yaml
 delete mode 100644 fanuc_moveit_config/launch/chomp_planning_pipeline.launch.xml
 delete mode 100644 fanuc_moveit_config/launch/default_warehouse_db.launch
 delete mode 100644 fanuc_moveit_config/launch/demo.launch
 delete mode 100644 fanuc_moveit_config/launch/demo_chomp.launch
 delete mode 100644 fanuc_moveit_config/launch/demo_gazebo.launch
 delete mode 100644 fanuc_moveit_config/launch/fake_moveit_controller_manager.launch.xml
 delete mode 100644 fanuc_moveit_config/launch/fanuc_moveit_controller_manager.launch.xml
 delete mode 100644 fanuc_moveit_config/launch/fanuc_moveit_sensor_manager.launch.xml
 delete mode 100644 fanuc_moveit_config/launch/gazebo.launch
 delete mode 100644 fanuc_moveit_config/launch/joystick_control.launch
 delete mode 100644 fanuc_moveit_config/launch/move_group.launch
 delete mode 100644 fanuc_moveit_config/launch/moveit_rviz.launch
 delete mode 100644 fanuc_moveit_config/launch/ompl_planning_pipeline.launch.xml
 delete mode 100644 fanuc_moveit_config/launch/planning_context.launch
 delete mode 100644 fanuc_moveit_config/launch/planning_pipeline.launch.xml
 delete mode 100644 fanuc_moveit_config/launch/ros_controllers.launch
 delete mode 100644 fanuc_moveit_config/launch/run_benchmark_ompl.launch
 delete mode 100644 fanuc_moveit_config/launch/sensor_manager.launch.xml
 delete mode 100644 fanuc_moveit_config/launch/setup_assistant.launch
 delete mode 100644 fanuc_moveit_config/launch/test_environment.launch
 delete mode 100644 fanuc_moveit_config/launch/trajectory_execution.launch.xml
 delete mode 100644 fanuc_moveit_config/launch/warehouse.launch
 delete mode 100644 fanuc_moveit_config/launch/warehouse_settings.launch.xml

diff --git a/fanuc_moveit_config/config/fake_controllers.yaml b/fanuc_moveit_config/config/fake_controllers.yaml
deleted file mode 100644
index 19cd38ef..00000000
--- a/fanuc_moveit_config/config/fake_controllers.yaml
+++ /dev/null
@@ -1,12 +0,0 @@
-controller_list:
-  - name: fake_manipulator_controller
-    joints:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
-initial:  # Define initial robot poses.
-  - group: manipulator
-    pose: all-zeros
diff --git a/fanuc_moveit_config/config/fanuc_ros2_controllers.yaml b/fanuc_moveit_config/config/fanuc_ros2_controllers.yaml
index 5bd9a148..f70562e6 100644
--- a/fanuc_moveit_config/config/fanuc_ros2_controllers.yaml
+++ b/fanuc_moveit_config/config/fanuc_ros2_controllers.yaml
@@ -24,4 +24,3 @@ fanuc_controller:
       - joint_4
       - joint_5
       - joint_6
-
diff --git a/fanuc_moveit_config/config/initial_positions.yaml b/fanuc_moveit_config/config/initial_positions.yaml
index 84038bb3..842abde6 100644
--- a/fanuc_moveit_config/config/initial_positions.yaml
+++ b/fanuc_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
-# Default initial positions for the panda arm's ros2_control fake system
+# Default initial positions for the FANUC arm's ros2_control fake system
 initial_positions:
   joint_1: 0.0
   joint_2: 0.0
   joint_3: 0.0
   joint_4: 0.0
   joint_5: 0.0
-  joint_6: 0.0
\ No newline at end of file
+  joint_6: 0.0
diff --git a/fanuc_moveit_config/config/ros_controllers.yaml b/fanuc_moveit_config/config/ros_controllers.yaml
deleted file mode 100644
index 1806f48d..00000000
--- a/fanuc_moveit_config/config/ros_controllers.yaml
+++ /dev/null
@@ -1,35 +0,0 @@
-# Simulation settings for using moveit_sim_controllers
-moveit_sim_hw_interface:
-  joint_model_group: manipulator
-  joint_model_group_pose: all-zeros
-# Settings for ros_control_boilerplate control loop
-generic_hw_control_loop:
-  loop_hz: 300
-  cycle_time_error_threshold: 0.01
-# Settings for ros_control hardware interface
-hardware_interface:
-  joints:
-    - joint_1
-    - joint_2
-    - joint_3
-    - joint_4
-    - joint_5
-    - joint_6
-  sim_control_mode: 1  # 0: position, 1: velocity
-# Publish all joint states
-# Creates the /joint_states topic necessary in ROS
-joint_state_broadcaster:
-  type: joint_state_broadcaster/JointStateBroadcaster
-  publish_rate: 50
-controller_list:
-  - name: manipulator_controller
-    action_ns: follow_joint_trajectory
-    default: True
-    type: FollowJointTrajectory
-    joints:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
diff --git a/fanuc_moveit_config/launch/chomp_planning_pipeline.launch.xml b/fanuc_moveit_config/launch/chomp_planning_pipeline.launch.xml
deleted file mode 100644
index d0e8dd37..00000000
--- a/fanuc_moveit_config/launch/chomp_planning_pipeline.launch.xml
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-   <!-- CHOMP Plugin for MoveIt -->
-   <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
-   <arg name="start_state_max_bounds_error" value="0.1" />
-
-   <param name="planning_plugin" value="$(arg planning_plugin)" />
-   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
-
-   <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/chomp_planning.yaml" />
-</launch>
diff --git a/fanuc_moveit_config/launch/default_warehouse_db.launch b/fanuc_moveit_config/launch/default_warehouse_db.launch
deleted file mode 100644
index e588849e..00000000
--- a/fanuc_moveit_config/launch/default_warehouse_db.launch
+++ /dev/null
@@ -1,15 +0,0 @@
-<launch>
-
-  <arg name="reset" default="false"/>
-  <!-- If not specified, we'll use a default database location -->
-  <arg name="moveit_warehouse_database_path" default="$(find moveit_resources_fanuc_moveit_config)/default_warehouse_mongo_db" />
-
-  <!-- Launch the warehouse with the configured database location -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/warehouse.launch">
-    <arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
-  </include>
-
-  <!-- If we want to reset the database, run this node -->
-  <node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />
-
-</launch>
diff --git a/fanuc_moveit_config/launch/demo.launch b/fanuc_moveit_config/launch/demo.launch
deleted file mode 100644
index d577b415..00000000
--- a/fanuc_moveit_config/launch/demo.launch
+++ /dev/null
@@ -1,62 +0,0 @@
-<launch>
-
-  <!-- specify the planning pipeline -->
-  <arg name="pipeline" default="ompl" />
-
-  <!-- By default, we do not start a database (it can be large) -->
-  <arg name="db" default="false" />
-  <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find moveit_resources_fanuc_moveit_config)/default_warehouse_mongo_db" />
-
-  <!-- By default, we are not in debug mode -->
-  <arg name="debug" default="false" />
-
-  <!--
-  By default, hide joint_state_publisher's GUI
-
-  MoveIt's "demo" mode replaces the real robot driver with the joint_state_publisher.
-  The latter one maintains and publishes the current joint configuration of the simulated robot.
-  It also provides a GUI to move the simulated robot around "manually".
-  This corresponds to moving around the real robot without the use of MoveIt.
-  -->
-  <arg name="use_gui" default="false" />
-  <arg name="use_rviz" default="true" />
-
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-  
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="use_gui" value="$(arg use_gui)"/>
-    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
-  </node>
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-    <arg name="pipeline" value="$(arg pipeline)"/>
-  </include>
-
-  <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
-    <arg name="rviz_config" value="$(find moveit_resources_fanuc_moveit_config)/launch/moveit.rviz"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
-    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
-  </include>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/demo_chomp.launch b/fanuc_moveit_config/launch/demo_chomp.launch
deleted file mode 100644
index 797d3225..00000000
--- a/fanuc_moveit_config/launch/demo_chomp.launch
+++ /dev/null
@@ -1,48 +0,0 @@
-<launch>
-
-  <!-- By default, we do not start a database (it can be large) -->
-  <arg name="db" default="false" />
-  <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find moveit_resources_fanuc_moveit_config)/default_warehouse_mongo_db" />
-
-  <!-- By default, we are not in debug mode -->
-  <arg name="debug" default="false" />
-
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="false"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-    <arg name="planner" value="chomp"/>
-  </include>
-
-  <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/moveit_rviz.launch">
-    <arg name="config" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
-    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
-  </include>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/demo_gazebo.launch b/fanuc_moveit_config/launch/demo_gazebo.launch
deleted file mode 100644
index f9d704d1..00000000
--- a/fanuc_moveit_config/launch/demo_gazebo.launch
+++ /dev/null
@@ -1,70 +0,0 @@
-<launch>
-
-  <!-- By default, we do not start a database (it can be large) -->
-  <arg name="db" default="false" />
-  <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find moveit_resources_fanuc_moveit_config)/default_warehouse_mongo_db" />
-
-  <!-- By default, we are not in debug mode -->
-  <arg name="debug" default="false" />
-
-  <!--
-  By default, hide joint_state_publisher's GUI
-
-  MoveIt's "demo" mode replaces the real robot driver with the joint_state_publisher.
-  The latter one maintains and publishes the current joint configuration of the simulated robot.
-  It also provides a GUI to move the simulated robot around "manually".
-  This corresponds to moving around the real robot without the use of MoveIt.
-  -->
-  <arg name="use_gui" default="false" />
-
-  <!-- Gazebo specific options -->
-  <arg name="gazebo_gui" default="true"/>
-  <arg name="paused" default="false"/>
-  <!-- By default, use the urdf location provided from the package -->
-  <arg name="urdf_path" default="$(find moveit_resources_fanuc_description)/urdf/fanuc.urdf"/>
-
-  <!-- launch the gazebo simulator and spawn the robot -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/gazebo.launch" >
-    <arg name="paused" value="$(arg paused)"/>
-    <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
-    <arg name="urdf_path" value="$(arg urdf_path)"/>
-  </include>
-
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="false"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-  
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="use_gui" value="$(arg use_gui)"/>
-    <rosparam param="source_list">[/joint_states]</rosparam>
-  </node>
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="false"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>  
-
-  <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/moveit_rviz.launch">
-    <arg name="rviz_config" value="$(find moveit_resources_fanuc_moveit_config)/launch/moveit.rviz"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
-    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
-  </include>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/fanuc_moveit_config/launch/fake_moveit_controller_manager.launch.xml
deleted file mode 100644
index 8a44cf5e..00000000
--- a/fanuc_moveit_config/launch/fake_moveit_controller_manager.launch.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<launch>
-
-  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
-  <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
-
-  <!-- The rest of the params are specific to this plugin -->
-  <rosparam file="$(find moveit_resources_fanuc_moveit_config)/config/fake_controllers.yaml"/>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/fanuc_moveit_controller_manager.launch.xml b/fanuc_moveit_config/launch/fanuc_moveit_controller_manager.launch.xml
deleted file mode 100644
index ac5e62be..00000000
--- a/fanuc_moveit_config/launch/fanuc_moveit_controller_manager.launch.xml
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-
-  <!-- loads moveit_controller_manager on the parameter server which is taken as argument 
-    if no argument is passed, moveit_simple_controller_manager will be set -->
-  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
-  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
-
-  <!-- loads ros_controllers to the param server -->
-  <rosparam file="$(find moveit_resources_fanuc_moveit_config)/config/ros_controllers.yaml"/>
-</launch>
diff --git a/fanuc_moveit_config/launch/fanuc_moveit_sensor_manager.launch.xml b/fanuc_moveit_config/launch/fanuc_moveit_sensor_manager.launch.xml
deleted file mode 100644
index 5d02698d..00000000
--- a/fanuc_moveit_config/launch/fanuc_moveit_sensor_manager.launch.xml
+++ /dev/null
@@ -1,3 +0,0 @@
-<launch>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/gazebo.launch b/fanuc_moveit_config/launch/gazebo.launch
deleted file mode 100644
index 85f9ff9f..00000000
--- a/fanuc_moveit_config/launch/gazebo.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <arg name="paused" default="false"/>
-  <arg name="gazebo_gui" default="true"/>
-  <arg name="urdf_path" default="$(find moveit_resources_fanuc_description)/urdf/fanuc.urdf"/>
-
-  <!-- startup simulated world -->
-  <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="world_name" default="worlds/empty.world"/>
-    <arg name="paused" value="$(arg paused)"/>
-    <arg name="gui" value="$(arg gazebo_gui)"/>
-  </include>
-
-  <!-- send robot urdf to param server -->
-  <param name="robot_description" textfile="$(arg urdf_path)" />
-
-  <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
-  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0"
-    respawn="false" output="screen" />
-
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/ros_controllers.launch"/>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/joystick_control.launch b/fanuc_moveit_config/launch/joystick_control.launch
deleted file mode 100644
index 9411f6e6..00000000
--- a/fanuc_moveit_config/launch/joystick_control.launch
+++ /dev/null
@@ -1,17 +0,0 @@
-<launch>
-  <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
-
-  <arg name="dev" default="/dev/input/js0" />
-
-  <!-- Launch joy node -->
-  <node pkg="joy" type="joy_node" name="joy">
-    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
-    <param name="deadzone" value="0.2" />
-    <param name="autorepeat_rate" value="40" />
-    <param name="coalesce_interval" value="0.025" />
-  </node>
-
-  <!-- Launch python interface -->
-  <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/move_group.launch b/fanuc_moveit_config/launch/move_group.launch
deleted file mode 100644
index b6b02dcd..00000000
--- a/fanuc_moveit_config/launch/move_group.launch
+++ /dev/null
@@ -1,82 +0,0 @@
-<launch>
-
-  <!-- GDB Debug Option -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix"
-	   value="gdb -x $(find moveit_resources_fanuc_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
-
-  <!-- Verbose Mode Option -->
-  <arg name="info" default="$(arg debug)" />
-  <arg unless="$(arg info)" name="command_args" value="" />
-  <arg     if="$(arg info)" name="command_args" value="--debug" />
-
-  <!-- move_group settings -->
-  <arg name="pipeline" default="ompl" />
-  <arg name="allow_trajectory_execution" default="true"/>
-  <arg name="fake_execution" default="false"/>
-  <arg name="max_safe_path_cost" default="1"/>
-  <arg name="jiggle_fraction" default="0.05" />
-  <arg name="publish_monitored_planning_scene" default="true"/>
-
-  <arg name="capabilities" default=""/>
-  <arg name="disable_capabilities" default=""/>
-  <!-- load these non-default MoveGroup capabilities (space seperated) -->
-  <!--
-  <arg name="capabilities" value="
-                a_package/AwsomeMotionPlanningCapability
-                another_package/GraspPlanningPipeline
-                " />
-  -->
-
-  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
-  <!--
-  <arg name="disable_capabilities" value="
-                move_group/MoveGroupKinematicsService
-                move_group/ClearOctomapService
-                " />
-  -->
-
-  <arg name="load_robot_description" default="true" />
-  <!-- load URDF, SRDF and joint_limits configuration -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="$(arg load_robot_description)" />
-  </include>
-
-  <!-- Planning Functionality -->
-  <include ns="move_group" file="$(find moveit_resources_fanuc_moveit_config)/launch/planning_pipeline.launch.xml">
-    <arg name="pipeline" value="$(arg pipeline)" />
-  </include>
-
-  <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(find moveit_resources_fanuc_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="fanuc" unless="$(arg fake_execution)"/>
-    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
-  </include>
-
-  <!-- Sensors Functionality -->
-  <include ns="move_group" file="$(find moveit_resources_fanuc_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_sensor_manager" value="fanuc" />
-  </include>
-
-  <!-- Start the actual move_group node/action server -->
-  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
-    <!-- Set the display variable, in case OpenGL code is used internally -->
-    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
-
-    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
-    <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
-    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-    <param name="capabilities" value="$(arg capabilities)"/>
-    <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
-
-
-    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
-    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
-  </node>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/moveit_rviz.launch b/fanuc_moveit_config/launch/moveit_rviz.launch
deleted file mode 100644
index 615b2f12..00000000
--- a/fanuc_moveit_config/launch/moveit_rviz.launch
+++ /dev/null
@@ -1,15 +0,0 @@
-<launch>
-
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
-
-  <arg name="rviz_config" default="" />
-  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
-  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
-
-  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
-	args="$(arg command_args)" output="screen">
-  </node>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/ompl_planning_pipeline.launch.xml b/fanuc_moveit_config/launch/ompl_planning_pipeline.launch.xml
deleted file mode 100644
index f1ffcc22..00000000
--- a/fanuc_moveit_config/launch/ompl_planning_pipeline.launch.xml
+++ /dev/null
@@ -1,25 +0,0 @@
-<launch>
-
-  <!-- OMPL Plugin for MoveIt -->
-  <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
-
-  <!-- The request adapters (plugins) used when planning with OMPL.
-       ORDER MATTERS -->
-  <arg name="planning_adapters"
-       value="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"
-              />
-
-  <arg name="start_state_max_bounds_error" value="0.1" />
-
-  <param name="planning_plugin" value="$(arg planning_plugin)" />
-  <param name="request_adapters" value="$(arg planning_adapters)" />
-  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
-
-  <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/ompl_planning.yaml"/>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/planning_context.launch b/fanuc_moveit_config/launch/planning_context.launch
deleted file mode 100644
index 6fa31b63..00000000
--- a/fanuc_moveit_config/launch/planning_context.launch
+++ /dev/null
@@ -1,25 +0,0 @@
-<launch>
-  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
-  <arg name="load_robot_description" default="false"/>
-
-  <!-- The name of the parameter under which the URDF is loaded -->
-  <arg name="robot_description" default="robot_description"/>
-
-  <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find moveit_resources_fanuc_description)/urdf/fanuc.urdf"/>
-
-  <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find moveit_resources_fanuc_moveit_config)/config/fanuc.srdf" />
-
-  <!-- Load updated joint limits (override information from URDF) -->
-  <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/joint_limits.yaml"/>
-  </group>
-
-  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
-  <group ns="$(arg robot_description)_kinematics">
-    <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/kinematics.yaml"/>
-
-  </group>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/planning_pipeline.launch.xml b/fanuc_moveit_config/launch/planning_pipeline.launch.xml
deleted file mode 100644
index 00a08b59..00000000
--- a/fanuc_moveit_config/launch/planning_pipeline.launch.xml
+++ /dev/null
@@ -1,10 +0,0 @@
-<launch>
-
-  <!-- This file makes it easy to include different planning pipelines;
-       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->
-
-  <arg name="pipeline" default="ompl" />
-
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
-
-</launch>
diff --git a/fanuc_moveit_config/launch/ros_controllers.launch b/fanuc_moveit_config/launch/ros_controllers.launch
deleted file mode 100644
index 46bf5ea4..00000000
--- a/fanuc_moveit_config/launch/ros_controllers.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-
-  <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam file="$(find moveit_resources_fanuc_moveit_config)/config/ros_controllers.yaml" command="load"/>
-
-  <!-- Load the controllers -->
-  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args=""/>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/run_benchmark_ompl.launch b/fanuc_moveit_config/launch/run_benchmark_ompl.launch
deleted file mode 100644
index ff71d3d8..00000000
--- a/fanuc_moveit_config/launch/run_benchmark_ompl.launch
+++ /dev/null
@@ -1,21 +0,0 @@
-<launch>
-
-  <!-- This argument must specify the list of .cfg files to process for benchmarking -->
-  <arg name="cfg" />
-
-  <!-- Load URDF -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- Start the database -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/warehouse.launch">
-    <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
-  </include>
-
-  <!-- Start Benchmark Executable -->
-  <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
-    <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/ompl_planning.yaml"/>
-  </node>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/sensor_manager.launch.xml b/fanuc_moveit_config/launch/sensor_manager.launch.xml
deleted file mode 100644
index efd02218..00000000
--- a/fanuc_moveit_config/launch/sensor_manager.launch.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-<launch>
-
-  <!-- This file makes it easy to include the settings for sensor managers -->
-
-  <!-- Params for 3D sensors config -->
-  <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/sensors_3d.yaml" />
-
-  <!-- Params for the octomap monitor -->
-  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
-  <param name="octomap_resolution" type="double" value="0.025" />
-  <param name="max_range" type="double" value="5.0" />
-
-  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
-  <arg name="moveit_sensor_manager" default="fanuc" />
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
-
-</launch>
diff --git a/fanuc_moveit_config/launch/setup_assistant.launch b/fanuc_moveit_config/launch/setup_assistant.launch
deleted file mode 100644
index dcda2688..00000000
--- a/fanuc_moveit_config/launch/setup_assistant.launch
+++ /dev/null
@@ -1,15 +0,0 @@
-<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
-<launch>
-
-  <!-- Debug Info -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
-
-  <!-- Run -->
-  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
-	args="--config_pkg=moveit_resources_fanuc_moveit_config"
-	launch-prefix="$(arg launch_prefix)"
-        output="screen" />
-
-</launch>
diff --git a/fanuc_moveit_config/launch/test_environment.launch b/fanuc_moveit_config/launch/test_environment.launch
deleted file mode 100644
index d1ca70d7..00000000
--- a/fanuc_moveit_config/launch/test_environment.launch
+++ /dev/null
@@ -1,6 +0,0 @@
-<launch>
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/demo.launch">
-    <arg name="use_gui" value="false"/>
-    <arg name="use_rviz" value="false"/>
-  </include>
-</launch>
diff --git a/fanuc_moveit_config/launch/trajectory_execution.launch.xml b/fanuc_moveit_config/launch/trajectory_execution.launch.xml
deleted file mode 100644
index 287bc75e..00000000
--- a/fanuc_moveit_config/launch/trajectory_execution.launch.xml
+++ /dev/null
@@ -1,20 +0,0 @@
-<launch>
-
-  <!-- This file makes it easy to include the settings for trajectory execution  -->
-
-  <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
-  <arg name="moveit_manage_controllers" default="true"/>
-  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
-
-  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
-  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
-  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
-  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
-  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
-  <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
-
-  <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
-  <arg name="moveit_controller_manager" default="fanuc" />
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
-
-</launch>
diff --git a/fanuc_moveit_config/launch/warehouse.launch b/fanuc_moveit_config/launch/warehouse.launch
deleted file mode 100644
index c4320522..00000000
--- a/fanuc_moveit_config/launch/warehouse.launch
+++ /dev/null
@@ -1,15 +0,0 @@
-<launch>
-
-  <!-- The path to the database must be specified -->
-  <arg name="moveit_warehouse_database_path" />
-
-  <!-- Load warehouse parameters -->
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/warehouse_settings.launch.xml" />
-
-  <!-- Run the DB server -->
-  <node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
-    <param name="overwrite" value="false"/>
-    <param name="database_path" value="$(arg moveit_warehouse_database_path)" />
-  </node>
-
-</launch>
diff --git a/fanuc_moveit_config/launch/warehouse_settings.launch.xml b/fanuc_moveit_config/launch/warehouse_settings.launch.xml
deleted file mode 100644
index e473b083..00000000
--- a/fanuc_moveit_config/launch/warehouse_settings.launch.xml
+++ /dev/null
@@ -1,16 +0,0 @@
-<launch>
-  <!-- Set the parameters for the warehouse and run the mongodb server. -->
-
-  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
-  <arg name="moveit_warehouse_port" default="33829" />
-
-  <!-- The default DB host for moveit -->
-  <arg name="moveit_warehouse_host" default="localhost" />
-
-  <!-- Set parameters for the warehouse -->
-  <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
-  <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
-  <param name="warehouse_exec" value="mongod" />
-  <param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
-
-</launch>

From c9bb46ab320beb0c4fec4656aa285a88ac998e1a Mon Sep 17 00:00:00 2001
From: Stephanie Eng <stephanie.eng@picknik.ai>
Date: Fri, 25 Mar 2022 11:56:51 -0400
Subject: [PATCH 3/6] Renamed controller files

---
 .../{fanuc_controllers.yaml => moveit_controllers.yaml}       | 0
 .../{fanuc_ros2_controllers.yaml => ros2_controllers.yaml}    | 0
 fanuc_moveit_config/launch/demo.launch.py                     | 4 ++--
 3 files changed, 2 insertions(+), 2 deletions(-)
 rename fanuc_moveit_config/config/{fanuc_controllers.yaml => moveit_controllers.yaml} (100%)
 rename fanuc_moveit_config/config/{fanuc_ros2_controllers.yaml => ros2_controllers.yaml} (100%)

diff --git a/fanuc_moveit_config/config/fanuc_controllers.yaml b/fanuc_moveit_config/config/moveit_controllers.yaml
similarity index 100%
rename from fanuc_moveit_config/config/fanuc_controllers.yaml
rename to fanuc_moveit_config/config/moveit_controllers.yaml
diff --git a/fanuc_moveit_config/config/fanuc_ros2_controllers.yaml b/fanuc_moveit_config/config/ros2_controllers.yaml
similarity index 100%
rename from fanuc_moveit_config/config/fanuc_ros2_controllers.yaml
rename to fanuc_moveit_config/config/ros2_controllers.yaml
diff --git a/fanuc_moveit_config/launch/demo.launch.py b/fanuc_moveit_config/launch/demo.launch.py
index f78e56dc..bca7d9a4 100644
--- a/fanuc_moveit_config/launch/demo.launch.py
+++ b/fanuc_moveit_config/launch/demo.launch.py
@@ -19,7 +19,7 @@ def generate_launch_description():
         MoveItConfigsBuilder("moveit_resources_fanuc")
         .robot_description(file_path="config/fanuc.urdf.xacro")
         .robot_description_semantic(file_path="config/fanuc.srdf")
-        .trajectory_execution(file_path="config/fanuc_controllers.yaml")
+        .trajectory_execution(file_path="config/moveit_controllers.yaml")
         .to_moveit_configs()
     )
 
@@ -73,7 +73,7 @@ def generate_launch_description():
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_fanuc_moveit_config"),
         "config",
-        "fanuc_ros2_controllers.yaml",
+        "ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",

From bc9c1594589659a9699bdc161eafdee4b7224532 Mon Sep 17 00:00:00 2001
From: Stephanie Eng <stephanie-eng@users.noreply.github.com>
Date: Fri, 1 Apr 2022 08:51:42 -0400
Subject: [PATCH 4/6] Update fanuc_moveit_config/launch/demo.launch.py

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>
---
 fanuc_moveit_config/launch/demo.launch.py | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/fanuc_moveit_config/launch/demo.launch.py b/fanuc_moveit_config/launch/demo.launch.py
index bca7d9a4..6ccd2504 100644
--- a/fanuc_moveit_config/launch/demo.launch.py
+++ b/fanuc_moveit_config/launch/demo.launch.py
@@ -79,10 +79,7 @@ def generate_launch_description():
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[moveit_config.robot_description, ros2_controllers_path],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
+        output="both",
     )
 
     # Load controllers

From 39acc8c90add6d3f8ae8f3a4ddec6045d69c395f Mon Sep 17 00:00:00 2001
From: Stephanie Eng <stephanie.eng@picknik.ai>
Date: Fri, 1 Apr 2022 09:22:13 -0400
Subject: [PATCH 5/6] Launch RViz with MotionPlanning panel loaded

---
 fanuc_moveit_config/launch/moveit.rviz | 200 ++++++++++++++++++++++++-
 1 file changed, 195 insertions(+), 5 deletions(-)

diff --git a/fanuc_moveit_config/launch/moveit.rviz b/fanuc_moveit_config/launch/moveit.rviz
index 50468619..f1f2802c 100644
--- a/fanuc_moveit_config/launch/moveit.rviz
+++ b/fanuc_moveit_config/launch/moveit.rviz
@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.5
-    Tree Height: 802
+    Tree Height: 423
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -62,6 +62,10 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         base_link:
           Alpha: 1
           Show Axes: false
@@ -97,6 +101,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        tool0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
       Loop Animation: false
       Name: Trajectory
       Robot Alpha: 0.5
@@ -130,6 +138,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           base_link:
             Alpha: 1
             Show Axes: false
@@ -165,10 +177,181 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
       Value: true
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 3x
+        Trail Step Size: 1
+        Trajectory Topic: /display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: manipulator
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.1
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -184,6 +367,9 @@ Visualization Manager:
     - Class: rviz_default_plugins/Measure
       Line color: 128; 128; 0
     - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -233,12 +419,16 @@ Visualization Manager:
       Yaw: 5.958578109741211
     Saved: ~
 Window Geometry:
+  "":
+    collapsed: false
+  " - Trajectory Slider":
+    collapsed: false
   Displays:
     collapsed: false
-  Height: 1025
+  Height: 1024
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000003abfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003ab000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005e1000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ca000003aafc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000230000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffbffffffff0100000271000001740000016900ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000574000003aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
@@ -247,6 +437,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1853
-  X: 67
+  Width: 1860
+  X: 2620
   Y: 27

From 04312eddd3810c3c66073811e637c1829893f816 Mon Sep 17 00:00:00 2001
From: Stephanie Eng <stephanie.eng@picknik.ai>
Date: Fri, 1 Apr 2022 09:25:10 -0400
Subject: [PATCH 6/6] Formatting

---
 fanuc_moveit_config/launch/demo.launch.py | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/fanuc_moveit_config/launch/demo.launch.py b/fanuc_moveit_config/launch/demo.launch.py
index 6ccd2504..e17d51bb 100644
--- a/fanuc_moveit_config/launch/demo.launch.py
+++ b/fanuc_moveit_config/launch/demo.launch.py
@@ -8,6 +8,7 @@
 from ament_index_python.packages import get_package_share_directory
 from moveit_configs_utils import MoveItConfigsBuilder
 
+
 def generate_launch_description():
 
     # Command-line arguments
@@ -28,13 +29,13 @@ def generate_launch_description():
         package="moveit_ros_move_group",
         executable="move_group",
         output="screen",
-        parameters=[
-            moveit_config.to_dict()
-        ],
+        parameters=[moveit_config.to_dict()],
     )
 
     # RViz
-    rviz_base = os.path.join(get_package_share_directory("moveit_resources_fanuc_moveit_config"), "launch")
+    rviz_base = os.path.join(
+        get_package_share_directory("moveit_resources_fanuc_moveit_config"), "launch"
+    )
     rviz_full_config = os.path.join(rviz_base, "moveit.rviz")
 
     rviz_node = Node(
@@ -104,7 +105,7 @@ def generate_launch_description():
             {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"},
         ],
         output="screen",
-        condition=IfCondition(db_config)
+        condition=IfCondition(db_config),
     )
 
     return LaunchDescription(