diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 11643cfc..ae83b02d 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/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:
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..5dc3f320 100644
--- a/ros/kxreus/CMakeLists.txt
+++ b/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})
diff --git a/ros/kxreus/euslisp/kxr-interface.l b/ros/kxreus/euslisp/kxr-interface.l
new file mode 100644
index 00000000..da49c4d1
--- /dev/null
+++ b/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*))))
diff --git a/ros/kxreus/launch/eusmodel_server.launch b/ros/kxreus/launch/eusmodel_server.launch
new file mode 100644
index 00000000..842bc1c0
--- /dev/null
+++ b/ros/kxreus/launch/eusmodel_server.launch
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ros/kxreus/models/cache/.gitignore b/ros/kxreus/models/cache/.gitignore
new file mode 100644
index 00000000..d6b7ef32
--- /dev/null
+++ b/ros/kxreus/models/cache/.gitignore
@@ -0,0 +1,2 @@
+*
+!.gitignore
diff --git a/ros/kxreus/node_scripts/eus_model_server.py b/ros/kxreus/node_scripts/eus_model_server.py
new file mode 100644
index 00000000..6d8889ff
--- /dev/null
+++ b/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()
diff --git a/ros/kxreus/node_scripts/http_server_node.py b/ros/kxreus/node_scripts/http_server_node.py
new file mode 100644
index 00000000..7256dff8
--- /dev/null
+++ b/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()
diff --git a/ros/kxreus/package.xml b/ros/kxreus/package.xml
index 986333b5..cfce845a 100644
--- a/ros/kxreus/package.xml
+++ b/ros/kxreus/package.xml
@@ -18,6 +18,7 @@
+ pr2eus
roseus
diff --git a/ros/kxreus/requirements.in b/ros/kxreus/requirements.in
index 40b60395..8154018c 100644
--- a/ros/kxreus/requirements.in
+++ b/ros/kxreus/requirements.in
@@ -1,2 +1,2 @@
-scikit-robot==0.0.32
-urdfeus==0.0.5
+scikit-robot
+urdfeus