From c74f1cbae4368e6ca2c5ba5d250996bd586bfd6d Mon Sep 17 00:00:00 2001
From: Ryosuke Tajima <ryosuke.tajima@opensource-robotics.tokyo.jp>
Date: Thu, 16 Aug 2018 14:56:06 +0900
Subject: [PATCH] Add nextage settings

---
 .../config/nextage_controllers.yaml           | 53 +++++++++++++++++++
 fake_joint_launch/launch/nextage.launch       | 37 +++++++++++++
 fake_joint_launch/package.xml                 |  1 +
 3 files changed, 91 insertions(+)
 create mode 100644 fake_joint_launch/config/nextage_controllers.yaml
 create mode 100644 fake_joint_launch/launch/nextage.launch

diff --git a/fake_joint_launch/config/nextage_controllers.yaml b/fake_joint_launch/config/nextage_controllers.yaml
new file mode 100644
index 0000000..c4eebc5
--- /dev/null
+++ b/fake_joint_launch/config/nextage_controllers.yaml
@@ -0,0 +1,53 @@
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+larm_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - LARM_JOINT0
+    - LARM_JOINT1
+    - LARM_JOINT2
+    - LARM_JOINT3
+    - LARM_JOINT4
+    - LARM_JOINT5
+
+rarm_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - RARM_JOINT0
+    - RARM_JOINT1
+    - RARM_JOINT2
+    - RARM_JOINT3
+    - RARM_JOINT4
+    - RARM_JOINT5
+
+torso_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - CHEST_JOINT0
+
+head_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - HEAD_JOINT0
+    - HEAD_JOINT1
+
+fullbody_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - CHEST_JOINT0
+    - HEAD_JOINT0
+    - HEAD_JOINT1
+    - RARM_JOINT0
+    - RARM_JOINT1
+    - RARM_JOINT2
+    - RARM_JOINT3
+    - RARM_JOINT4
+    - RARM_JOINT5
+    - LARM_JOINT0
+    - LARM_JOINT1
+    - LARM_JOINT2
+    - LARM_JOINT3
+    - LARM_JOINT4
+    - LARM_JOINT5
diff --git a/fake_joint_launch/launch/nextage.launch b/fake_joint_launch/launch/nextage.launch
new file mode 100644
index 0000000..b2d307b
--- /dev/null
+++ b/fake_joint_launch/launch/nextage.launch
@@ -0,0 +1,37 @@
+<launch>
+  <arg name="use_rviz" default="true"/>
+  <arg name="use_rqt" default="false"/>
+  
+  <!-- load robot description  -->
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find nextage_description)/urdf/NextageOpen.urdf'" />
+
+  <!-- fake_joint_driver_node -->
+  <node name="fake_joint_driver" pkg="fake_joint_driver" type="fake_joint_driver_node"/>
+  
+  <!-- robot_state_publisher -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+
+  <rosparam file="$(find fake_joint_launch)/config/nextage_controllers.yaml" />
+
+  <!-- Load and start joint trajecotory controller -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" args="joint_state_controller larm_controller rarm_controller torso_controller head_controller"/>
+
+  <!-- Launch rviz -->
+  <node if="$(arg use_rviz)"
+	name="rviz" pkg="rviz" type="rviz"
+	args="-d $(find fake_joint_launch)/launch/fake_joint_launch.rviz"/>
+
+  <!-- Launch rqt joint_trajectory_controller -->
+  <node if="$(arg use_rqt)"
+	name="rqt" pkg="rqt_gui" type="rqt_gui"
+	args="-s rqt_joint_trajectory_controller.joint_trajectory_controller.JointTrajectoryController"/>
+
+  <!-- Run tests -->
+  <test test-name="hztest" pkg="rostest" type="hztest">
+    <param name="topic" value="joint_states"/>
+    <param name="hz" value="50"/>
+    <param name="hzerror" value="10"/>
+    <param name="test_duration" value="5.0"/>  
+  </test>
+
+</launch>
diff --git a/fake_joint_launch/package.xml b/fake_joint_launch/package.xml
index 171ab10..ead6a1f 100644
--- a/fake_joint_launch/package.xml
+++ b/fake_joint_launch/package.xml
@@ -19,6 +19,7 @@
 
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>denso_ros_control</exec_depend>
+  <exec_depend>nextage_description</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rviz</exec_depend>
   <exec_depend>tra1_description</exec_depend>