From 41e39108fffb329ef0cb0a2582c1aba5f678fc3e Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Sun, 11 Feb 2024 13:34:01 +0900
Subject: [PATCH] Add interface.py

---
 ros/kxr_controller/CMakeLists.txt       |  2 +-
 ros/kxr_controller/requirements.in      |  1 +
 ros/kxr_controller/scripts/interface.py | 44 +++++++++++++++++++++++++
 3 files changed, 46 insertions(+), 1 deletion(-)
 create mode 100644 ros/kxr_controller/scripts/interface.py

diff --git a/ros/kxr_controller/CMakeLists.txt b/ros/kxr_controller/CMakeLists.txt
index eb981805..6342238f 100644
--- a/ros/kxr_controller/CMakeLists.txt
+++ b/ros/kxr_controller/CMakeLists.txt
@@ -88,7 +88,7 @@ target_link_libraries(kxr_controller
   ${Eigen_LIBRARIES}
 )
 
-file(GLOB PYTHON_SCRIPT_FILES node_scripts/* test/*)
+file(GLOB PYTHON_SCRIPT_FILES scripts/* node_scripts/* test/*)
 catkin_install_python(
   PROGRAMS ${PYTHON_SCRIPT_FILES}
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/ros/kxr_controller/requirements.in b/ros/kxr_controller/requirements.in
index 22a35f57..453f2b01 100644
--- a/ros/kxr_controller/requirements.in
+++ b/ros/kxr_controller/requirements.in
@@ -1 +1,2 @@
+pyglet==1.4.10
 scikit-robot>=0.0.37
diff --git a/ros/kxr_controller/scripts/interface.py b/ros/kxr_controller/scripts/interface.py
new file mode 100644
index 00000000..74f53ecf
--- /dev/null
+++ b/ros/kxr_controller/scripts/interface.py
@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+import argparse
+
+import IPython
+from kxr_controller.kxr_interface import KXRROSRobotInterface
+import rospy
+from skrobot.model import RobotModel
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description='Run KXRROSRobotInterface')
+    parser.add_argument('--viewer', type=str,
+                        help='Specify the viewer: trimesh', default=None)
+    parser.add_argument(
+        '--namespace', type=str, help='Specify the ROS namespace', default='')
+    args = parser.parse_args()
+
+    rospy.init_node('kxr_interface', anonymous=True)
+    robot_model = RobotModel()
+    robot_model.load_urdf_from_robot_description(
+        args.namespace + '/robot_description')
+    ri = KXRROSRobotInterface(  # NOQA
+        robot_model, namespace=args.namespace, controller_timeout=60.0)
+
+    if args.viewer is not None:
+        if args.viewer == 'trimesh':
+            from skrobot.viewers import TrimeshSceneViewer
+            viewer = TrimeshSceneViewer(resolution=(640, 480))
+            viewer.add(robot_model)
+            viewer.show()
+        else:
+            raise NotImplementedError(
+                'Not supported viewer {}'.format(args.viewer))
+
+    # Drop into an IPython shell
+    IPython.embed()
+
+
+if __name__ == '__main__':
+    main()