Skip to content

Commit 2a1c339

Browse files
ioryiory
committed
Enable kxreus for using model server
1 parent cb64f13 commit 2a1c339

8 files changed

+323
-20
lines changed

‎ros/kxreus/CATKIN_IGNORE

Whitespace-only changes.

‎ros/kxreus/CMakeLists.txt

+5-18
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,6 @@ find_package(catkin REQUIRED
66
kxr_models
77
)
88

9-
10-
if(EXISTS ${kxr_models_SOURCE_PREFIX}/urdf/kxrl2l2a6h2m.urdf)
11-
set(kxr_urdf ${kxr_models_SOURCE_PREFIX}/urdf/kxrl2l2a6h2m.urdf)
12-
else()
13-
message(WARNING "Could not found kxr_models")
14-
endif()
15-
169
catkin_package(
1710
DEPENDS
1811
LIBRARIES ${PROJECT_NAME}
@@ -21,18 +14,12 @@ catkin_package(
2114
catkin_generate_virtualenv(
2215
INPUT_REQUIREMENTS requirements.in
2316
PYTHON_INTERPRETER python3
24-
USE_SYSTEM_PACKAGES FALSE
17+
USE_SYSTEM_PACKAGES TRUE
2518
ISOLATE_REQUIREMENTS FALSE
2619
CHECK_VENV FALSE
2720
)
2821

29-
set(_urdf2eus "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/venv/bin/urdf2eus")
30-
set(_venv_setup "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/venv/bin/activate")
31-
set(_yaml_path ${PROJECT_SOURCE_DIR}/config/kxrl2l2a6h2m.yaml)
32-
33-
add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/kxrl2l2a6h2m.l
34-
COMMAND echo "${_urdf2eus} ${kxr_urdf} ${PROJECT_SOURCE_DIR}/kxrl2l2a6h2m.l"
35-
COMMAND . ${_venv_setup} && ${_urdf2eus} ${kxr_urdf} ${PROJECT_SOURCE_DIR}/models/kxrl2l2a6h2m.l --yaml-path ${_yaml_path}
36-
DEPENDS ${kxr_urdf} ${_urdf2eus} ${_venv_setup} ${_yaml_path})
37-
38-
add_custom_target(compile_kxr ALL DEPENDS ${PROJECT_SOURCE_DIR}/kxrl2l2a6h2m.l ${PROJECT_NAME}_generate_virtualenv)
22+
file(GLOB PYTHON_SCRIPT_FILES node_scripts/*)
23+
catkin_install_python(
24+
PROGRAMS ${PYTHON_SCRIPT_FILES}
25+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

‎ros/kxreus/euslisp/kxr-interface.l

+115
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
(require :robot-interface "package://pr2eus/robot-interface.l")
2+
3+
(ros::load-ros-manifest "control_msgs")
4+
(ros::load-ros-manifest "kxr_controller")
5+
6+
7+
(defun load-robot-model (&key (port 8000)
8+
(namespace nil))
9+
(let* ((port "8000")
10+
(hash (if namespace (format nil "~A~A" namespace (ros::get-param "/eusmodel_hash"))
11+
(ros::get-param "/eusmodel_hash")))
12+
(robot-name (if namespace (format nil "~A~A" namespace (ros::get-param "/eus_robot_name"))
13+
(ros::get-param "/eus_robot_name")))
14+
(outpath (ros::resolve-ros-path (format nil "package://kxreus/models/cache/~A.l" hash)))
15+
(server-url (format nil "http://~A:~A/~A.l"
16+
(ros::get-host)
17+
port
18+
hash)))
19+
(when (not (probe-file outpath))
20+
(while (not (= (unix:system (format nil "wget -O ~A ~A"
21+
outpath
22+
server-url
23+
))
24+
0))
25+
(ros::ros-info (format nil "Waiting model file from server url ~A" server-url))
26+
(unix::usleep (* 1000 1000))))
27+
(load outpath)
28+
(funcall (read-from-string robot-name))))
29+
30+
31+
(defclass kxr-interface
32+
:super robot-interface
33+
:slots (joint-names servo-on-off-client))
34+
35+
36+
(defmethod kxr-interface
37+
(:init (robot &rest args &key (type :default-controller) &allow-other-keys)
38+
(let* ((namespace (cadr (memq :namespace args)))
39+
(joint-param (if namespace (format nil "~A/~A" namespace "/kxr_fullbody_controller/joints")
40+
"/kxr_fullbody_controller/joints")))
41+
(setq joint-names (ros::get-param joint-param nil))
42+
(while (and (ros::ok) (null joint-names))
43+
(ros::ros-info (format nil "Waiting ~A rosparam set" joint-param))
44+
(setq joint-names (ros::get-param joint-param nil))
45+
(ros::sleep)))
46+
47+
(send-super* :init :robot robot :type type
48+
:groupname "kxr_interface"
49+
args)
50+
51+
(setq servo-on-off-client (instance ros::simple-action-client :init
52+
(if namespace (format nil "~A/~A" namespace "/kxr_fullbody_controller/servo_on_off") "/kxr_fullbody_controller/servo_on_off")
53+
kxr_controller::ServoOnOffAction
54+
:groupname groupname))
55+
(dolist (action (list servo-on-off-client))
56+
(unless (and joint-action-enable (send action :wait-for-server 3))
57+
(setq joint-action-enable nil)
58+
(ros::ros-warn "~A is not respond, kxr-interface is disabled" action)
59+
(return)))
60+
t)
61+
(:default-controller
62+
()
63+
(send self :fullbody-controller))
64+
(:fullbody-controller
65+
()
66+
(list
67+
(list
68+
(cons :controller-action "kxr_fullbody_controller/follow_joint_trajectory")
69+
(cons :controller-state "kxr_fullbody_controller/state")
70+
(cons :action-type control_msgs::FollowJointTrajectoryAction)
71+
(cons :joint-names joint-names))))
72+
(:servo-on
73+
(&key (names nil))
74+
(unless joint-action-enable
75+
(if viewer (send self :draw-objects))
76+
(return-from :servo-on t))
77+
(when (null names)
78+
(setq names joint-names))
79+
(let* (goal result)
80+
;; send current angle-vector
81+
(send self :angle-vector (send self :state :potentio-vector) 0.1)
82+
(send self :wait-interpolation)
83+
84+
(setq goal (instance kxr_controller::ServoOnOffGoal :init))
85+
(send goal :joint_names names)
86+
(send goal :servo_on_states (make-array (length names) :initial-element t))
87+
(send servo-on-off-client :send-goal goal)))
88+
(:servo-off
89+
(&key (names nil))
90+
(unless joint-action-enable
91+
(if viewer (send self :draw-objects))
92+
(return-from :servo-off t))
93+
(when (null names)
94+
(setq names joint-names))
95+
(let* (goal result)
96+
(setq goal (instance kxr_controller::ServoOnOffGoal :init))
97+
(send goal :joint_names names)
98+
(send goal :servo_on_states (make-array (length names) :initial-element nil))
99+
(send servo-on-off-client :send-goal goal))))
100+
101+
102+
(defun kxr-init (&key
103+
(namespace nil)
104+
(create-viewer t))
105+
(unless (boundp '*robot*)
106+
(setq *robot* (load-robot-model :namespace namespace
107+
:port (ros::get-param (if namespace (format nil "~A/port" namespace) "/port") 8000))))
108+
(unless (ros::ok) (ros::roseus "kxr_eus_interface"))
109+
(unless (boundp '*ri*)
110+
(setq *ri* (instance kxr-interface :init *robot* :namespace namespace)))
111+
112+
(ros::spin-once)
113+
(send *ri* :spin-once)
114+
(send *robot* :angle-vector (send *ri* :angle-vector))
115+
(when create-viewer (objects (list *robot*))))

‎ros/kxreus/launch/eusmodel_server.launch

+32
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
<launch>
2+
3+
<arg name="namespace" default="" />
4+
<arg name="port" default="8000" />
5+
6+
<group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
7+
<node name="eusmodel_server"
8+
pkg="kxreus" type="eus_model_server.py" >
9+
</node>
10+
11+
<node name="http_server_node"
12+
pkg="kxreus" type="http_server_node.py" >
13+
<rosparam subst_value="true" >
14+
port: $(arg port)
15+
</rosparam>
16+
</node>
17+
</group>
18+
19+
<group unless="$(eval len(arg('namespace')) > 0)">
20+
<node name="eusmodel_server"
21+
pkg="kxreus" type="eus_model_server.py" >
22+
</node>
23+
24+
<node name="http_server_node"
25+
pkg="kxreus" type="http_server_node.py" >
26+
<rosparam subst_value="true" >
27+
port: $(arg port)
28+
</rosparam>
29+
</node>
30+
</group>
31+
32+
</launch>

‎ros/kxreus/node_scripts/eus_model_server.py

+92
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
#!/usr/bin/env python
2+
3+
import hashlib
4+
import os
5+
import tempfile
6+
7+
from filelock import FileLock
8+
import rospkg
9+
import rospy
10+
from skrobot.model import RobotModel
11+
from skrobot.utils.urdf import no_mesh_load_mode
12+
from urdfeus.urdf2eus import urdf2eus
13+
14+
15+
def checksum_md5(filename, blocksize=8192):
16+
"""Calculate md5sum.
17+
18+
Parameters
19+
----------
20+
filename : str or pathlib.Path
21+
input filename.
22+
blocksize : int
23+
MD5 has 128-byte digest blocks (default: 8192 is 128x64).
24+
25+
Returns
26+
-------
27+
md5 : str
28+
calculated md5sum.
29+
"""
30+
filename = str(filename)
31+
hash_factory = hashlib.md5()
32+
with open(filename, 'rb') as f:
33+
for chunk in iter(lambda: f.read(blocksize), b''):
34+
hash_factory.update(chunk)
35+
return hash_factory.hexdigest()
36+
37+
38+
class EusModelServer(object):
39+
40+
def __init__(self):
41+
full_namespace = rospy.get_namespace()
42+
last_slash_pos = full_namespace.rfind('/')
43+
self.clean_namespace = full_namespace[:last_slash_pos] \
44+
if last_slash_pos != 0 else ''
45+
46+
def run(self):
47+
rate = rospy.Rate(1)
48+
while not rospy.is_shutdown():
49+
rate.sleep()
50+
urdf = rospy.get_param(
51+
self.clean_namespace + '/robot_description',
52+
None)
53+
if urdf is not None:
54+
tmp_file = tempfile.mktemp()
55+
with open(tmp_file, "w") as f:
56+
f.write(urdf)
57+
md5sum = checksum_md5(tmp_file)
58+
59+
r = RobotModel()
60+
with open(tmp_file, 'r') as f:
61+
with no_mesh_load_mode():
62+
r.load_urdf_file(f)
63+
robot_name = r.urdf_robot_model.name
64+
65+
rospack = rospkg.RosPack()
66+
kxreus_path = rospack.get_path('kxreus')
67+
eus_path = os.path.join(
68+
kxreus_path, 'models', '{}.l'.format(md5sum))
69+
if os.path.exists(eus_path):
70+
rospy.set_param(self.clean_namespace + '/eus_robot_name',
71+
robot_name)
72+
rospy.set_param(self.clean_namespace + '/eusmodel_hash',
73+
md5sum)
74+
continue
75+
76+
lock_path = eus_path + ".lock"
77+
lock = FileLock(lock_path, timeout=10)
78+
try:
79+
with lock:
80+
with open(eus_path, 'w') as f:
81+
urdf2eus(tmp_file, fp=f)
82+
rospy.loginfo('Eusmodel is saved to {}'.format(eus_path))
83+
os.remove(lock_path)
84+
finally:
85+
if os.path.exists(tmp_file):
86+
os.remove(tmp_file)
87+
88+
89+
if __name__ == '__main__':
90+
rospy.init_node('model_server')
91+
server = EusModelServer()
92+
server.run()

‎ros/kxreus/node_scripts/http_server_node.py

+76
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
#!/usr/bin/env python
2+
3+
from http.server import HTTPServer
4+
from http.server import SimpleHTTPRequestHandler
5+
import os
6+
import threading
7+
8+
from filelock import FileLock
9+
from filelock import Timeout
10+
import rospkg
11+
import rospy
12+
13+
14+
class CustomHTTPRequestHandler(SimpleHTTPRequestHandler):
15+
def __init__(self, *args, directory=None, **kwargs):
16+
self.base_directory = directory
17+
super().__init__(*args, **kwargs)
18+
19+
def translate_path(self, path):
20+
path = super().translate_path(path)
21+
rel_path = os.path.relpath(path, os.getcwd())
22+
full_path = os.path.join(self.base_directory, rel_path) if self.base_directory else path
23+
return full_path
24+
25+
def do_GET(self):
26+
filepath = self.translate_path(self.path)
27+
lock_path = filepath + ".lock"
28+
29+
if os.path.exists(lock_path):
30+
try:
31+
with FileLock(lock_path, timeout=1):
32+
super().do_GET()
33+
except Timeout:
34+
self.send_response(503)
35+
self.end_headers()
36+
self.wfile.write(b"Service Unavailable: File is currently locked.")
37+
else:
38+
super().do_GET()
39+
40+
41+
class ThreadedHTTPServer(object):
42+
def __init__(self, host, port, handler_class, directory):
43+
handler_factory = lambda *args, **kwargs: handler_class(*args, directory=directory, **kwargs)
44+
self.server = HTTPServer((host, port), handler_factory)
45+
self.server_thread = threading.Thread(target=self.server.serve_forever)
46+
self.server_thread.daemon = True
47+
48+
def start(self):
49+
self.server_thread.start()
50+
rospy.loginfo("HTTP Server Running...")
51+
52+
def stop(self):
53+
self.server.shutdown()
54+
self.server.server_close()
55+
rospy.loginfo("HTTP Server Stopped.")
56+
57+
58+
if __name__ == '__main__':
59+
rospy.init_node('http_server_node')
60+
61+
rospack = rospkg.RosPack()
62+
kxreus_path = rospack.get_path('kxreus')
63+
www_directory = os.path.join(kxreus_path, 'models')
64+
65+
full_namespace = rospy.get_namespace()
66+
last_slash_pos = full_namespace.rfind('/')
67+
clean_namespace = full_namespace[:last_slash_pos] \
68+
if last_slash_pos != 0 else ''
69+
70+
port = rospy.get_param(clean_namespace + '/port', 8000)
71+
server = ThreadedHTTPServer('0.0.0.0', port, CustomHTTPRequestHandler, www_directory)
72+
server.start()
73+
74+
rospy.spin()
75+
76+
server.stop()

‎ros/kxreus/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
<!-- ALPHABETICAL ORDER -->
1919

2020
<!-- ALPHABETICAL ORDER -->
21+
<build_depend>pr2eus</build_depend>
2122
<build_depend>roseus</build_depend>
2223
<!-- ALPHABETICAL ORDER -->
2324

‎ros/kxreus/requirements.in

+2-2
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,2 @@
1-
scikit-robot==0.0.32
2-
urdfeus==0.0.5
1+
scikit-robot
2+
urdfeus

0 commit comments

Comments
 (0)