From 006dfd0cbe7df367a530e3d64deaa069c0301304 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Thu, 31 May 2018 22:07:58 +0900
Subject: [PATCH] add MoveItFakeControllerManager

---
 .../config/fake_controllers.yaml              | 34 +++++++++++++++++++
 nextage_moveit_config/launch/demo.launch      |  6 +++-
 .../fake_moveit_controller_manager.launch     |  9 +++++
 .../launch/move_group.launch                  |  4 ++-
 4 files changed, 51 insertions(+), 2 deletions(-)
 create mode 100644 nextage_moveit_config/config/fake_controllers.yaml
 create mode 100644 nextage_moveit_config/launch/fake_moveit_controller_manager.launch

diff --git a/nextage_moveit_config/config/fake_controllers.yaml b/nextage_moveit_config/config/fake_controllers.yaml
new file mode 100644
index 00000000..355fb212
--- /dev/null
+++ b/nextage_moveit_config/config/fake_controllers.yaml
@@ -0,0 +1,34 @@
+controller_list:
+  - name: fake_rarm_controller
+    joints:
+      - RARM_JOINT0
+      - RARM_JOINT1
+      - RARM_JOINT2
+      - RARM_JOINT3
+      - RARM_JOINT4
+      - RARM_JOINT5
+    initial: [0, 0, 1, 0, 0, 0]      
+  - name: fake_larm_controller
+    joints:
+      - LARM_JOINT0
+      - LARM_JOINT1
+      - LARM_JOINT2
+      - LARM_JOINT3
+      - LARM_JOINT4
+      - LARM_JOINT5
+    initial: [0, 0, 1, 0, 0, 0]
+  - name: fake_head_controller
+    joints:
+      - HEAD_JOINT0
+      - HEAD_JOINT0
+    initial: [0, 0]
+  - name: fake_torso_controller
+    joints:
+      - CHEST_JOINT0
+    initial: [0]
+initial:
+  - group: right_arm
+    pose : right_arm_initial_pose
+  - group: left_arm
+    pose : left_arm_initial_pose
+    
\ No newline at end of file
diff --git a/nextage_moveit_config/launch/demo.launch b/nextage_moveit_config/launch/demo.launch
index 9da39134..2fb5d0b4 100644
--- a/nextage_moveit_config/launch/demo.launch
+++ b/nextage_moveit_config/launch/demo.launch
@@ -4,14 +4,18 @@
   </include>
 
   <node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world WAIST 100" />
+
+  <!-- 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>
   
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
   <include file="$(find nextage_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="false"/>
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="fake_execution" value="true"/>
   </include>
 
   <include file="$(find nextage_moveit_config)/launch/moveit_rviz.launch">
diff --git a/nextage_moveit_config/launch/fake_moveit_controller_manager.launch b/nextage_moveit_config/launch/fake_moveit_controller_manager.launch
new file mode 100644
index 00000000..37529323
--- /dev/null
+++ b/nextage_moveit_config/launch/fake_moveit_controller_manager.launch
@@ -0,0 +1,9 @@
+<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 nextage_moveit_config)/config/fake_controllers.yaml"/>
+
+</launch>
diff --git a/nextage_moveit_config/launch/move_group.launch b/nextage_moveit_config/launch/move_group.launch
index 42f19518..f1242444 100644
--- a/nextage_moveit_config/launch/move_group.launch
+++ b/nextage_moveit_config/launch/move_group.launch
@@ -11,6 +11,7 @@
   <arg     if="$(arg info)" name="command_args" value="--debug" />
 
   <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"/>
@@ -22,7 +23,8 @@
   </include>
 
   <include ns="move_group" file="$(find nextage_moveit_config)/launch/trajectory_execution.launch" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_controller_manager" value="NextageOpen" /> 
+    <arg name="moveit_controller_manager" value="NextageOpen" unless="$(arg fake_execution)"/>
+    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
     <arg name="moveit_manage_controllers" value="true" />
   </include>