def add_urdf(self, name, urdf, pose, js_topic=u'', set_js_topic=None):
"""
Adds a urdf to the world
:param name: name it will have in the world
:type name: str
:param urdf: urdf as string, no path
:type urdf: str
:type pose: PoseStamped
:param js_topic: Giskard will listen on that topic for joint states and update the urdf accordingly
:type js_topic: str
:param set_js_topic: A topic that the python wrapper will use to set the urdf joint state.
If None, set_js_topic == js_topic
:type set_js_topic: str
:return: UpdateWorldResponse
"""
if set_js_topic is None:
set_js_topic = js_topic
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False,
pose)
if js_topic:
# FIXME publisher has to be removed, when object gets deleted
# FIXME there could be sync error, if objects get added/removed by something else
self._object_js_topics[name] = rospy.Publisher(set_js_topic,
JointState,
queue_size=10)
return self._update_world_srv.call(req)
def add_kitchen_request():
pr2 = WorldBody()
pr2.name = "kitchen"
pr2.type = WorldBody.URDF_BODY
pr2.urdf = rospy.get_param('/kitchen_description')
pr2.joint_state_topic = '/kitchen_joint_states'
tf = tf2_ros.BufferClient('tf2_buffer_server')
tf.wait_for_server(rospy.Duration(0.5))
transform = tf.lookup_transform('map', 'world', rospy.Time.now(),
rospy.Duration(0.5))
return UpdateWorldRequest(UpdateWorldRequest.ADD, pr2, False,
to_pose_stamped_msg(transform))
def add_urdf(self, name, urdf, pose, js_topic=u''):
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False, pose)
if js_topic:
# FIXME publisher has to be removed, when object gets deleted
# FIXME there could be sync error, if objects get added/removed by something else
self.object_js_topics[name] = rospy.Publisher(js_topic, JointState, queue_size=10)
return self.update_world.call(req)
def add_urdf(self, name, urdf, js_topic, pose):
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False,
pose)
self.object_js_topics[name] = rospy.Publisher(js_topic,
JointState,
queue_size=10)
return self.update_world.call(req)
