Merge pull request #38 from iory/eus

Enable kxreus for using model server

ros/kxr_controller/node_scripts/rcb4_ros_bridge.py

@@ -120,6 +120,7 @@ def __init__(self):
         last_slash_pos = full_namespace.rfind('/')
         clean_namespace = full_namespace[:last_slash_pos] \
             if last_slash_pos != 0 else ''
+        self.clean_namespace = clean_namespace
 
         set_robot_description(
             urdf_path,
@@ -344,7 +345,8 @@ def create_imu_message(self):
         return msg
 
     def run(self):
-        rate = rospy.Rate(100)
+        rate = rospy.Rate(rospy.get_param(
+            self.clean_namespace + '/control_loop_rate', 20))
 
         while not rospy.is_shutdown():
             if self.interface.is_opened() is False:

ros/kxreus/CMakeLists.txt

@@ -6,13 +6,6 @@ find_package(catkin REQUIRED
   kxr_models
 )
 
-
-if(EXISTS ${kxr_models_SOURCE_PREFIX}/urdf/kxrl2l2a6h2m.urdf)
-  set(kxr_urdf ${kxr_models_SOURCE_PREFIX}/urdf/kxrl2l2a6h2m.urdf)
-else()
-  message(WARNING "Could not found kxr_models")
-endif()
-
 catkin_package(
   DEPENDS
   LIBRARIES ${PROJECT_NAME}
@@ -21,18 +14,12 @@ 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
 )
 
-set(_urdf2eus "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/venv/bin/urdf2eus")
-set(_venv_setup "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/venv/bin/activate")
-set(_yaml_path ${PROJECT_SOURCE_DIR}/config/kxrl2l2a6h2m.yaml)
-
-add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/kxrl2l2a6h2m.l
-  COMMAND echo "${_urdf2eus} ${kxr_urdf} ${PROJECT_SOURCE_DIR}/kxrl2l2a6h2m.l"
-  COMMAND . ${_venv_setup} && ${_urdf2eus} ${kxr_urdf} ${PROJECT_SOURCE_DIR}/models/kxrl2l2a6h2m.l --yaml-path ${_yaml_path}
-  DEPENDS ${kxr_urdf} ${_urdf2eus} ${_venv_setup} ${_yaml_path})
-
-add_custom_target(compile_kxr ALL DEPENDS ${PROJECT_SOURCE_DIR}/kxrl2l2a6h2m.l ${PROJECT_NAME}_generate_virtualenv)
+file(GLOB PYTHON_SCRIPT_FILES node_scripts/*)
+catkin_install_python(
+  PROGRAMS ${PYTHON_SCRIPT_FILES}
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

ros/kxreus/euslisp/kxr-interface.l

@@ -0,0 +1,119 @@
+(require :robot-interface "package://pr2eus/robot-interface.l")
+
+(ros::load-ros-manifest "control_msgs")
+(ros::load-ros-manifest "kxr_controller")
+
+
+(defun load-robot-model (&key (port 8000) (namespace nil))
+  (let* ((port "8000")
+         (hash-param-name (if namespace (format nil "~A~A" namespace "/eusmodel_hash") "/eusmodel_hash"))
+         (robot-name-param-name (if namespace (format nil "~A~A" namespace "/eus_robot_name") "/eus_robot_name"))
+         hash robot-name outpath server-url)
+    (while (null hash)
+      (ros::ros-info (format nil "Waiting rosparam ~A" hash-param-name))
+      (setq hash (ros::get-param hash-param-name nil))
+      (unix::usleep (* 1000 1000)))
+    (ros::ros-info (format nil "Get rosparam ~A" hash-param-name))
+
+    (while (null robot-name)
+      (ros::ros-info (format nil "Waiting rosparam ~A" robot-name-param-name))
+      (setq robot-name (ros::get-param robot-name-param-name nil))
+      (unix::usleep (* 1000 1000)))
+    (ros::ros-info (format nil "Get rosparam ~A" robot-name-param-name))
+
+    (setq outpath (ros::resolve-ros-path (format nil "package://kxreus/models/cache/~A.l" hash)))
+    (setq server-url (format nil "http://~A:~A/~A.l" (ros::get-host) port hash))
+
+    (when (not (probe-file outpath))
+      (while (not (= (unix:system (format nil "wget -O ~A ~A" outpath server-url)) 0))
+        (ros::ros-info (format nil "Waiting model file from server url ~A" server-url))
+        (unix::usleep (* 1000 1000))))
+    (load outpath)
+    (funcall (read-from-string robot-name))))
+
+
+(defclass kxr-interface
+  :super robot-interface
+  :slots (joint-names servo-on-off-client))
+
+
+(defmethod kxr-interface
+  (:init (robot &rest args &key (type :default-controller) &allow-other-keys)
+   (let* ((namespace (cadr (memq :namespace args)))
+          (joint-param (if namespace (format nil "~A/~A" namespace "/kxr_fullbody_controller/joints")
+                           "/kxr_fullbody_controller/joints")))
+     (setq joint-names (ros::get-param joint-param nil))
+     (while (and (ros::ok) (null joint-names))
+       (ros::ros-info (format nil "Waiting ~A rosparam set" joint-param))
+       (setq joint-names (ros::get-param joint-param nil))
+       (unix::usleep (* 1000 1000))))
+
+   (send-super* :init :robot robot :type type
+                :groupname "kxr_interface"
+                args)
+
+   (setq servo-on-off-client (instance ros::simple-action-client :init
+                                       (if namespace (format nil "~A/~A" namespace "/kxr_fullbody_controller/servo_on_off") "/kxr_fullbody_controller/servo_on_off")
+                                       kxr_controller::ServoOnOffAction
+                                       :groupname groupname))
+   (dolist (action (list servo-on-off-client))
+     (unless (and joint-action-enable (send action :wait-for-server 3))
+       (setq joint-action-enable nil)
+       (ros::ros-warn "~A is not respond, kxr-interface is disabled" action)
+       (return)))
+   t)
+  (:default-controller
+   ()
+   (send self :fullbody-controller))
+  (:fullbody-controller
+   ()
+   (list
+    (list
+     (cons :controller-action "kxr_fullbody_controller/follow_joint_trajectory")
+     (cons :controller-state "kxr_fullbody_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (cons :joint-names joint-names))))
+  (:servo-on
+   (&key (names nil))
+   (unless joint-action-enable
+     (if viewer (send self :draw-objects))
+     (return-from :servo-on t))
+   (when (null names)
+     (setq names joint-names))
+   (let* (goal result)
+     ;; send current angle-vector
+     (send self :angle-vector (send self :state :potentio-vector) 0.1)
+     (send self :wait-interpolation)
+
+     (setq goal (instance kxr_controller::ServoOnOffGoal :init))
+     (send goal :joint_names names)
+     (send goal :servo_on_states (make-array (length names) :initial-element t))
+     (send servo-on-off-client :send-goal goal)))
+  (:servo-off
+   (&key (names nil))
+   (unless joint-action-enable
+     (if viewer (send self :draw-objects))
+     (return-from :servo-off t))
+   (when (null names)
+     (setq names joint-names))
+   (let* (goal result)
+     (setq goal (instance kxr_controller::ServoOnOffGoal :init))
+     (send goal :joint_names names)
+     (send goal :servo_on_states (make-array (length names) :initial-element nil))
+     (send servo-on-off-client :send-goal goal))))
+
+
+(defun kxr-init (&key
+                   (namespace nil)
+                   (create-viewer t))
+  (unless (boundp '*robot*)
+    (setq *robot* (load-robot-model :namespace namespace
+                                    :port (ros::get-param (if namespace (format nil "~A/port" namespace) "/port") 8123))))
+  (unless (ros::ok) (ros::roseus "kxr_eus_interface"))
+  (unless (boundp '*ri*)
+    (setq *ri* (instance kxr-interface :init *robot* :namespace namespace)))
+
+  (ros::spin-once)
+  (send *ri* :spin-once)
+  (send *robot* :angle-vector (send *ri* :state :potentio-vector))
+  (when create-viewer (objects (list *robot*))))

ros/kxreus/launch/eusmodel_server.launch

@@ -0,0 +1,28 @@
+<launch>
+
+  <arg name="namespace" default="" />
+  <arg name="port" default="8123" />
+
+  <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
+    <param name="port" value="$(arg port)" />
+    <node name="eusmodel_server"
+          pkg="kxreus" type="eus_model_server.py" >
+    </node>
+
+    <node name="http_server_node"
+          pkg="kxreus" type="http_server_node.py" >
+    </node>
+  </group>
+
+  <group unless="$(eval len(arg('namespace')) > 0)">
+    <param name="port" value="$(arg port)" />
+    <node name="eusmodel_server"
+          pkg="kxreus" type="eus_model_server.py" >
+    </node>
+
+    <node name="http_server_node"
+          pkg="kxreus" type="http_server_node.py" >
+    </node>
+  </group>
+
+</launch>

ros/kxreus/models/cache/.gitignore

@@ -0,0 +1,2 @@
+*
+!.gitignore

ros/kxreus/node_scripts/eus_model_server.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python
+
+import hashlib
+import os
+import tempfile
+
+from filelock import FileLock
+import rospkg
+import rospy
+from skrobot.model import RobotModel
+from skrobot.utils.urdf import no_mesh_load_mode
+from urdfeus.urdf2eus import urdf2eus
+
+
+def checksum_md5(filename, blocksize=8192):
+    """Calculate md5sum.
+
+    Parameters
+    ----------
+    filename : str or pathlib.Path
+        input filename.
+    blocksize : int
+        MD5 has 128-byte digest blocks (default: 8192 is 128x64).
+
+    Returns
+    -------
+    md5 : str
+        calculated md5sum.
+    """
+    filename = str(filename)
+    hash_factory = hashlib.md5()
+    with open(filename, 'rb') as f:
+        for chunk in iter(lambda: f.read(blocksize), b''):
+            hash_factory.update(chunk)
+    return hash_factory.hexdigest()
+
+
+class EusModelServer(object):
+
+    def __init__(self):
+        full_namespace = rospy.get_namespace()
+        last_slash_pos = full_namespace.rfind('/')
+        self.clean_namespace = full_namespace[:last_slash_pos] \
+            if last_slash_pos != 0 else ''
+
+    def load_robot_model(self, urdf_path):
+        r = RobotModel()
+        with open(urdf_path, 'r') as f:
+            with no_mesh_load_mode():
+                r.load_urdf_file(f)
+        return r
+
+    def run(self):
+        rate = rospy.Rate(1)
+
+        previous_md5sum = None
+        robot_model = None
+
+        rospack = rospkg.RosPack()
+        kxreus_path = rospack.get_path('kxreus')
+
+        while not rospy.is_shutdown():
+            rate.sleep()
+            urdf = rospy.get_param(
+                self.clean_namespace + '/robot_description',
+                None)
+            if urdf is not None:
+                tmp_file = tempfile.mktemp()
+                with open(tmp_file, "w") as f:
+                    f.write(urdf)
+                md5sum = checksum_md5(tmp_file)
+                if previous_md5sum is None or previous_md5sum != md5sum:
+                    robot_model = self.load_robot_model(tmp_file)
+                previous_md5sum = md5sum
+
+                eus_path = os.path.join(
+                    kxreus_path, 'models', '{}.l'.format(md5sum))
+                robot_name = robot_model.urdf_robot_model.name
+                if os.path.exists(eus_path):
+                    rospy.set_param(self.clean_namespace + '/eus_robot_name',
+                                    robot_name)
+                    rospy.set_param(self.clean_namespace + '/eusmodel_hash',
+                                    md5sum)
+                    continue
+
+                lock_path = eus_path + ".lock"
+                lock = FileLock(lock_path, timeout=10)
+                try:
+                    with lock:
+                        with open(eus_path, 'w') as f:
+                            urdf2eus(tmp_file, fp=f)
+                        rospy.loginfo(
+                            'Eusmodel is saved to {}'.format(eus_path))
+                    os.remove(lock_path)
+                finally:
+                    if os.path.exists(tmp_file):
+                        os.remove(tmp_file)
+
+
+if __name__ == '__main__':
+    rospy.init_node('model_server')
+    server = EusModelServer()
+    server.run()

ros/kxreus/node_scripts/http_server_node.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+
+from http.server import HTTPServer
+from http.server import SimpleHTTPRequestHandler
+import os
+import threading
+
+from filelock import FileLock
+from filelock import Timeout
+import rospkg
+import rospy
+
+
+class CustomHTTPRequestHandler(SimpleHTTPRequestHandler):
+    def __init__(self, *args, directory=None, **kwargs):
+        self.base_directory = directory
+        super().__init__(*args, **kwargs)
+
+    def translate_path(self, path):
+        path = super().translate_path(path)
+        rel_path = os.path.relpath(path, os.getcwd())
+        full_path = os.path.join(self.base_directory, rel_path) \
+            if self.base_directory else path
+        return full_path
+
+    def do_GET(self):
+        filepath = self.translate_path(self.path)
+        lock_path = filepath + ".lock"
+
+        if os.path.exists(lock_path):
+            try:
+                with FileLock(lock_path, timeout=1):
+                    super().do_GET()
+            except Timeout:
+                self.send_response(503)
+                self.end_headers()
+                self.wfile.write(
+                    b"Service Unavailable: File is currently locked.")
+        else:
+            super().do_GET()
+
+
+class ThreadedHTTPServer(object):
+    def __init__(self, host, port, handler_class, directory):
+        def handler_factory(*args, **kwargs):
+            return handler_class(
+                *args, directory=directory, **kwargs)
+        self.server = HTTPServer((host, port), handler_factory)
+        self.server_thread = threading.Thread(target=self.server.serve_forever)
+        self.server_thread.daemon = True
+
+    def start(self):
+        self.server_thread.start()
+        rospy.loginfo("HTTP Server Running...")
+
+    def stop(self):
+        self.server.shutdown()
+        self.server.server_close()
+        rospy.loginfo("HTTP Server Stopped.")
+
+
+if __name__ == '__main__':
+    rospy.init_node('http_server_node')
+
+    rospack = rospkg.RosPack()
+    kxreus_path = rospack.get_path('kxreus')
+    www_directory = os.path.join(kxreus_path, 'models')
+
+    full_namespace = rospy.get_namespace()
+    last_slash_pos = full_namespace.rfind('/')
+    clean_namespace = full_namespace[:last_slash_pos] \
+        if last_slash_pos != 0 else ''
+
+    port = rospy.get_param(clean_namespace + '/port', 8123)
+    server = ThreadedHTTPServer(
+        '0.0.0.0', port, CustomHTTPRequestHandler, www_directory)
+    server.start()
+
+    rospy.spin()
+
+    server.stop()