def add_sphere(self, name=u'sphere', size=1, frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): """ If pose is used, frame_id, position and orientation are ignored. :type name: str :param size: radius in m :type size: list :type frame_id: str :type position: list :type orientation: list :type pose: PoseStamped :rtype: UpdateWorldResponse """ object = WorldBody() object.type = WorldBody.PRIMITIVE_BODY object.name = str(name) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) object.shape.type = SolidPrimitive.SPHERE object.shape.dimensions.append(size) req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose) return self._update_world_srv.call(req)
def make_world_body_sphere(name=u'sphere', radius=1): sphere = WorldBody() sphere.type = WorldBody.PRIMITIVE_BODY sphere.name = str(name) sphere.shape.type = SolidPrimitive.SPHERE sphere.shape.dimensions.append(radius) return sphere
def add_mesh(self, name=u'mesh', mesh=u'', frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): """ If pose is used, frame_id, position and orientation are ignored. :type name: str :param mesh: path to the meshes location. e.g. package://giskardpy/test/urdfs/meshes/bowl_21.obj :type frame_id: str :type position: list :type orientation: list :type pose: PoseStamped :rtype: UpdateWorldResponse """ object = WorldBody() object.type = WorldBody.MESH_BODY object.name = str(name) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) object.mesh = mesh req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose) return self._update_world_srv.call(req)
def make_box(self, name=u'box', size=(1, 1, 1)): box = WorldBody() box.type = WorldBody.PRIMITIVE_BODY box.name = str(name) box.shape.type = SolidPrimitive.BOX box.shape.dimensions.append(size[0]) box.shape.dimensions.append(size[1]) box.shape.dimensions.append(size[2]) return box
def make_world_body_cylinder(name=u'cylinder', height=1, radius=1): cylinder = WorldBody() cylinder.type = WorldBody.PRIMITIVE_BODY cylinder.name = str(name) cylinder.shape.type = SolidPrimitive.CYLINDER cylinder.shape.dimensions = [0, 0] cylinder.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = height cylinder.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS] = radius return cylinder
def make_world_body_box(name=u'box', x_length=1, y_length=1, z_length=1): box = WorldBody() box.type = WorldBody.PRIMITIVE_BODY box.name = str(name) box.shape.type = SolidPrimitive.BOX box.shape.dimensions.append(x_length) box.shape.dimensions.append(y_length) box.shape.dimensions.append(z_length) return box
def remove_object(self, name): """ :param name: :type name: str :return: :rtype: UpdateWorldResponse """ object = WorldBody() object.name = str(name) req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False, PoseStamped()) return self.update_world.call(req)
def add_table_request(): table = WorldBody() table.type = WorldBody.MESH_BODY table.name = "table" pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "map" pose.pose.position = Point(-1.4, -1.05, 0.0) pose.pose.orientation = Quaternion(0, 0, 1, 0) table.mesh = 'package://iai_kitchen/meshes/misc/big_table_1.stl' return UpdateWorldRequest(UpdateWorldRequest.ADD, table, False, pose)
def add_wand_request(): wand = WorldBody() wand.type = WorldBody.PRIMITIVE_BODY wand.name = 'wand' wand.shape.type = SolidPrimitive.CYLINDER wand.shape.dimensions.append(0.15) # height of 15cm wand.shape.dimensions.append(0.005) # radius of 0.5cm pose = PoseStamped() pose.header.frame_id = 'l_gripper_tool_frame' pose.pose.orientation.w = 1.0 return UpdateWorldRequest(UpdateWorldRequest.ADD, wand, True, pose)
def add_sphere_request(): sphere = WorldBody() sphere.type = WorldBody.PRIMITIVE_BODY sphere.name = 'sphere' pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = 'map' pose.pose.position = Point(-1.25, -1.0, 0.77) pose.pose.orientation = Quaternion(0, 0, 0, 1) sphere.shape.type = SolidPrimitive.SPHERE sphere.shape.dimensions.append(0.05) # radius of 5cm return UpdateWorldRequest(UpdateWorldRequest.ADD, sphere, False, pose)
def add_cone_request(): cone = WorldBody() cone.type = WorldBody.PRIMITIVE_BODY cone.name = 'cone' pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = 'map' pose.pose.orientation = Quaternion(0, 0, 0, 1) cone.shape.type = SolidPrimitive.CONE cone.shape.dimensions.append(0.01) # height of 1cm cone.shape.dimensions.append(0.05) # radius of 5cm return UpdateWorldRequest(UpdateWorldRequest.ADD, cone, False, pose)
def add_cylinder_request(): cylinder = WorldBody() cylinder.type = WorldBody.PRIMITIVE_BODY cylinder.name = 'cylinder' pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = 'map' pose.pose.position = Point(-1.1, -1.0, 0.77) pose.pose.orientation = Quaternion(0, 0, 0, 1) cylinder.shape.type = SolidPrimitive.CYLINDER cylinder.shape.dimensions.append(0.1) # height of 10cm cylinder.shape.dimensions.append(0.03) # radius of 3cm return UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, False, pose)
def add_mesh(self, name=u'mesh', mesh=u'', frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): object = WorldBody() object.type = WorldBody.MESH_BODY object.name = str(name) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) object.mesh = mesh req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose) return self.update_world.call(req)
def add_box_request(): box = WorldBody() box.type = WorldBody.PRIMITIVE_BODY box.name = 'box' pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = 'map' pose.pose.position = Point(-1.5, -1.0, 0.745) pose.pose.orientation = Quaternion(0, 0, 0, 1) box.shape.type = SolidPrimitive.BOX box.shape.dimensions.append(0.2) # X of 20cm box.shape.dimensions.append(0.3) # Y of 30cm box.shape.dimensions.append(0.05) # Z of 5cm return UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
def test_unsupported_options(self, kitchen_setup): """ :type kitchen_setup: GiskardTestWrapper """ wb = WorldBody() pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(u'map') pose.pose.position = Point() pose.pose.orientation = Quaternion(w=1) wb.type = WorldBody.URDF_BODY req = UpdateWorldRequest(UpdateWorldRequest.ADD, wb, True, pose) assert kitchen_setup.wrapper.update_world.call( req).error_codes == UpdateWorldResponse.UNSUPPORTED_OPTIONS
def add_sphere(self, name=u'sphere', size=1, frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): object = WorldBody() object.type = WorldBody.PRIMITIVE_BODY object.name = str(name) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) object.shape.type = SolidPrimitive.SPHERE object.shape.dimensions.append(size) req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose) return self.update_world.call(req)
def clear_world(self): """ :rtype: UpdateWorldResponse """ req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(), False, PoseStamped()) return self.update_world.call(req)
def test_invalid_update_world(self, zero_pose): """ :type zero_pose: GiskardTestWrapper """ req = UpdateWorldRequest(42, WorldBody(), True, PoseStamped()) assert zero_pose.wrapper.update_world.call( req).error_codes == UpdateWorldResponse.INVALID_OPERATION
def clear_world(self): """ Removes any objects and attached objects from Giskard's world and reverts the robots urdf to what it got from the parameter server. :rtype: UpdateWorldResponse """ req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(), False, PoseStamped()) return self._update_world_srv.call(req)
def add_cylinder(self, name=u'cylinder', size=(1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1)): object = WorldBody() object.type = WorldBody.PRIMITIVE_BODY object.name = str(name) pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) object.shape.type = SolidPrimitive.CYLINDER object.shape.dimensions.append(size[0]) object.shape.dimensions.append(size[1]) req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose) return self.update_world.call(req)
def test_corrupt_shape_error(self, zero_pose): """ :type zero_pose: GiskardTestWrapper """ req = UpdateWorldRequest( UpdateWorldRequest.ADD, WorldBody(type=WorldBody.PRIMITIVE_BODY, shape=SolidPrimitive(type=42)), True, PoseStamped()) assert zero_pose.wrapper.update_world.call( req).error_codes == UpdateWorldResponse.CORRUPT_SHAPE_ERROR
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_cylinder(self, name=u'cylinder', height=1, radius=1, frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1), pose=None): """ If pose is used, frame_id, position and orientation are ignored. :type name: str :param height: in m :type height: float :param radius: in m :type radius: float :type frame_id: str :type position: list :type orientation: list :type pose: PoseStamped :rtype: UpdateWorldResponse """ object = WorldBody() object.type = WorldBody.PRIMITIVE_BODY object.name = str(name) if pose is None: pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = str(frame_id) pose.pose.position = Point(*position) pose.pose.orientation = Quaternion(*orientation) object.shape.type = SolidPrimitive.CYLINDER object.shape.dimensions = [0, 0] object.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = height object.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS] = radius req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose) return self._update_world_srv.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 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 rem_sphere_request(): sphere = WorldBody() sphere.name = 'sphere' return UpdateWorldRequest(UpdateWorldRequest.REMOVE, sphere, False, PoseStamped())
def make_urdf_world_body(name, urdf): wb = WorldBody() wb.name = name wb.type = wb.URDF_BODY wb.urdf = urdf return wb
def clear_world_request(): return UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(), False, PoseStamped())