From 7f6dc62f17987cab9bc709dcfecab7f1451813ee Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Mon, 8 Jan 2024 15:31:34 +0900
Subject: [PATCH] Add

---
 .github/workflows/test.yml                    |  3 -
 ros/kxr_controller/test/kxr_controller.test   |  9 +++
 .../config/kxr_test_head_servo_config.yaml    |  4 +
 ros/kxr_models/urdf/kxr_test_head.urdf        | 77 +++++++++++++++++++
 ros/kxreus/CATKIN_IGNORE                      |  0
 ros/kxreus/CMakeLists.txt                     |  2 +-
 6 files changed, 91 insertions(+), 4 deletions(-)
 create mode 100644 ros/kxr_controller/test/kxr_controller.test
 create mode 100644 ros/kxr_models/config/kxr_test_head_servo_config.yaml
 create mode 100644 ros/kxr_models/urdf/kxr_test_head.urdf
 delete mode 100644 ros/kxreus/CATKIN_IGNORE

diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml
index 850e6182..d3346338 100644
--- a/.github/workflows/test.yml
+++ b/.github/workflows/test.yml
@@ -73,10 +73,7 @@ jobs:
         env:
           - {ROS_DISTRO: noetic, ROS_REPO: main}
         os: [self-hosted]
-        python-version: [3.7]
     steps:
       - uses: actions/checkout@v3
       - uses: 'iory/industrial_ci@master'
         env: ${{matrix.env}}
-      - name: Check Python Version
-        run: python --version
diff --git a/ros/kxr_controller/test/kxr_controller.test b/ros/kxr_controller/test/kxr_controller.test
new file mode 100644
index 00000000..9aac0eba
--- /dev/null
+++ b/ros/kxr_controller/test/kxr_controller.test
@@ -0,0 +1,9 @@
+<launch>
+
+  <include file="$(find kxr_controller)/launch/kxr_controller.launch">
+    <arg name="urdf_path" value="$(find kxr_models)/urdf/kxr_test_head.urdf" />
+    <arg name="servo_config_path" value="$(find kxr_models)/config/kxr_test_head_servo_config.yaml" />
+    <arg name="namespace" value="robot_a" />
+  </include>
+
+</launch>
diff --git a/ros/kxr_models/config/kxr_test_head_servo_config.yaml b/ros/kxr_models/config/kxr_test_head_servo_config.yaml
new file mode 100644
index 00000000..b0c7447c
--- /dev/null
+++ b/ros/kxr_models/config/kxr_test_head_servo_config.yaml
@@ -0,0 +1,4 @@
+joint_name_to_servo_id: {
+  HEAD_JOINT0: 32,
+  HEAD_JOINT1: 34
+}
diff --git a/ros/kxr_models/urdf/kxr_test_head.urdf b/ros/kxr_models/urdf/kxr_test_head.urdf
new file mode 100644
index 00000000..680428d5
--- /dev/null
+++ b/ros/kxr_models/urdf/kxr_test_head.urdf
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot name="kxrl2l2a6h2m"
+       xmlns:xi="http://www.w3.org/2001/XInclude">
+  <link name="bodyset94472077639384">
+    <visual>
+      <origin xyz="0 0 0.1205" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="package://kxr_models/meshes/kxrl2l2a6h2m/bodyset94472077639384_mesh.dae" scale="1 1 1" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.1205" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="package://kxr_models/meshes/kxrl2l2a6h2m/bodyset94472077639384_mesh.dae" scale="1 1 1" />
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.437124" />
+      <origin xyz="-0.005319 2.13443e-10 0.029207" rpy="0 -0 0"/>
+      <inertia ixx="0.001306" ixy="-8.28224e-06" ixz="-1.69917e-06" iyy="0.001381" iyz="1.4569e-06" izz="0.002216"/>
+    </inertial>
+  </link>
+  <link name="HEAD_LINK0">
+    <visual>
+      <origin xyz="-0.00725 0 0.084" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="package://kxr_models/meshes/kxrl2l2a6h2m/HEAD_LINK0_mesh.dae" scale="1 1 1" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="-0.00725 0 0.084" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="package://kxr_models/meshes/kxrl2l2a6h2m/HEAD_LINK0_mesh.dae" scale="1 1 1" />
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.037398" />
+      <origin xyz="0 1.22056e-18 0.023093" rpy="0 -0 0"/>
+      <inertia ixx="6.28707e-05" ixy="3.12188e-21" ixz="8.61164e-09" iyy="6.69539e-05" iyz="-5.25778e-09" izz="2.5436e-05"/>
+    </inertial>
+  </link>
+  <link name="HEAD_LINK1">
+    <visual>
+      <origin xyz="0 0 0.036" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="package://kxr_models/meshes/kxrl2l2a6h2m/HEAD_LINK1_mesh.dae" scale="1 1 1" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.036" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="package://kxr_models/meshes/kxrl2l2a6h2m/HEAD_LINK1_mesh.dae" scale="1 1 1" />
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.160516" />
+      <origin xyz="0.001469 -6.19726e-19 0.171559" rpy="0 -0 0"/>
+      <inertia ixx="0.00092" ixy="-1.79922e-07" ixz="-6.65543e-08" iyy="0.000849" iyz="-1.79583e-08" izz="0.000187"/>
+    </inertial>
+  </link>
+  <joint name="HEAD_JOINT0" type="revolute">
+    <parent link="bodyset94472077639384"/>
+    <child  link="HEAD_LINK0"/>
+    <origin xyz="-0.00725 0 0.084" rpy="0 -0 0 "/>
+    <axis   xyz="0 0 -1"/>
+    <limit lower="-2.96706" upper="2.96706" effort="0.656248" velocity="7.47998" />
+    <dynamics damping="0.2" friction="0" />
+  </joint>
+  <joint name="HEAD_JOINT1" type="revolute">
+    <parent link="HEAD_LINK0"/>
+    <child  link="HEAD_LINK1"/>
+    <origin xyz="0 0 0.036" rpy="0 -0 0 "/>
+    <axis   xyz="0 -1 0"/>
+    <limit lower="-2.0944" upper="2.0944" effort="0.656248" velocity="7.47998" />
+    <dynamics damping="0.2" friction="0" />
+  </joint>
+</robot>
diff --git a/ros/kxreus/CATKIN_IGNORE b/ros/kxreus/CATKIN_IGNORE
deleted file mode 100644
index e69de29b..00000000
diff --git a/ros/kxreus/CMakeLists.txt b/ros/kxreus/CMakeLists.txt
index b2e1e519..8ed1b0af 100644
--- a/ros/kxreus/CMakeLists.txt
+++ b/ros/kxreus/CMakeLists.txt
@@ -21,7 +21,7 @@ catkin_package(
 catkin_generate_virtualenv(
   INPUT_REQUIREMENTS requirements.in
   PYTHON_INTERPRETER python3
-  USE_SYSTEM_PACKAGES FALSE
+  USE_SYSTEM_PACKAGES TRUE
   ISOLATE_REQUIREMENTS FALSE
   CHECK_VENV FALSE
 )