From 9618a69704ac40f265340313cf2ac335cfdf6590 Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Fri, 27 Aug 2021 14:29:23 +0200
Subject: [PATCH] Adds the panda_control_moveit_rviz launch file (#50)

This commit adds the panda_control_moveit_rviz launch file. This file can be used to control a real robot using MoveIt.
---
 config/panda_gripper_moveit_controllers.yaml  | 20 +++++
 config/panda_moveit_controllers.yaml          | 13 ++++
 config/ros_controllers.yaml                   | 55 +-------------
 launch/demo_gazebo.launch                     | 74 -------------------
 launch/gazebo.launch                          | 23 ------
 launch/move_group.launch                      |  3 +-
 launch/panda_control_moveit_rviz.launch       | 30 ++++++++
 ...ipper_moveit_controller_manager.launch.xml | 12 +++
 launch/panda_moveit.launch                    | 10 +++
 ...panda_moveit_controller_manager.launch.xml |  2 +-
 launch/ros_controllers.launch                 |  2 +-
 11 files changed, 90 insertions(+), 154 deletions(-)
 create mode 100644 config/panda_gripper_moveit_controllers.yaml
 create mode 100644 config/panda_moveit_controllers.yaml
 delete mode 100644 launch/demo_gazebo.launch
 delete mode 100644 launch/gazebo.launch
 create mode 100644 launch/panda_control_moveit_rviz.launch
 create mode 100644 launch/panda_gripper_moveit_controller_manager.launch.xml
 create mode 100644 launch/panda_moveit.launch

diff --git a/config/panda_gripper_moveit_controllers.yaml b/config/panda_gripper_moveit_controllers.yaml
new file mode 100644
index 0000000..8a6efb6
--- /dev/null
+++ b/config/panda_gripper_moveit_controllers.yaml
@@ -0,0 +1,20 @@
+controller_list:
+  - name: position_joint_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
+  - name: franka_gripper
+    action_ns: gripper_action
+    type: GripperCommand
+    default: true
+    joints:
+      - panda_finger_joint1
+      - panda_finger_joint2
diff --git a/config/panda_moveit_controllers.yaml b/config/panda_moveit_controllers.yaml
new file mode 100644
index 0000000..3cd123f
--- /dev/null
+++ b/config/panda_moveit_controllers.yaml
@@ -0,0 +1,13 @@
+controller_list:
+  - name: position_joint_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
diff --git a/config/ros_controllers.yaml b/config/ros_controllers.yaml
index a67b290..6ad2d06 100644
--- a/config/ros_controllers.yaml
+++ b/config/ros_controllers.yaml
@@ -17,57 +17,4 @@ hardware_interface:
     - panda_joint6
     - panda_joint7
     - panda_finger_joint1
-  sim_control_mode: 1  # 0: position, 1: velocity
-# Publish all joint states
-# Creates the /joint_states topic necessary in ROS
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-controller_list:
-  []
-position_joint_trajectory_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - panda_joint1
-    - panda_joint2
-    - panda_joint3
-    - panda_joint4
-    - panda_joint5
-    - panda_joint6
-    - panda_joint7
-  gains:
-    panda_joint1:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint2:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint3:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint4:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint5:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint6:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
-    panda_joint7:
-      p: 100
-      d: 1
-      i: 1
-      i_clamp: 1
\ No newline at end of file
+  sim_control_mode: 1  # 0: position, 1: velocity
\ No newline at end of file
diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
deleted file mode 100644
index caae7cd..0000000
--- a/launch/demo_gazebo.launch
+++ /dev/null
@@ -1,74 +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 panda_moveit_config)/default_warehouse_mongo_db" />
-
-  <!-- By default, we are not in debug mode -->
-  <arg name="debug" default="false" />
-
-  <!-- By default, we won't load or override the robot_description -->
-  <arg name="load_robot_description" 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 franka_description)/robots/panda_arm.urdf.xacro"/>
-
-  <!-- launch the gazebo simulator and spawn the robot -->
-  <include file="$(dirname)/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>
-
-  <!-- If needed, broadcast static tf for robot root -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0" />
-
-
-  <!-- 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" unless="$(arg use_gui)">
-    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
-    <rosparam param="source_list">[/joint_states]</rosparam>
-  </node>
-  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
-    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
-    <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="$(dirname)/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)"/>
-    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
-  </include>
-
-  <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(dirname)/moveit_rviz.launch">
-    <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
-    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
-  </include>
-
-</launch>
diff --git a/launch/gazebo.launch b/launch/gazebo.launch
deleted file mode 100644
index 16adbad..0000000
--- a/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 franka_description)/robots/panda_arm.urdf.xacro"/>
-
-  <!-- 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="$(dirname)/ros_controllers.launch"/>
-
-</launch>
diff --git a/launch/move_group.launch b/launch/move_group.launch
index 18da573..f746839 100644
--- a/launch/move_group.launch
+++ b/launch/move_group.launch
@@ -76,7 +76,8 @@
   <!-- Trajectory Execution Functionality -->
   <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="panda" unless="$(arg fake_execution)"/>
