def obj_type(type_name, bbx, bby, bbz):
ot = ObjectType()
ot.name = type_name
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions = [bbx, bby, bbz]
return ot
def obj_type(type_name, bbx, bby, bbz, container=False):
ot = ObjectType()
ot.name = type_name
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions = [bbx, bby, bbz]
ot.container = container
return ot
def setUp(self):
self.ui_core = UICore(0, 0, 2, 1, 1000, 1234)
self.type1 = ObjectType()
self.type1.name = "type1"
self.type1.bbox.type = SolidPrimitive.BOX
self.type1.bbox.dimensions = [0.1, 0.1, 0.1]
self.type2 = ObjectType()
self.type2.name = "type2"
self.type2.bbox.type = SolidPrimitive.BOX
self.type2.bbox.dimensions = [0.1, 0.1, 0.1]
self.ps = PoseStamped()
self.ps.pose.orientation.w = 1.0
def srv_get_object_cb(self, request):
return getObjectTypeResponse(success=True,
object_type=ObjectType(
name=request.name,
container=True,
bbox=SolidPrimitive(
type=1,
dimensions=[0.1, 0.1, 0.1])))
def test_object_type(self):
ot = ObjectType()
ot.name = "profile_test_1"
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions.append(0.1)
ot.bbox.dimensions.append(0.1)
ot.bbox.dimensions.append(0.1)
try:
resp_store = self.store_object_srv(ot)
except rospy.ServiceException:
pass
self.assertEquals(resp_store.success, True, "object_type_store")
try:
resp_get = self.get_object_srv(name="profile_test_1")
except rospy.ServiceException:
pass
self.assertEquals(resp_get.success, True, "object_type_get")
self.assertEquals(resp_get.object_type.name, "profile_test_1", "object_type_get")
def test_object_type(self):
ot = ObjectType()
ot.name = "profile_test_1"
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions.append(0.1)
ot.bbox.dimensions.append(0.1)
ot.bbox.dimensions.append(0.1)
try:
resp_store = self.store_object_srv(ot)
except rospy.ServiceException:
pass
self.assertEquals(resp_store.success, True, "object_type_store")
try:
resp_get = self.get_object_srv(name="profile_test_1")
except rospy.ServiceException:
pass
self.assertEquals(resp_get.success, True, "object_type_get")
self.assertEquals(resp_get.object_type.name, "profile_test_1", "object_type_get")
def main(args):
global p
rospy.init_node('art_db_service_tester', anonymous=True)
# -------------------------------------------------------------------------------------------
# PROGRAMS
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 0
prog.header.name = "Advanced pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = "GetReady"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 3
p.on_failure = 0
p.type = "WaitForUser"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 4
p.on_failure = 0
p.type = "PickFromFeeder"
p.object.append("")
pf = PoseStamped()
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 4
p.on_success = 6
p.on_failure = 0
p.type = "PlaceToPose"
p.ref_id.append(3)
pp = PoseStamped()
pp.header.frame_id = "marker"
p.pose.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 6
p.on_success = 7
p.on_failure = 0
p.type = "GetReady"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 7
p.on_success = 8
p.on_failure = 0
p.type = "WaitUntilUserFinishes"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 8
p.on_success = 9
p.on_failure = 0
p.type = "PickFromFeeder"
p.object.append("")
pp.header.frame_id = "marker"
pf = PoseStamped()
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 9
p.on_success = 1
p.on_failure = 0
p.type = "PlaceToPose"
p.ref_id.append(8)
pp = PoseStamped()
p.pose.append(pp)
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 2
prog.header.name = "Classic pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = "GetReady"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 3
p.on_failure = 0
p.type = "WaitForUser"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 4
p.on_failure = 0
p.type = "PickFromPolygon"
p.object.append("")
pp = PolygonStamped()
pp.header.frame_id = "marker"
p.polygon.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 4
p.on_success = 5
p.on_failure = 0
p.type = "PlaceToPose"
p.ref_id.append(3)
pp = PoseStamped()
pp.header.frame_id = "marker"
p.pose.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 5
p.on_success = 6
p.on_failure = 0
p.type = "GetReady"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 6
p.on_success = 7
p.on_failure = 0
p.type = "WaitUntilUserFinishes"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 7
p.on_success = 8
p.on_failure = 0
p.type = "PickFromPolygon"
p.object.append("")
p.object.append("profile_20_60")
pp = PolygonStamped()
pp.header.frame_id = "marker"
p.polygon.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 8
p.on_success = 1
p.on_failure = 0
p.type = "PlaceToPose"
p.ref_id.append(7)
pp = PoseStamped()
pp.header.frame_id = "marker"
p.pose.append(pp)
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 3
prog.header.name = "Welding seam"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = "GetReady"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 2
p.on_failure = 0
p.type = "WeldingSeam"
p.object.append("")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.4
pf.pose.position.y = 0.4
pf.pose.position.z = 0.30
pf.pose.orientation.x = -0.707
pf.pose.orientation.y = 0
pf.pose.orientation.z = 0.707
pf.pose.orientation.w = 0
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.5
p.pose.append(deepcopy(pf))
pf.pose.position.y = 0.2
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.4
p.pose.append(deepcopy(pf))
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 4
prog.header.name = "Welding points"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = "GetReady"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 2
p.on_failure = 0
p.type = "WeldingPoints"
p.object.append("")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.4
pf.pose.position.y = 0.4
pf.pose.position.z = 0.30
pf.pose.orientation.x = -0.707
pf.pose.orientation.y = 0
pf.pose.orientation.z = 0.707
pf.pose.orientation.w = 0
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.5
p.pose.append(deepcopy(pf))
pf.pose.position.y = 0.2
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.4
p.pose.append(deepcopy(pf))
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 5
prog.header.name = "Drill points"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = "GetReady"
p.flags = [KeyValue]
p.flags[0].key = "CLEAR_OBJECT_FLAGS"
p.flags[0].value = "true"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 2
p.on_failure = 3
p.type = "DrillPoints"
p.object.append("profile_20_60")
pf = PoseStamped()
pf.header.frame_id = "object_id_profile_20_60"
pf.pose.position.x = 0.4
pf.pose.position.y = 0.4
pf.pose.position.z = 0.30
pf.pose.orientation.x = -0.707
pf.pose.orientation.y = 0
pf.pose.orientation.z = 0.707
pf.pose.orientation.w = 0
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.5
p.pose.append(deepcopy(pf))
pf.pose.position.y = 0.2
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.4
p.pose.append(deepcopy(pf))
dp = PolygonStamped()
dp.header.frame_id = "marker"
p.polygon.append(dp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 3
p.on_failure = 0
p.type = "DrillPoints"
p.object.append("profile_20_60")
pf = PoseStamped()
pf.header.frame_id = "object_id_profile_20_60"
pf.pose.position.x = 0.4
pf.pose.position.y = 0.4
pf.pose.position.z = 0.30
pf.pose.orientation.x = -0.707
pf.pose.orientation.y = 0
pf.pose.orientation.z = 0.707
pf.pose.orientation.w = 0
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.5
p.pose.append(deepcopy(pf))
pf.pose.position.y = 0.2
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.4
p.pose.append(deepcopy(pf))
dp = PolygonStamped()
dp.header.frame_id = "marker"
p.polygon.append(dp)
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
"""
prog = Program()
prog.header.id = 9
prog.header.name = "Place to grid"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First Block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = "GetReady"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 3
p.on_failure = 0
p.type = "WaitForUser"
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 4
p.on_failure = 0
p.type = "PickFromPolygon"
p.object.append("")
p.object.append("profile_20_60")
pp = PolygonStamped()
pp.header.frame_id = "marker"
p.polygon.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 4
p.on_success = 3
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_GRID
pp = PolygonStamped()
p.polygon.append(pp)
p.ref_id.append(3)
p.pose.append(PoseStamped())
p.pose.append(PoseStamped())
p.pose.append(PoseStamped())
p.pose.append(PoseStamped())
pb.items.append(deepcopy(p))
store_program(prog)
"""
# -------------------------------------------------------------------------------------------
# OBJECT TYPES
# -------------------------------------------------------------------------------------------
ot = ObjectType()
ot.name = "profile_20_60"
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions.append(0.05)
ot.bbox.dimensions.append(0.05)
ot.bbox.dimensions.append(0.16)
store_object_type(ot)
ot = ObjectType()
ot.name = "placka"
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions.append(0.1)
ot.bbox.dimensions.append(0.1)
ot.bbox.dimensions.append(0.008)
store_object_type(ot)
ot = ObjectType()
ot.name = "wood_46_150"
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions.append(0.046)
ot.bbox.dimensions.append(0.046)
ot.bbox.dimensions.append(0.154)
store_object_type(ot)
ot = ObjectType()
ot.name = "wood_46_300"
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions.append(0.046)
ot.bbox.dimensions.append(0.046)
ot.bbox.dimensions.append(0.298)
store_object_type(ot)
ot = ObjectType()
ot.name = "karta"
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions.append(0.08)
ot.bbox.dimensions.append(0.04)
ot.bbox.dimensions.append(0.002)
store_object_type(ot)
ot.name = "profile_20_80"
store_object_type(ot)
def main(args):
global p
rospy.init_node('art_db_service_tester', anonymous=True)
# -------------------------------------------------------------------------------------------
# PROGRAMS
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 0
prog.header.name = "Advanced pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 3
p.on_failure = 0
p.type = ProgramItem.WAIT_FOR_USER
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_FEEDER
p.object.append("profile_20_60")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 4
p.on_success = 5
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(3)
p.ref_id.append(5)
pp = PoseStamped()
pp.header.frame_id = "marker"
pp.pose.position.x = 0.75
pp.pose.position.y = 0.5
p.pose.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 5
p.on_success = 6
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_FEEDER
p.object.append("profile_20_60")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 6
p.on_success = 7
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 7
p.on_success = 8
p.on_failure = 0
p.type = ProgramItem.WAIT_UNTIL_USER_FINISHES
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 8
p.on_success = 9
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_POLYGON
p.object.append("profile_20_60")
pp = PolygonStamped()
pp.header.frame_id = "marker"
pp.polygon.points.append(Point32(0.4, 0.1, 0))
pp.polygon.points.append(Point32(1.0, 0.1, 0))
pp.polygon.points.append(Point32(1.0, 0.6, 0))
pp.polygon.points.append(Point32(0.4, 0.6, 0))
p.polygon.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 9
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(8)
pp = PoseStamped()
pp.header.frame_id = "marker"
pp.pose.position.x = 0.75
pp.pose.position.y = 0.5
p.pose.append(pp)
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 3
prog.header.name = "Basic pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 3
p.on_failure = 0
p.type = ProgramItem.WAIT_FOR_USER
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.PICK_OBJECT_ID
p.object.append("")
pf = PoseStamped()
pf.header.frame_id = "marker"
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 4
p.on_success = 3
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(3)
pp = PoseStamped()
pp.header.frame_id = "marker"
p.pose.append(pp)
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 2
prog.header.name = "Classic pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 3
p.on_failure = 0
p.type = ProgramItem.WAIT_FOR_USER
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_POLYGON
p.object.append("")
pp = PolygonStamped()
pp.header.frame_id = "marker"
p.polygon.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 4
p.on_success = 5
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(3)
pp = PoseStamped()
pp.header.frame_id = "marker"
p.pose.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 5
p.on_success = 6
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 6
p.on_success = 7
p.on_failure = 0
p.type = ProgramItem.WAIT_UNTIL_USER_FINISHES
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 7
p.on_success = 8
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_POLYGON
p.object.append("")
p.object.append("profile_20_60")
pp = PolygonStamped()
pp.header.frame_id = "marker"
p.polygon.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 8
p.on_success = 1
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(7)
pp = PoseStamped()
pp.header.frame_id = "marker"
p.pose.append(pp)
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 3
prog.header.name = "Welding seam"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.WELDING_SEAM
p.object.append("")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.4
pf.pose.position.y = 0.4
pf.pose.position.z = 0.30
pf.pose.orientation.x = -0.707
pf.pose.orientation.y = 0
pf.pose.orientation.z = 0.707
pf.pose.orientation.w = 0
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.5
p.pose.append(deepcopy(pf))
pf.pose.position.y = 0.2
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.4
p.pose.append(deepcopy(pf))
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 4
prog.header.name = "Welding points"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.WELDING_POINTS
p.object.append("")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.4
pf.pose.position.y = 0.4
pf.pose.position.z = 0.30
pf.pose.orientation.x = -0.707
pf.pose.orientation.y = 0
pf.pose.orientation.z = 0.707
pf.pose.orientation.w = 0
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.5
p.pose.append(deepcopy(pf))
pf.pose.position.y = 0.2
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.4
p.pose.append(deepcopy(pf))
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 5
prog.header.name = "Drill points"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.DRILL_POINTS
p.object.append("")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.4
pf.pose.position.y = 0.4
pf.pose.position.z = 0.30
pf.pose.orientation.x = -0.707
pf.pose.orientation.y = 0
pf.pose.orientation.z = 0.707
pf.pose.orientation.w = 0
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.5
p.pose.append(deepcopy(pf))
pf.pose.position.y = 0.2
p.pose.append(deepcopy(pf))
pf.pose.position.x = 0.4
p.pose.append(deepcopy(pf))
pb.items.append(deepcopy(p))
store_program(prog)
# -------------------------------------------------------------------------------------------
# OBJECT TYPES
# -------------------------------------------------------------------------------------------
ot = ObjectType()
ot.name = "profile_20_60"
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions.append(0.04)
ot.bbox.dimensions.append(0.04)
ot.bbox.dimensions.append(0.13)
store_object_type(ot)
ot.name = "profile_20_80"
store_object_type(ot)
print "Service call failed: " + str(e)
return
rospy.wait_for_service('/art/db/program/get')
try:
get_program_srv = rospy.ServiceProxy('/art/db/program/get', getProgram)
resp = get_program_srv(id=0)
print resp
except rospy.ServiceException, e:
print "Service call failed: " + str(e)
return
rospy.wait_for_service('/art/db/object_type/store')
ot = ObjectType()
ot.name = "profile_20_60"
ot.bbox.type = SolidPrimitive.BOX
ot.bbox.dimensions.append(0.05)
ot.bbox.dimensions.append(0.05)
ot.bbox.dimensions.append(0.16)
try:
store_object_srv = rospy.ServiceProxy('/art/db/object_type/store', storeObjectType)
resp = store_object_srv(ot)
except rospy.ServiceException, e:
print "Service call failed: " + str(e)
return
ot.name = "profile_20_80"
