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