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)
def make_urdf_world_body(name, urdf): wb = WorldBody() wb.name = name wb.type = wb.URDF_BODY wb.urdf = urdf return wb