Merge pull request #358 from k-okada/fake

add MoveItFakeControllerManager

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
+

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">

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>

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>