Merge pull request #42 from iory/python-interface

Download viz model for python kxr interface

ros/kxr_controller/scripts/interface.py

@@ -6,6 +6,7 @@
 
 import IPython
 from kxr_controller.kxr_interface import KXRROSRobotInterface
+from kxr_models.download_urdf import download_urdf_mesh_files
 import rospy
 from skrobot.model import RobotModel
 
@@ -20,9 +21,12 @@ def main():
     args = parser.parse_args()
 
     rospy.init_node('kxr_interface', anonymous=True)
+
+    download_urdf_mesh_files()
+
     robot_model = RobotModel()
     robot_model.load_urdf_from_robot_description(
-        args.namespace + '/robot_description')
+        args.namespace + '/robot_description_viz')
     ri = KXRROSRobotInterface(  # NOQA
         robot_model, namespace=args.namespace, controller_timeout=60.0)
 

ros/kxr_models/python/kxr_models/download_urdf.py

@@ -0,0 +1,39 @@
+import os.path as osp
+
+import gdown
+from kxr_models.ros import get_namespace
+import rosgraph
+import rospkg
+import rospy
+
+
+def download_urdf_mesh_files():
+    clean_namespace = get_namespace()
+    port = rospy.get_param(clean_namespace + '/model_server_port', 8123)
+
+    urdf_hash = rospy.get_param(clean_namespace + '/urdf_hash', None)
+    rate = rospy.Rate(1)
+    while not rospy.is_shutdown() and urdf_hash is None:
+        rate.sleep()
+        rospy.loginfo('Waiting rosparam {} set'.format(
+            clean_namespace + '/urdf_hash'))
+        urdf_hash = rospy.get_param(clean_namespace + '/urdf_hash', None)
+
+    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)))

ros/kxr_models/python/kxr_models/ros.py

@@ -0,0 +1,9 @@
+import rospy
+
+
+def get_namespace():
+    full_namespace = rospy.get_namespace()
+    last_slash_pos = full_namespace.rfind('/')
+    clean_namespace = full_namespace[:last_slash_pos] \
+        if last_slash_pos != 0 else ''
+    return clean_namespace

ros/kxr_models/scripts/get_urdf_model.py

@@ -1,53 +1,29 @@
 #!/usr/bin/env python
 
-import os.path as osp
-
-import gdown
-import rosgraph
-import rospkg
+from kxr_models.download_urdf import download_urdf_mesh_files
+from kxr_models.ros import get_namespace
 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)))
+    clean_namespace = get_namespace()
+    download_urdf_mesh_files()
 
     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)
+        rate = rospy.Rate(rospy.get_param('~rate', 20))
         while not rospy.is_shutdown():
             br = tf.TransformBroadcaster()
             br.sendTransform((0, 0, 0),
                              (0, 0, 0, 1),
                              rospy.Time.now(),
                              root_link,
                              "map")
+            rate.sleep()