def test_calibrate_base_plane_response():
response = {
"timestamp": {
"sec": 1593000384,
"nsec": 386523224
},
"plane": {
"distance": -0.7552042203510121,
"normal": {
"y": -0.02136428281262259,
"x": 0.02327135522722706,
"z": 0.999500881163089,
},
"pose_frame": "camera",
},
"return_code": {
"message": "test",
"value": 0
},
}
ros_res = convert_dictionary_to_ros_message(
CalibrateBasePlane._response_class(), response)
assert_plane(ros_res.plane, response["plane"])
assert ros_res.pose_frame == response["plane"]["pose_frame"]
assert ros_res.return_code.value == response["return_code"]["value"]
assert ros_res.return_code.message == response["return_code"]["message"]
assert_timestamp(ros_res.timestamp, response["timestamp"])
def test_get_base_plane_calibration():
ros_req = GetBasePlaneCalibration._request_class()
# in silhouettematch, the robot_pose can also be used in camera frame!
ros_req.pose_frame = "camera"
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.pose_frame == api_req["pose_frame"]
assert_pose(ros_req.robot_pose, api_req["robot_pose"])
ros_req.pose_frame = "external"
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.pose_frame == api_req["pose_frame"]
assert_pose(ros_req.robot_pose, api_req["robot_pose"])
response = {
"plane": {
"distance": -0.7552042203510121,
"normal": {
"y": -0.02136428281262259,
"x": 0.02327135522722706,
"z": 0.999500881163089,
},
"pose_frame": "camera",
},
"return_code": {
"message": "foo",
"value": 1234
},
}
ros_res = convert_dictionary_to_ros_message(
GetBasePlaneCalibration._response_class(), response)
assert_plane(ros_res.plane, response["plane"])
assert ros_res.pose_frame == response["plane"]["pose_frame"]
assert ros_res.return_code.value == response["return_code"]["value"]
assert ros_res.return_code.message == response["return_code"]["message"]
def test_get_lcs():
ros_req = GetLoadCarriers._request_class()
ros_req.load_carrier_ids = ["foo", "bar"]
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.load_carrier_ids == api_req["load_carrier_ids"]
api_res = {
"load_carriers": [{
"id": "test",
"inner_dimensions": {
"x": 0.8,
"y": 0.2,
"z": 0.8
},
"outer_dimensions": {
"x": 1.0,
"y": 0.6,
"z": 1.0
},
"pose": {
"orientation": {
"w": 0.0,
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
},
"pose_frame": "",
"rim_thickness": {
"x": 0.0,
"y": 0.0
},
}],
"return_code": {
"message": "test",
"value": 1234
},
}
ros_res = convert_dictionary_to_ros_message(
GetLoadCarriers._response_class(), api_res)
assert_lc(ros_res.load_carriers[0], api_res["load_carriers"][0])
assert ros_res.return_code.value == api_res["return_code"]["value"]
assert ros_res.return_code.message == api_res["return_code"]["message"]
def test_detect_items():
ros_req = DetectItems._request_class()
ros_req.pose_frame = "camera"
im = ItemModel(type=ItemModel.RECTANGLE)
im.rectangle.min_dimensions.x = 0.2
im.rectangle.min_dimensions.y = 0.3
im.rectangle.max_dimensions.x = 0.4
im.rectangle.max_dimensions.y = 0.5
ros_req.item_models.append(im)
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.pose_frame == api_req["pose_frame"]
for i, ros_im in enumerate(ros_req.item_models):
api_im = api_req["item_models"][i]
assert_primitives(ros_im, api_im)
# don't send robot_pose if pose_frame is camera
assert "robot_pose" not in api_req
ros_req.pose_frame = "external"
ros_req.robot_pose.position.y = 1.0
ros_req.robot_pose.orientation.z = 1.0
api_req = convert_ros_message_to_dictionary(ros_req)
assert "robot_pose" in api_req
assert_pose(ros_req.robot_pose, api_req["robot_pose"])
assert len(api_req["item_models"]) == 1
assert "region_of_interest_id" not in api_req
assert "load_carrier_id" not in api_req
assert "load_carrier_compartment" not in api_req
ros_req.region_of_interest_id = "testroi"
ros_req.load_carrier_id = "mylc"
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.region_of_interest_id == api_req["region_of_interest_id"]
assert ros_req.load_carrier_id == api_req["load_carrier_id"]
assert "load_carrier_compartment" not in api_req
# add valid compartment
ros_req.load_carrier_compartment.box.x = 0.1
ros_req.load_carrier_compartment.box.y = 0.2
ros_req.load_carrier_compartment.box.z = 0.3
api_req = convert_ros_message_to_dictionary(ros_req)
assert_primitives(ros_req.load_carrier_compartment,
api_req["load_carrier_compartment"])
api_res = {
"timestamp": {
"sec": 1587035449,
"nsec": 321465164
},
"return_code": {
"message": "abcdef",
"value": 1234
},
"load_carriers": [{
"pose": {
"position": {
"y": -0.0168824336375496,
"x": 0.1189406043995812,
"z": 0.8875302697155399,
},
"orientation": {
"y": -0.04632486703729271,
"x": 0.998664879978751,
"z": 0.006342615882086765,
"w": 0.02195985184108778,
},
},
"pose_frame": "camera",
"inner_dimensions": {
"y": 0.27,
"x": 0.37,
"z": 0.14
},
"outer_dimensions": {
"y": 0.3,
"x": 0.4,
"z": 0.2
},
"overfilled": True,
"rim_thickness": {
"y": 0.0,
"x": 0.0
},
"id": "xyz",
}],
"items": [
{
"uuid": "ee0b2bc4-7fd2-11ea-a789-00142d2cd4ce",
"pose_frame": "camera",
"timestamp": {
"sec": 1587035449,
"nsec": 321465164
},
"pose": {
"position": {
"y": 0.0015526703948114742,
"x": 0.018541338820357283,
"z": 0.9062130178042246,
},
"orientation": {
"y": -0.29071987884727973,
"x": 0.9563493015403196,
"z": 0.019845952916433495,
"w": 0.02200235531039019,
},
},
"type": "RECTANGLE",
"rectangle": {
"y": 0.05698329755083764,
"x": 0.10302717395647137
},
},
{
"uuid": "ee0b2f34-7fd2-11ea-a789-00142d2cd4ce",
"pose_frame": "camera",
"timestamp": {
"sec": 1587035449,
"nsec": 321465164
},
"pose": {
"position": {
"y": -0.07512277927447901,
"x": 0.01837425822827067,
"z": 0.9083435428699417,
},
"orientation": {
"y": -0.10244608240444944,
"x": 0.9947325521529258,
"z": 0.0022848846348791315,
"w": 0.0025940681395760523,
},
},
"type": "RECTANGLE",
"rectangle": {
"y": 0.05739744695533501,
"x": 0.10506054260132827
},
},
],
}
ros_res = convert_dictionary_to_ros_message(DetectItems._response_class(),
api_res)
ros_lc = ros_res.load_carriers[0]
api_lc = api_res["load_carriers"][0]
assert_lc(ros_lc, api_lc, api_res["timestamp"])
assert ros_lc.overfilled == api_lc["overfilled"]
for i, ros_item in enumerate(ros_res.items):
api_item = api_res["items"][i]
assert_item(ros_item, api_item)
def test_compute_grasps():
ros_req = ComputeGrasps._request_class()
ros_req.pose_frame = "camera"
ros_req.suction_surface_length = 0.05
ros_req.suction_surface_width = 0.02
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.pose_frame == api_req["pose_frame"]
assert ros_req.suction_surface_length == api_req["suction_surface_length"]
assert ros_req.suction_surface_width == api_req["suction_surface_width"]
# don't send robot_pose if pose_frame is camera
assert "robot_pose" not in api_req
ros_req.pose_frame = "external"
ros_req.robot_pose.position.y = 1.0
ros_req.robot_pose.orientation.z = 1.0
api_req = convert_ros_message_to_dictionary(ros_req)
assert "robot_pose" in api_req
assert_pose(ros_req.robot_pose, api_req["robot_pose"])
assert len(api_req["item_models"]) == 0
assert "region_of_interest_id" not in api_req
assert "load_carrier_id" not in api_req
assert "load_carrier_compartment" not in api_req
assert "collision_detection" not in api_req
ros_req.region_of_interest_id = "testroi"
ros_req.load_carrier_id = "mylc"
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.region_of_interest_id == api_req["region_of_interest_id"]
assert ros_req.load_carrier_id == api_req["load_carrier_id"]
assert "load_carrier_compartment" not in api_req
assert "collision_detection" not in api_req
# add valid compartment
ros_req.load_carrier_compartment.box.x = 0.1
ros_req.load_carrier_compartment.box.y = 0.2
ros_req.load_carrier_compartment.box.z = 0.3
api_req = convert_ros_message_to_dictionary(ros_req)
assert_primitives(ros_req.load_carrier_compartment,
api_req["load_carrier_compartment"])
assert "collision_detection" not in api_req
ros_req.collision_detection.gripper_id = "mygripper"
ros_req.collision_detection.pre_grasp_offset.x = 1.0
ros_req.collision_detection.pre_grasp_offset.y = 0.1
ros_req.collision_detection.pre_grasp_offset.z = -0.5
api_req = convert_ros_message_to_dictionary(ros_req)
assert_primitives(ros_req.collision_detection,
api_req["collision_detection"])
api_res = {
"timestamp": {
"sec": 1587033051,
"nsec": 179231838
},
"grasps": [
{
"uuid": "589ce564-7fcd-11ea-a789-00142d2cd4ce",
"quality": 0.6812791396682123,
"pose_frame": "camera",
"item_uuid": "",
"timestamp": {
"sec": 1587033051,
"nsec": 179231838
},
"pose": {
"position": {
"y": 0.04363611955261629,
"x": 0.07198067450728357,
"z": 0.8879013030941614,
},
"orientation": {
"y": -0.18422641602024026,
"x": 0.03447462246434434,
"z": 0.5485609394110762,
"w": 0.8148331263508596,
},
},
"max_suction_surface_width": 0.051409365319609185,
"max_suction_surface_length": 0.09779866352968565,
"type": "SUCTION",
},
{
"uuid": "58a02a80-7fcd-11ea-a789-00142d2cd4ce",
"quality": 0.7596309125250751,
"pose_frame": "camera",
"item_uuid": "",
"timestamp": {
"sec": 1587033051,
"nsec": 179231838
},
"pose": {
"position": {
"y": 0.010809225849663155,
"x": 0.13863803337363131,
"z": 0.904460429033145,
},
"orientation": {
"y": -0.00608028855302178,
"x": -0.02148840363751206,
"z": 0.5712238542641026,
"w": 0.8204904551058998,
},
},
"max_suction_surface_width": 0.0546039270399626,
"max_suction_surface_length": 0.1002807889119014,
"type": "SUCTION",
},
],
"load_carriers": [{
"pose": {
"position": {
"y": -0.0168824336375496,
"x": 0.1189406043995812,
"z": 0.8875302697155399,
},
"orientation": {
"y": -0.04632486703729271,
"x": 0.998664879978751,
"z": 0.006342615882086765,
"w": 0.02195985184108778,
},
},
"pose_frame": "camera",
"inner_dimensions": {
"y": 0.27,
"x": 0.37,
"z": 0.14
},
"outer_dimensions": {
"y": 0.3,
"x": 0.4,
"z": 0.2
},
"overfilled": True,
"rim_thickness": {
"y": 0.0,
"x": 0.0
},
"id": "mylc",
}],
"return_code": {
"message": "",
"value": 0
},
}
ros_res = convert_dictionary_to_ros_message(
ComputeGrasps._response_class(), api_res)
ros_lc = ros_res.load_carriers[0]
api_lc = api_res["load_carriers"][0]
assert_lc(ros_lc, api_lc, api_res["timestamp"])
for i, ros_grasp in enumerate(ros_res.grasps):
api_grasp = api_res["grasps"][i]
assert_suction_grasp(ros_grasp, api_grasp)
def test_filling_level():
ros_req = DetectFillingLevel._request_class()
ros_req.pose_frame = "camera"
ros_req.region_of_interest_id = "testroi"
ros_req.load_carrier_ids = ["mylc"]
ros_req.filling_level_cell_count.x = 2
ros_req.filling_level_cell_count.y = 3
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.pose_frame == api_req["pose_frame"]
assert ros_req.region_of_interest_id == api_req["region_of_interest_id"]
assert ros_req.load_carrier_ids == api_req["load_carrier_ids"]
assert_primitives(ros_req.filling_level_cell_count,
api_req["filling_level_cell_count"])
# don't send robot_pose if pose_frame is camera
assert "robot_pose" not in api_req
ros_req.pose_frame = "external"
ros_req.robot_pose.position.y = 1.0
ros_req.robot_pose.orientation.z = 1.0
api_req = convert_ros_message_to_dictionary(ros_req)
assert "robot_pose" in api_req
assert_pose(ros_req.robot_pose, api_req["robot_pose"])
api_res = {
"timestamp": {
"sec": 1586451776,
"nsec": 640984219
},
"load_carriers": [{
"pose": {
"position": {
"y": -0.016895702313915,
"x": 0.11865983503829655,
"z": 0.88755382218002,
},
"orientation": {
"y": -0.04670608639905016,
"x": 0.99864717766196,
"z": 0.006390199364671242,
"w": 0.021943839675236384,
},
},
"filling_level_cell_count": {
"y": 1,
"x": 1
},
"pose_frame":
"camera",
"inner_dimensions": {
"y": 0.27,
"x": 0.37,
"z": 0.14
},
"outer_dimensions": {
"y": 0.3,
"x": 0.4,
"z": 0.2
},
"overall_filling_level": {
"level_free_in_meters": {
"max": 0.14,
"mean": 0.131,
"min": 0.085
},
"cell_size": {
"y": 0.26,
"x": 0.36
},
"coverage": 1.0,
"level_in_percent": {
"max": 39.3,
"mean": 6.4,
"min": 0.0
},
"cell_position": {
"y": -0.021338225361242996,
"x": 0.11973116380121526,
"z": 0.7876582912409178,
},
},
"overfilled":
False,
"rim_thickness": {
"y": 0.0,
"x": 0.0
},
"cells_filling_levels": [{
"level_free_in_meters": {
"max": 0.14,
"mean": 0.131,
"min": 0.085,
},
"cell_size": {
"y": 0.26,
"x": 0.36
},
"coverage": 1.0,
"level_in_percent": {
"max": 39.3,
"mean": 6.4,
"min": 0.0
},
"cell_position": {
"y": -0.021338225361242996,
"x": 0.11973116380121526,
"z": 0.7876582912409178,
},
}],
"id":
"auer_40x30",
}],
"return_code": {
"message": "",
"value": 0
},
}
ros_res = convert_dictionary_to_ros_message(
DetectFillingLevel._response_class(), api_res)
ros_lc = ros_res.load_carriers[0]
api_lc = api_res["load_carriers"][0]
assert_lc(ros_lc, api_lc)
assert ros_lc.overfilled == api_lc["overfilled"]
assert_primitives(ros_lc.overall_filling_level,
api_lc["overall_filling_level"])
for i, ros_l in enumerate(ros_lc.cells_filling_levels):
assert_primitives(ros_l, api_lc["cells_filling_levels"][i])
assert ros_res.return_code.value == api_res["return_code"]["value"]
assert ros_res.return_code.message == api_res["return_code"]["message"]
assert_timestamp(ros_res.timestamp, api_res["timestamp"])
def test_get_roi_box():
ros_req = GetRegionsOfInterest3D._request_class()
ros_req.region_of_interest_ids = ["foo", "bar", "baz"]
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.region_of_interest_ids == api_req["region_of_interest_ids"]
api_res = {
"regions_of_interest": [
{
"box": {
"x": 0.6,
"y": 0.37,
"z": 0.23
},
"id": "test",
"pose": {
"orientation": {
"w": 1.0,
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"position": {
"x": 0.06,
"y": 0.0,
"z": 0.83
},
},
"pose_frame": "camera",
"type": "BOX",
},
{
"sphere": {
"radius": 0.8
},
"id": "test_external",
"pose": {
"orientation": {
"w": 0.7071067811865476,
"x": 0.0,
"y": 0.0,
"z": 0.7071067811865475,
},
"position": {
"x": -0.1,
"y": -0.5,
"z": 0.5
},
},
"pose_frame": "external",
"type": "SPHERE",
},
],
"return_code": {
"message": "",
"value": 0
},
}
ros_res = convert_dictionary_to_ros_message(
GetRegionsOfInterest3D._response_class(), api_res)
for i, ros_roi in enumerate(ros_res.regions_of_interest):
api_roi = api_res["regions_of_interest"][i]
assert ros_roi.id == api_roi["id"]
assert_pose(ros_roi.pose.pose, api_roi["pose"])
if api_roi["type"] == "BOX":
assert ros_roi.primitive.type == SolidPrimitive.BOX
assert len(ros_roi.primitive.dimensions) == 3
assert ros_roi.primitive.dimensions[0] == api_roi["box"]["x"]
assert ros_roi.primitive.dimensions[1] == api_roi["box"]["y"]
assert ros_roi.primitive.dimensions[2] == api_roi["box"]["z"]
elif api_roi["type"] == "SPHERE":
assert ros_roi.primitive.type == SolidPrimitive.SPHERE
assert len(ros_roi.primitive.dimensions) == 1
assert ros_roi.primitive.dimensions[0] == api_roi["sphere"][
"radius"]
def test_detect_lcs():
ros_req = DetectLoadCarriers._request_class()
ros_req.load_carrier_ids = ["test"]
ros_req.pose_frame = "camera"
ros_req.region_of_interest_id = "my_roi"
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.load_carrier_ids == api_req["load_carrier_ids"]
assert ros_req.pose_frame == api_req["pose_frame"]
assert ros_req.region_of_interest_id == api_req["region_of_interest_id"]
assert "robot_pose" not in api_req
# check that API request contains robot pose if pose_frame is external
ros_req.pose_frame = "external"
api_req = convert_ros_message_to_dictionary(ros_req)
assert "robot_pose" in api_req
api_res = {
"timestamp": {
"sec": 1581614679,
"nsec": 917540309
},
"load_carriers": [{
"pose": {
"position": {
"y": -0.01687562098439693,
"x": 0.11888062561732585,
"z": 0.8873076959237803,
},
"orientation": {
"y": -0.046329159479736336,
"x": 0.9986625754609102,
"z": 0.006671112421383355,
"w": 0.021958331489780644,
},
},
"pose_frame": "camera",
"inner_dimensions": {
"y": 0.27,
"x": 0.37,
"z": 0.14
},
"outer_dimensions": {
"y": 0.3,
"x": 0.4,
"z": 0.2
},
"overfilled": True,
"rim_thickness": {
"y": 0.0,
"x": 0.0
},
"id": "test",
}],
"return_code": {
"message": "foo",
"value": 123
},
}
ros_res = convert_dictionary_to_ros_message(
DetectLoadCarriers._response_class(), api_res)
ros_lc = ros_res.load_carriers[0]
api_lc = api_res["load_carriers"][0]
assert_lc(ros_lc, api_lc, api_res["timestamp"])
assert ros_res.return_code.value == api_res["return_code"]["value"]
assert ros_res.return_code.message == api_res["return_code"]["message"]
assert_timestamp(ros_res.timestamp, api_res["timestamp"])
def test_plane():
plane = {"distance": 0.3, "normal": {"y": 0.2, "x": 0.1, "z": 0.3}}
ros_plane = convert_dictionary_to_ros_message(Plane(), plane)
assert_plane(ros_plane, plane)
def test_detect_tags():
ros_req = DetectTags._request_class()
# robot_pose should only be provided if pose_frame is external
ros_req.pose_frame = "camera"
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.pose_frame == api_req["pose_frame"]
assert "robot_pose" not in api_req
ros_req.pose_frame = "external"
ros_req.robot_pose.orientation.x = 1.0
ros_req.robot_pose.orientation.y = 0.0
ros_req.robot_pose.orientation.z = 0.0
ros_req.robot_pose.orientation.w = 0.0
ros_req.robot_pose.position.x = 1.1
ros_req.robot_pose.position.y = -1.1
ros_req.robot_pose.position.z = 0.5
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.pose_frame == api_req["pose_frame"]
assert_pose(ros_req.robot_pose, api_req["robot_pose"])
api_res = {
"tags": [{
"id": "36h10_2319",
"instance_id": "2367151514",
"pose": {
"orientation": {
"w": 0.661847902940126,
"x": -0.14557814643855335,
"y": -0.14578886497641433,
"z": -0.7207703958280759,
},
"position": {
"x": 0.0419080633254838,
"y": -0.02554360431324062,
"z": 0.4794844275109502,
},
},
"pose_frame": "camera",
"size": 0.026511077553573122,
"timestamp": {
"nsec": 764237750,
"sec": 1591101453
},
}],
"timestamp": {
"nsec": 764237750,
"sec": 1591101453
},
"return_code": {
"message": "foo",
"value": 1234
},
}
ros_res = convert_dictionary_to_ros_message(DetectTags._response_class(),
api_res)
for i, ros_tag in enumerate(ros_res.tags):
tag = api_res["tags"][i]
assert ros_tag.tag.id == tag["id"]
assert ros_tag.tag.size == tag["size"]
assert ros_tag.instance_id == tag["instance_id"]
assert_header(ros_tag.header, tag["timestamp"], tag["pose_frame"])
assert_pose(ros_tag.pose.pose, tag["pose"])
assert ros_tag.pose.header == ros_tag.header
assert ros_res.return_code.value == api_res["return_code"]["value"]
assert ros_res.return_code.message == api_res["return_code"]["message"]
assert_timestamp(ros_res.timestamp, api_res["timestamp"])
def test_silhouettematch_detect_object():
ros_req = SilhouetteMatchDetectObject._request_class()
ros_req.pose_frame = "camera"
ros_req.object_to_detect.object_id = "foo_template"
ros_req.object_to_detect.region_of_interest_2d_id = "bar_roi"
ros_req.offset = 0.1
api_req = convert_ros_message_to_dictionary(ros_req)
assert ros_req.pose_frame == api_req["pose_frame"]
assert ros_req.object_to_detect.object_id == api_req["object_to_detect"][
"object_id"]
assert ros_req.object_to_detect.region_of_interest_2d_id == api_req[
"object_to_detect"]["region_of_interest_2d_id"]
assert ros_req.offset == api_req["offset"]
# don't send robot_pose if pose_frame is camera
assert "robot_pose" not in api_req
ros_req.pose_frame = "external"
ros_req.robot_pose.position.y = 1.0
ros_req.robot_pose.orientation.z = 1.0
api_req = convert_ros_message_to_dictionary(ros_req)
assert "robot_pose" in api_req
assert_pose(ros_req.robot_pose, api_req["robot_pose"])
api_res = {
"timestamp": {
"sec": 1587035449,
"nsec": 321465164
},
"return_code": {
"message": "abcdef",
"value": 1234
},
"load_carriers": [{
"pose": {
"position": {
"y": -0.0168824336375496,
"x": 0.1189406043995812,
"z": 0.8875302697155399,
},
"orientation": {
"y": -0.04632486703729271,
"x": 0.998664879978751,
"z": 0.006342615882086765,
"w": 0.02195985184108778,
},
},
"pose_frame": "camera",
"inner_dimensions": {
"y": 0.27,
"x": 0.37,
"z": 0.14
},
"outer_dimensions": {
"y": 0.3,
"x": 0.4,
"z": 0.2
},
"overfilled": True,
"rim_thickness": {
"y": 0.0,
"x": 0.0
},
"id": "xyz",
}],
"object_id":
"foo_template",
"grasps": [{
"instance_uuid": "666ab9d5-613d-4ab5-8150-e0f57bc64f0b",
"uuid": "8566591f-a973-48dc-868d-d88dd36b1c2f",
"pose_frame": "camera",
"timestamp": {
"sec": 1618242033,
"nsec": 580367964
},
"pose": {
"position": {
"y": -0.10296900579068122,
"x": -0.0266098059453003,
"z": 0.7429549952451179
},
"orientation": {
"y": -0.03440297300901497,
"x": -0.0521801300739437,
"z": -0.5059767984782052,
"w": 0.86027969223698
}
},
"id": "grasp1"
}, {
"instance_uuid": "666ab9d5-613d-4ab5-8150-e0f57bc64f0b",
"uuid": "032d85c9-897f-45bc-b6c0-962c72d77ff5",
"pose_frame": "camera",
"timestamp": {
"sec": 1618242033,
"nsec": 580367964
},
"pose": {
"position": {
"y": -0.12056604440014489,
"x": -0.017118856731590644,
"z": 0.74522584119423
},
"orientation": {
"y": 0.0521801300739437,
"x": -0.03440297300901496,
"z": 0.8602796922369801,
"w": 0.5059767984782052
}
},
"id": "grasp1"
}],
"instances": [{
"uuid":
"666ab9d5-613d-4ab5-8150-e0f57bc64f0b",
"pose_frame":
"camera",
"timestamp": {
"sec": 1605085203,
"nsec": 437828567
},
"pose": {
"position": {
"y": -0.030490136926486944,
"x": 0.11629137366368486,
"z": 0.750721261686269
},
"orientation": {
"y": 0.00888023218635928,
"x": 0.01306525525093437,
"z": 0.2290564806473834,
"w": 0.973284937341054
}
},
"object_id":
"foo_template",
"grasp_uuids": [
"8566591f-a973-48dc-868d-d88dd36b1c2f",
"032d85c9-897f-45bc-b6c0-962c72d77ff5"
],
"id":
"666ab9d5-613d-4ab5-8150-e0f57bc64f0b"
}, {
"uuid": "1b9a8b0d-1add-4c2b-81e7-240a21ec65c5",
"pose_frame": "camera",
"timestamp": {
"sec": 1605085203,
"nsec": 437828567
},
"pose": {
"position": {
"y": 0.10677343069137911,
"x": 0.021505663098054306,
"z": 0.7558621572235659
},
"orientation": {
"y": 0.010262094705195637,
"x": 0.012010363471490575,
"z": 0.12072303580908399,
"w": 0.9925605216844882
}
},
"object_id": "foo_template",
"grasp_uuids": [],
"id": "1b9a8b0d-1add-4c2b-81e7-240a21ec65c5"
}]
}
ros_res = convert_dictionary_to_ros_message(
SilhouetteMatchDetectObject._response_class(), api_res)
ros_lc = ros_res.load_carriers[0]
api_lc = api_res["load_carriers"][0]
assert_lc(ros_lc, api_lc, api_res["timestamp"])
assert ros_lc.overfilled == api_lc["overfilled"]
for i, ros_match in enumerate(ros_res.matches):
api_match = api_res["instances"][i]
assert_match(ros_match, api_match)
for i, ros_grasp in enumerate(ros_res.grasps):
api_grasp = api_res["grasps"][i]
assert_grasp(ros_grasp, api_grasp)
