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())
