diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch index c7803a23..7f8dea24 100644 --- a/ros/kxr_controller/launch/kxr_controller.launch +++ b/ros/kxr_controller/launch/kxr_controller.launch @@ -8,6 +8,12 @@ + + + + + @@ -25,7 +31,9 @@ use_rcb4: $(arg use_rcb4) - + + @@ -45,6 +53,9 @@ + + diff --git a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch index 0ac58e4a..7ac816d4 100644 --- a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch +++ b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch @@ -4,12 +4,14 @@ + + diff --git a/ros/kxr_controller/launch/rviz/viewer.rviz b/ros/kxr_controller/launch/rviz/viewer.rviz new file mode 100644 index 00000000..682ae518 --- /dev/null +++ b/ros/kxr_controller/launch/rviz/viewer.rviz @@ -0,0 +1,251 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + HEAD_LINK0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + HEAD_LINK1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_LINK0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_LINK1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_LINK2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_LINK3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_LINK4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_LINK5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_LINK6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_LINK7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LLEG_LINK0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LLEG_LINK1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RARM_LINK0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_LINK1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_LINK2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_LINK3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_LINK4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_LINK5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_LINK6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_LINK7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RLEG_LINK0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RLEG_LINK1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bodyset94472077639384: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description_viz + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.9285101890563965 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6703981161117554 + Target Frame: + Yaw: 0.510398268699646 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000169000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000022c000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 368 + Y: 214 diff --git a/ros/kxr_controller/launch/viewer.launch b/ros/kxr_controller/launch/viewer.launch new file mode 100644 index 00000000..d16443e1 --- /dev/null +++ b/ros/kxr_controller/launch/viewer.launch @@ -0,0 +1,17 @@ + + + + + + + connect_tf: $(arg connect_tf) + + + + + + + diff --git a/ros/kxr_controller/package.xml b/ros/kxr_controller/package.xml index a264a221..c90807de 100644 --- a/ros/kxr_controller/package.xml +++ b/ros/kxr_controller/package.xml @@ -33,6 +33,7 @@ urdf cmake_modules kxr_models + rviz roslaunch rostest diff --git a/ros/kxr_controller/requirements.in b/ros/kxr_controller/requirements.in index 453f2b01..ff348427 100644 --- a/ros/kxr_controller/requirements.in +++ b/ros/kxr_controller/requirements.in @@ -1,2 +1,3 @@ +numpy==1.24.4 pyglet==1.4.10 scikit-robot>=0.0.37 diff --git a/ros/kxr_models/CMakeLists.txt b/ros/kxr_models/CMakeLists.txt index e33bb9fd..8f855d5c 100644 --- a/ros/kxr_models/CMakeLists.txt +++ b/ros/kxr_models/CMakeLists.txt @@ -4,8 +4,15 @@ project(kxr_models) find_package(catkin REQUIRED) +catkin_python_setup() + catkin_package() +file(GLOB PYTHON_SCRIPT_FILES scripts/* node_scripts/*) +catkin_install_python( + PROGRAMS ${PYTHON_SCRIPT_FILES} + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + find_package(roslaunch) foreach(dir config meshes urdf) install(DIRECTORY ${dir}/ diff --git a/ros/kxr_models/models/.gitignore b/ros/kxr_models/models/.gitignore new file mode 100644 index 00000000..d6b7ef32 --- /dev/null +++ b/ros/kxr_models/models/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/ros/kxr_models/models/urdf/.gitignore b/ros/kxr_models/models/urdf/.gitignore new file mode 100644 index 00000000..d6b7ef32 --- /dev/null +++ b/ros/kxr_models/models/urdf/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/ros/kxreus/node_scripts/http_server_node.py b/ros/kxr_models/node_scripts/http_server_node.py old mode 100644 new mode 100755 similarity index 87% rename from ros/kxreus/node_scripts/http_server_node.py rename to ros/kxr_models/node_scripts/http_server_node.py index 7256dff8..781908db --- a/ros/kxreus/node_scripts/http_server_node.py +++ b/ros/kxr_models/node_scripts/http_server_node.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from http.server import HTTPServer from http.server import SimpleHTTPRequestHandler @@ -63,15 +63,10 @@ def stop(self): rospy.init_node('http_server_node') rospack = rospkg.RosPack() - kxreus_path = rospack.get_path('kxreus') + kxreus_path = rospack.get_path('kxr_models') 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) + port = rospy.get_param('/model_server_port', 8123) server = ThreadedHTTPServer( '0.0.0.0', port, CustomHTTPRequestHandler, www_directory) server.start() diff --git a/ros/kxr_models/node_scripts/urdf_model_server.py b/ros/kxr_models/node_scripts/urdf_model_server.py new file mode 100755 index 00000000..0a90e6df --- /dev/null +++ b/ros/kxr_models/node_scripts/urdf_model_server.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 + +import os +import tempfile +import xml.etree.ElementTree as ET + +from filelock import FileLock +from kxr_models.md5sum_utils import checksum_md5 +from kxr_models.urdf import aggregate_urdf_mesh_files +import rospkg +import rospy + + +class URDFModelServer(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 run(self): + rate = rospy.Rate(1) + + rospack = rospkg.RosPack() + kxr_models_path = rospack.get_path('kxr_models') + + while not rospy.is_shutdown(): + rate.sleep() + urdf = rospy.get_param( + self.clean_namespace + '/robot_description', + None) + if urdf is not None: + urdf_path = tempfile.mktemp() + with open(urdf_path, "w") as f: + f.write(urdf) + md5sum = checksum_md5(urdf_path) + compressed_urdf_path = os.path.join( + kxr_models_path, 'models', 'urdf', + '{}.tar.gz'.format(md5sum)) + if os.path.exists(compressed_urdf_path): + parser = ET.XMLParser( + target=ET.TreeBuilder(insert_comments=True)) + tree = ET.parse(urdf_path, parser) + root = tree.getroot() + robot_name = root.get('name') + + rospy.set_param(self.clean_namespace + '/urdf_hash', + md5sum) + with open(os.path.join( + kxr_models_path, 'models', 'urdf', + md5sum, '{}.urdf'.format(robot_name))) as f: + rospy.set_param( + self.clean_namespace + '/robot_description_viz', + f.read()) + continue + + lock_path = compressed_urdf_path + ".lock" + lock = FileLock(lock_path, timeout=10) + try: + with lock: + aggregate_urdf_mesh_files( + urdf_path, + os.path.join( + kxr_models_path, 'models', 'urdf'), + compress=True) + rospy.loginfo( + 'Compressed urdf model is saved to {}' + .format(compressed_urdf_path)) + os.remove(lock_path) + finally: + if os.path.exists(urdf_path): + os.remove(urdf_path) + + +if __name__ == '__main__': + rospy.init_node('urdf_model_server') + server = URDFModelServer() + server.run() diff --git a/ros/kxr_models/package.xml b/ros/kxr_models/package.xml index 06c65b3b..c71e11fd 100644 --- a/ros/kxr_models/package.xml +++ b/ros/kxr_models/package.xml @@ -17,6 +17,7 @@ rviz joint_state_publisher_gui gazebo + urdfdom_py diff --git a/ros/kxr_models/python/kxr_models/__init__.py b/ros/kxr_models/python/kxr_models/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros/kxr_models/python/kxr_models/compress.py b/ros/kxr_models/python/kxr_models/compress.py new file mode 100644 index 00000000..687210f8 --- /dev/null +++ b/ros/kxr_models/python/kxr_models/compress.py @@ -0,0 +1,26 @@ +import os.path as osp +import tarfile + + +def make_tarfile(output_filename, source_dir=None): + """Make tarfile + + Parameters + ---------- + output_filename : str + output filename. If source_dir is None, output_filename is considered + as the source_dir name. + source_dir : str or None + target directory to compress. + + Returns + ------- + output_filename : str + output filename + """ + if source_dir is None: + source_dir = output_filename + output_filename = "{}.tar.gz".format(source_dir) + with tarfile.open(output_filename, "w:gz") as tar: + tar.add(source_dir, arcname=osp.basename(source_dir)) + return output_filename diff --git a/ros/kxr_models/python/kxr_models/md5sum_utils.py b/ros/kxr_models/python/kxr_models/md5sum_utils.py new file mode 100644 index 00000000..d827db98 --- /dev/null +++ b/ros/kxr_models/python/kxr_models/md5sum_utils.py @@ -0,0 +1,24 @@ +import hashlib + + +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() diff --git a/ros/kxr_models/python/kxr_models/path.py b/ros/kxr_models/python/kxr_models/path.py new file mode 100644 index 00000000..54380ee5 --- /dev/null +++ b/ros/kxr_models/python/kxr_models/path.py @@ -0,0 +1,37 @@ +import functools +import os.path as osp +from urllib.parse import urlparse + +import rospkg + + +@functools.lru_cache(maxsize=None) +def get_path_with_cache(ros_package): + rospack = rospkg.RosPack() + return rospack.get_path(ros_package) + + +def resolve_filepath(file_path, urdf_path): + if file_path.startswith('file://') and osp.exists(file_path[7:]): + return file_path[7:] + + parsed_url = urlparse(file_path) + if rospkg and parsed_url.scheme == 'package': + try: + ros_package = parsed_url.netloc + package_path = get_path_with_cache(ros_package) + resolve_filepath = package_path + parsed_url.path + if osp.exists(resolve_filepath): + return resolve_filepath + except rospkg.common.ResourceNotFound: + pass + + base_path = osp.abspath(urdf_path) + dirname = base_path + file_path = parsed_url.netloc + parsed_url.path + while dirname and dirname != '/': + resolved_filepath = osp.join(dirname, file_path) + if osp.exists(resolved_filepath): + return resolved_filepath + dirname = osp.dirname(dirname) + return None diff --git a/ros/kxr_models/python/kxr_models/urdf.py b/ros/kxr_models/python/kxr_models/urdf.py new file mode 100644 index 00000000..8b30c0e0 --- /dev/null +++ b/ros/kxr_models/python/kxr_models/urdf.py @@ -0,0 +1,58 @@ +import os +from pathlib import Path +import shutil +import xml.etree.ElementTree as ET + +from kxr_models.compress import make_tarfile +from kxr_models.md5sum_utils import checksum_md5 +from kxr_models.path import resolve_filepath + + +def aggregate_urdf_mesh_files(input_urdf_path, output_directory, + compress=False): + urdf_path = Path(input_urdf_path) + if not urdf_path.exists(): + raise OSError('No such urdf {}'.format(urdf_path)) + robot_md5sum = checksum_md5(urdf_path) + parser = ET.XMLParser(target=ET.TreeBuilder(insert_comments=True)) + tree = ET.parse(urdf_path, parser) + root = tree.getroot() + + output_directory = Path(output_directory) + + output_dir = output_directory / robot_md5sum + os.makedirs(output_dir, mode=0o777, exist_ok=True) + + for mesh in root.findall('.//mesh'): + filename = mesh.get('filename') + if filename is None: + continue + abs_path = resolve_filepath(filename, urdf_path) + if abs_path is not None: + mesh_robot_md5sum = checksum_md5(abs_path) + suffix = Path(abs_path).suffix + shutil.copy(abs_path, + output_dir / '{}{}'.format(mesh_robot_md5sum, + suffix)) + if suffix == '.obj': + mtl_path = Path(abs_path).with_suffix('.mtl') + if mtl_path.exists(): + shutil.copy(mtl_path, output_dir / mtl_path.name) + mesh.set('filename', f'package://kxr_models/models/urdf/{robot_md5sum}/{mesh_robot_md5sum}{suffix}') # NOQA + for mesh in root.findall('.//texture'): + filename = mesh.get('filename') + if filename is None: + continue + abs_path = resolve_filepath(filename, urdf_path) + if abs_path is not None: + mesh_robot_md5sum = checksum_md5(abs_path) + suffix = Path(abs_path).suffix + shutil.copy(abs_path, output_dir / '{}{}'.format(mesh_robot_md5sum, + suffix)) + mesh.set('filename', + f'package://kxr_models/models/urdf/{robot_md5sum}/{mesh_robot_md5sum}{suffix}') # NOQA + output_urdf_path = output_dir / '{}.urdf'.format(root.get('name')) + tree.write(output_urdf_path) + if compress: + return make_tarfile(output_dir) + return output_urdf_path diff --git a/ros/kxr_models/scripts/get_urdf_model.py b/ros/kxr_models/scripts/get_urdf_model.py new file mode 100755 index 00000000..8fd60236 --- /dev/null +++ b/ros/kxr_models/scripts/get_urdf_model.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python + +import os.path as osp + +import gdown +import rosgraph +import rospkg +import rospy +import tf +from urdf_parser_py.urdf import URDF + + +if __name__ == '__main__': + rospy.init_node('get_urdf_model') + 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 + '/model_server_port', 8123) + urdf_hash = rospy.get_param(clean_namespace + '/urdf_hash') + + master = rosgraph.Master("") + host = master.lookupNode("/rosout").split(':')[1][2:] + server_url = "http://{}:{}/urdf/{}.tar.gz".format( + host, port, urdf_hash) + + rospack = rospkg.RosPack() + kxr_models_path = rospack.get_path('kxr_models') + + compressed_urdf_path = osp.join(kxr_models_path, 'models', 'urdf', + "{}.tar.gz".format(urdf_hash)) + while not osp.exists(compressed_urdf_path): + rospy.loginfo('Waiting {} from server'.format(compressed_urdf_path)) + rospy.sleep(1.0) + gdown.cached_download( + url=server_url, + path=compressed_urdf_path) + gdown.extractall(osp.join(kxr_models_path, 'models', 'urdf', + "{}.tar.gz".format(urdf_hash))) + + if rospy.get_param('~connect_tf', False): + robot = URDF.from_parameter_server( + key=clean_namespace + '/robot_description') + root_link = robot.get_root() + + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + br = tf.TransformBroadcaster() + br.sendTransform((0, 0, 0), + (0, 0, 0, 1), + rospy.Time.now(), + root_link, + "map") diff --git a/ros/kxr_models/setup.py b/ros/kxr_models/setup.py new file mode 100644 index 00000000..3042d533 --- /dev/null +++ b/ros/kxr_models/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup + + +d = generate_distutils_setup( + packages=['kxr_models'], + package_dir={'': 'python'}, +) + +setup(**d) diff --git a/ros/kxreus/euslisp/kxr-interface.l b/ros/kxreus/euslisp/kxr-interface.l index da49c4d1..2c33bd70 100644 --- a/ros/kxreus/euslisp/kxr-interface.l +++ b/ros/kxreus/euslisp/kxr-interface.l @@ -5,10 +5,10 @@ (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")) + (let* ((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) + (setq port (truncate port)) (while (null hash) (ros::ros-info (format nil "Waiting rosparam ~A" hash-param-name)) (setq hash (ros::get-param hash-param-name nil)) @@ -108,7 +108,7 @@ (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)))) + :port (ros::get-param (if namespace (format nil "~A/model_server_port" namespace) "/model_server_port") 8123)))) (unless (ros::ok) (ros::roseus "kxr_eus_interface")) (unless (boundp '*ri*) (setq *ri* (instance kxr-interface :init *robot* :namespace namespace))) diff --git a/ros/kxreus/launch/eusmodel_server.launch b/ros/kxreus/launch/eusmodel_server.launch index 842bc1c0..abc38a10 100644 --- a/ros/kxreus/launch/eusmodel_server.launch +++ b/ros/kxreus/launch/eusmodel_server.launch @@ -1,28 +1,26 @@ - + + + + + + + + - - - - - - - - diff --git a/ros/kxreus/node_scripts/eus_model_server.py b/ros/kxreus/node_scripts/eus_model_server.py index 6d8889ff..3de87dca 100644 --- a/ros/kxreus/node_scripts/eus_model_server.py +++ b/ros/kxreus/node_scripts/eus_model_server.py @@ -1,10 +1,10 @@ #!/usr/bin/env python -import hashlib import os import tempfile from filelock import FileLock +from kxr_models.md5sum_utils import checksum_md5 import rospkg import rospy from skrobot.model import RobotModel @@ -12,29 +12,6 @@ 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): @@ -57,7 +34,7 @@ def run(self): robot_model = None rospack = rospkg.RosPack() - kxreus_path = rospack.get_path('kxreus') + kxr_models_path = rospack.get_path('kxr_models') while not rospy.is_shutdown(): rate.sleep() @@ -74,7 +51,8 @@ def run(self): previous_md5sum = md5sum eus_path = os.path.join( - kxreus_path, 'models', '{}.l'.format(md5sum)) + kxr_models_path, 'models', + 'euslisp', '{}.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',