From 3a7bb07baf091795a1d5f374d58ab0bf3cdf7bde Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Wed, 20 Oct 2021 21:46:45 +0200
Subject: [PATCH] Add demo_gazebo.launch using franka_gazebo

This commit adds demo_gazebo.launch using the gazebo simulation of the franka_gazebo package
Additionally, the virtual_joint is removed from the move group definition.
---
 config/panda_arm.xacro        |  1 -
 config/ros_controllers.yaml   |  2 +-
 launch/demo_gazebo.launch     | 52 +++++++++++++++++++++++++++++++++++
 launch/ros_controllers.launch |  3 +-
 package.xml                   |  1 +
 5 files changed, 55 insertions(+), 4 deletions(-)
 create mode 100644 launch/demo_gazebo.launch

diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro
index c9eaddb..0904da3 100644
--- a/config/panda_arm.xacro
+++ b/config/panda_arm.xacro
@@ -7,7 +7,6 @@
         <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
         <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
         <group name="panda_arm">
-            <joint name="virtual_joint"/>
             <joint name="panda_joint1"/>
             <joint name="panda_joint2"/>
             <joint name="panda_joint3"/>
diff --git a/config/ros_controllers.yaml b/config/ros_controllers.yaml
index 6ad2d06..a4e9561 100644
--- a/config/ros_controllers.yaml
+++ b/config/ros_controllers.yaml
@@ -17,4 +17,4 @@ hardware_interface:
     - panda_joint6
     - panda_joint7
     - panda_finger_joint1
-  sim_control_mode: 1  # 0: position, 1: velocity
\ No newline at end of file
+  sim_control_mode: 1  # 0: position, 1: velocity
diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
new file mode 100644
index 0000000..d3096c7
--- /dev/null
+++ b/launch/demo_gazebo.launch
@@ -0,0 +1,52 @@
+<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"/>
+  <arg name="load_gripper" default="true" />
+
+  <!-- Gazebo specific options -->
+  <arg name="gazebo_gui" default="true"/>
+  <arg name="paused" default="false"/>
+
+  <!-- launch the gazebo simulator and spawn the robot -->
+  <include file="$(find franka_gazebo)/launch/panda.launch">
+    <arg unless="$(arg gazebo_gui)" name="headless" value="true"/>
+    <arg     if="$(arg gazebo_gui)" name="headless" value="false"/>
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_gripper" default="$(arg load_gripper)"/>
+    <arg name="transmission" value="hardware_interface/PositionJointInterface"/>
+  </include>
+
+  <!-- Load position_joint_trajectory_controller -->
+  <include ns="panda" file="$(find panda_moveit_config)/launch/ros_controllers.launch"/>
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include ns="panda" file="$(find panda_moveit_config)/launch/move_group.launch">
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="fake_execution" value="false"/>
+    <arg name="load_gripper" value="$(arg load_gripper)"/>
+    <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 ns="panda" 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/ros_controllers.launch b/launch/ros_controllers.launch
index 590c9c9..bb223d3 100644
--- a/launch/ros_controllers.launch
+++ b/launch/ros_controllers.launch
@@ -5,7 +5,6 @@
   <rosparam file="$(find panda_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="position_joint_trajectory_controller"/>
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="position_joint_trajectory_controller"/>
 
 </launch>
diff --git a/package.xml b/package.xml
index ad49ce7..cff60c7 100644
--- a/package.xml
+++ b/package.xml
@@ -34,6 +34,7 @@
       We don't include them by default to prevent installing gazebo and all its dependencies. -->
   <!-- <run_depend>joint_trajectory_controller</run_depend> -->
   <!-- <run_depend>gazebo_ros_control</run_depend> -->
+  <!-- <run_depend>franka_gazebo</run_depend> -->
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
     <run_depend>franka_description</run_depend>