+    <arg name="moveit_controller_manager" value="panda" if="$(eval not arg('fake_execution') and not arg('load_gripper'))"/>
+    <arg name="moveit_controller_manager" value="panda_gripper" if="$(eval not arg('fake_execution') and arg('load_gripper'))"/>
     <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
     <arg name="execution_type" value="$(arg execution_type)" />
   </include>
diff --git a/launch/panda_control_moveit_rviz.launch b/launch/panda_control_moveit_rviz.launch
new file mode 100644
index 0000000..ce41e83
--- /dev/null
+++ b/launch/panda_control_moveit_rviz.launch
@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<!--
+  Used to control a real robot using moveit
+-->
+<launch>
+  <arg name="load_gripper" default="true" />
+  <arg name="launch_rviz" default="true" />
+  <!--
+    By default we load the franka controllers.Disable this option if the controllers are
+    running on a seperate workstation
+  -->
+  <arg name="launch_franka_control" default="true" />
+  <group if="$(arg launch_franka_control)">
+    <arg name="robot_ip" />
+  </group>
+
+  <!-- If needed, broadcast static tf for robot root -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
+
+  <include if="$(arg launch_franka_control)" file="$(find franka_control)/launch/franka_control.launch">
+    <arg name="robot_ip" value="$(arg robot_ip)" />
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+  </include>
+
+  <include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+  </include>
+
+  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg launch_rviz)"/>
+</launch>
diff --git a/launch/panda_gripper_moveit_controller_manager.launch.xml b/launch/panda_gripper_moveit_controller_manager.launch.xml
new file mode 100644
index 0000000..e275653
--- /dev/null
+++ b/launch/panda_gripper_moveit_controller_manager.launch.xml
@@ -0,0 +1,12 @@
+<launch>
+
+  <!-- this argument is not used here, only necessary so that this launch file has the same args as fake_moveit_controller_manager.launch -->
+  <arg name="execution_type" default="unused" />
+
+  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
+  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
+  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
+
+  <!-- load controller_list -->
+  <rosparam file="$(find panda_moveit_config)/config/panda_gripper_moveit_controllers.yaml"/>
+</launch>
diff --git a/launch/panda_moveit.launch b/launch/panda_moveit.launch
new file mode 100644
index 0000000..3010bd7
--- /dev/null
+++ b/launch/panda_moveit.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="load_gripper" default="true" />
+
+  <include file="$(dirname)/ros_controllers.launch"/>
+
+  <include file="$(find panda_moveit_config)/launch/move_group.launch">
+    <arg name="load_gripper" value="$(arg load_gripper)" />
+  </include>
+</launch>
diff --git a/launch/panda_moveit_controller_manager.launch.xml b/launch/panda_moveit_controller_manager.launch.xml
index febf584..f1123a7 100644
--- a/launch/panda_moveit_controller_manager.launch.xml
+++ b/launch/panda_moveit_controller_manager.launch.xml
@@ -9,5 +9,5 @@
   <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 
   <!-- loads ros_controllers to the param server -->
-  <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml"/>
+  <rosparam file="$(find panda_moveit_config)/config/panda_moveit_controllers.yaml"/>
 </launch>
diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch
index 85f011b..590c9c9 100644
--- a/launch/ros_controllers.launch
+++ b/launch/ros_controllers.launch
@@ -6,6 +6,6 @@
 
   <!-- Load the controllers -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args="position_joint_trajectory_controller "/>
+    output="screen" args="position_joint_trajectory_controller"/>
 
 </launch>