test_mappings.py
Go to the documentation of this file.
1 # Copyright 2021 Roboception GmbH
2 #
3 # Redistribution and use in source and binary forms, with or without
4 # modification, are permitted provided that the following conditions are met:
5 #
6 # * Redistributions of source code must retain the above copyright
7 # notice, this list of conditions and the following disclaimer.
8 #
9 # * Redistributions in binary form must reproduce the above copyright
10 # notice, this list of conditions and the following disclaimer in the
11 # documentation and/or other materials provided with the distribution.
12 #
13 # * Neither the name of the {copyright_holder} nor the names of its
14 # contributors may be used to endorse or promote products derived from
15 # this software without specific prior written permission.
16 #
17 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 # POSSIBILITY OF SUCH DAMAGE.
28 
29 import pytest
30 
31 from rc_reason_clients.message_converter import (
32  convert_dictionary_to_ros_message,
33  convert_ros_message_to_dictionary,
34 )
35 from rc_reason_msgs.msg import DetectedTag, Box, ItemModel
36 from shape_msgs.msg import Plane, SolidPrimitive
37 from rc_reason_msgs.srv import CalibrateBasePlane, GetBasePlaneCalibration
38 from rc_reason_msgs.srv import (
39  SetLoadCarrier,
40  GetLoadCarriers,
41  DetectLoadCarriers,
42  DetectFillingLevel,
43  DetectTags,
44 )
45 from rc_reason_msgs.srv import SetRegionOfInterest3D, GetRegionsOfInterest3D
46 from rc_reason_msgs.srv import ComputeGrasps, DetectItems
47 from rc_reason_msgs.srv import SilhouetteMatchDetectObject
48 
49 
50 def assert_timestamp(ros_ts, ts):
51  assert ros_ts.secs == ts["sec"]
52  assert ros_ts.nsecs == ts["nsec"]
53 
54 
55 def assert_header(ros_header, timestamp, frame_id):
56  assert_timestamp(ros_header.stamp, timestamp)
57  assert ros_header.frame_id == frame_id
58 
59 
60 def assert_pose(ros_pose, api_pose):
61  assert ros_pose.position.x == api_pose["position"]["x"]
62  assert ros_pose.position.y == api_pose["position"]["y"]
63  assert ros_pose.position.z == api_pose["position"]["z"]
64  assert ros_pose.orientation.x == api_pose["orientation"]["x"]
65  assert ros_pose.orientation.y == api_pose["orientation"]["y"]
66  assert ros_pose.orientation.z == api_pose["orientation"]["z"]
67  assert ros_pose.orientation.w == api_pose["orientation"]["w"]
68 
69 
70 def assert_plane(ros_plane, api_plane):
71  assert ros_plane.coef[0] == api_plane["normal"]["x"]
72  assert ros_plane.coef[1] == api_plane["normal"]["y"]
73  assert ros_plane.coef[2] == api_plane["normal"]["z"]
74  assert ros_plane.coef[3] == api_plane["distance"]
75 
76 
77 def assert_primitives(ros, api):
78  for field_name, field_type in zip(ros.__slots__, ros._slot_types):
79  if "/" in field_type:
80  assert_primitives(getattr(ros, field_name), api[field_name])
81  else:
82  assert api[field_name] == getattr(ros, field_name)
83 
84 
85 def assert_lc(ros_lc, api_lc, timestamp=None, pose_required=True):
86  if pose_required:
87  assert_pose(ros_lc.pose.pose, api_lc["pose"])
88  assert ros_lc.pose.header.frame_id == api_lc["pose_frame"]
89  if timestamp is not None:
90  assert_timestamp(ros_lc.pose.header.stamp, timestamp)
91  assert ros_lc.id == api_lc["id"]
92  assert_primitives(ros_lc.inner_dimensions, api_lc["inner_dimensions"])
93  assert_primitives(ros_lc.outer_dimensions, api_lc["outer_dimensions"])
94  assert_primitives(ros_lc.rim_thickness, api_lc["rim_thickness"])
95  if "overfilled" in api_lc:
96  assert ros_lc.overfilled == api_lc["overfilled"]
97 
98 
99 def assert_suction_grasp(ros_grasp, api_grasp):
100  assert_pose(ros_grasp.pose.pose, api_grasp["pose"])
101  assert_header(
102  ros_grasp.pose.header, api_grasp["timestamp"], api_grasp["pose_frame"]
103  )
104  assert ros_grasp.quality == api_grasp["quality"]
105  assert (
106  ros_grasp.max_suction_surface_length == api_grasp["max_suction_surface_length"]
107  )
108  assert ros_grasp.max_suction_surface_width == api_grasp["max_suction_surface_width"]
109 
110 
111 def assert_grasp(ros_grasp, api_grasp):
112  assert_pose(ros_grasp.pose.pose, api_grasp["pose"])
113  assert_header(
114  ros_grasp.pose.header, api_grasp["timestamp"], api_grasp["pose_frame"]
115  )
116  assert ros_grasp.id == api_grasp["id"]
117  assert ros_grasp.uuid == api_grasp["uuid"]
118  assert ros_grasp.match_uuid == api_grasp.get("match_uuid") or api_grasp.get("instance_uuid")
119 
120 
121 def assert_item(ros_item, api_item):
122  assert_pose(ros_item.pose.pose, api_item["pose"])
123  assert_header(ros_item.pose.header, api_item["timestamp"], api_item["pose_frame"])
124 
125 
126 def assert_match(ros_match, api_match):
127  assert_pose(ros_match.pose.pose, api_match["pose"])
128  assert_header(ros_match.pose.header, api_match["timestamp"], api_match["pose_frame"])
129  assert ros_match.uuid == api_match["uuid"]
130  assert ros_match.template_id == api_match.get("template_id") or api_match.get("object_id")
131  if 'score' in api_match:
132  assert ros_match.score == api_match['score']
133  else:
134  assert ros_match.score == -1.0
135 
136 
137 def test_detect_tags():
138  ros_req = DetectTags._request_class()
139 
140  # robot_pose should only be provided if pose_frame is external
141  ros_req.pose_frame = "camera"
142  api_req = convert_ros_message_to_dictionary(ros_req)
143  assert ros_req.pose_frame == api_req["pose_frame"]
144  assert "robot_pose" not in api_req
145 
146  ros_req.pose_frame = "external"
147  ros_req.robot_pose.orientation.x = 1.0
148  ros_req.robot_pose.orientation.y = 0.0
149  ros_req.robot_pose.orientation.z = 0.0
150  ros_req.robot_pose.orientation.w = 0.0
151  ros_req.robot_pose.position.x = 1.1
152  ros_req.robot_pose.position.y = -1.1
153  ros_req.robot_pose.position.z = 0.5
154  api_req = convert_ros_message_to_dictionary(ros_req)
155  assert ros_req.pose_frame == api_req["pose_frame"]
156  assert_pose(ros_req.robot_pose, api_req["robot_pose"])
157 
158  api_res = {
159  "tags": [
160  {
161  "id": "36h10_2319",
162  "instance_id": "2367151514",
163  "pose": {
164  "orientation": {
165  "w": 0.661847902940126,
166  "x": -0.14557814643855335,
167  "y": -0.14578886497641433,
168  "z": -0.7207703958280759,
169  },
170  "position": {
171  "x": 0.0419080633254838,
172  "y": -0.02554360431324062,
173  "z": 0.4794844275109502,
174  },
175  },
176  "pose_frame": "camera",
177  "size": 0.026511077553573122,
178  "timestamp": {"nsec": 764237750, "sec": 1591101453},
179  }
180  ],
181  "timestamp": {"nsec": 764237750, "sec": 1591101453},
182  "return_code": {"message": "foo", "value": 1234},
183  }
184 
185  ros_res = convert_dictionary_to_ros_message(DetectTags._response_class(), api_res)
186  for i, ros_tag in enumerate(ros_res.tags):
187  tag = api_res["tags"][i]
188  assert ros_tag.tag.id == tag["id"]
189  assert ros_tag.tag.size == tag["size"]
190  assert ros_tag.instance_id == tag["instance_id"]
191  assert_header(ros_tag.header, tag["timestamp"], tag["pose_frame"])
192  assert_pose(ros_tag.pose.pose, tag["pose"])
193  assert ros_tag.pose.header == ros_tag.header
194  assert ros_res.return_code.value == api_res["return_code"]["value"]
195  assert ros_res.return_code.message == api_res["return_code"]["message"]
196  assert_timestamp(ros_res.timestamp, api_res["timestamp"])
197 
198 
199 def test_plane():
200  plane = {"distance": 0.3, "normal": {"y": 0.2, "x": 0.1, "z": 0.3}}
201  ros_plane = convert_dictionary_to_ros_message(Plane(), plane)
202  assert_plane(ros_plane, plane)
203 
204 
205 def test_get_base_plane_calibration():
206  ros_req = GetBasePlaneCalibration._request_class()
207 
208  # in silhouettematch, the robot_pose can also be used in camera frame!
209  ros_req.pose_frame = "camera"
210  api_req = convert_ros_message_to_dictionary(ros_req)
211  assert ros_req.pose_frame == api_req["pose_frame"]
212  assert_pose(ros_req.robot_pose, api_req["robot_pose"])
213 
214  ros_req.pose_frame = "external"
215  api_req = convert_ros_message_to_dictionary(ros_req)
216  assert ros_req.pose_frame == api_req["pose_frame"]
217  assert_pose(ros_req.robot_pose, api_req["robot_pose"])
218 
219  response = {
220  "plane": {
221  "distance": -0.7552042203510121,
222  "normal": {
223  "y": -0.02136428281262259,
224  "x": 0.02327135522722706,
225  "z": 0.999500881163089,
226  },
227  "pose_frame": "camera",
228  },
229  "return_code": {"message": "foo", "value": 1234},
230  }
231  ros_res = convert_dictionary_to_ros_message(GetBasePlaneCalibration._response_class(), response)
232  assert_plane(ros_res.plane, response["plane"])
233  assert ros_res.pose_frame == response["plane"]["pose_frame"]
234  assert ros_res.return_code.value == response["return_code"]["value"]
235  assert ros_res.return_code.message == response["return_code"]["message"]
236 
237 
238 def test_calibrate_base_plane_response():
239  response = {
240  "timestamp": {"sec": 1593000384, "nsec": 386523224},
241  "plane": {
242  "distance": -0.7552042203510121,
243  "normal": {
244  "y": -0.02136428281262259,
245  "x": 0.02327135522722706,
246  "z": 0.999500881163089,
247  },
248  "pose_frame": "camera",
249  },
250  "return_code": {"message": "test", "value": 0},
251  }
252  ros_res = convert_dictionary_to_ros_message(CalibrateBasePlane._response_class(), response)
253  assert_plane(ros_res.plane, response["plane"])
254  assert ros_res.pose_frame == response["plane"]["pose_frame"]
255  assert ros_res.return_code.value == response["return_code"]["value"]
256  assert ros_res.return_code.message == response["return_code"]["message"]
257  assert_timestamp(ros_res.timestamp, response["timestamp"])
258 
259 
260 @pytest.mark.parametrize("method", ["STEREO", "APRILTAG", "MANUAL"])
261 def test_calibrate_base_plane_request(method):
262  ros_req = CalibrateBasePlane._request_class()
263  ros_req.pose_frame = "camera"
264  ros_req.offset = 0.123
265  ros_req.plane_estimation_method = method
266  if method == "STEREO":
267  ros_req.stereo_plane_preference = "CLOSEST"
268  elif method == "MANUAL":
269  ros_req.plane.coef = [0.1, -0.2, 0.3, 0.4]
270  api_req = convert_ros_message_to_dictionary(ros_req)
271  assert ros_req.pose_frame == api_req["pose_frame"]
272  assert ros_req.plane_estimation_method == api_req["plane_estimation_method"]
273  assert ros_req.offset == api_req["offset"]
274  if method == "STEREO":
275  assert ros_req.stereo_plane_preference == api_req["stereo"]["plane_preference"]
276  elif method == "MANUAL":
277  assert ros_req.plane.coef[0] == api_req["plane"]["normal"]["x"]
278  assert ros_req.plane.coef[1] == api_req["plane"]["normal"]["y"]
279  assert ros_req.plane.coef[2] == api_req["plane"]["normal"]["z"]
280  assert ros_req.plane.coef[3] == api_req["plane"]["distance"]
281  # in silhouettematch, the robot_pose can also be used in camera frame!
282  assert_pose(ros_req.robot_pose, api_req["robot_pose"])
283 
284  ros_req.pose_frame = "external"
285  api_req = convert_ros_message_to_dictionary(ros_req)
286  assert ros_req.pose_frame == api_req["pose_frame"]
287  assert_pose(ros_req.robot_pose, api_req["robot_pose"])
288 
289 
290 def test_get_lcs():
291  ros_req = GetLoadCarriers._request_class()
292  ros_req.load_carrier_ids = ["foo", "bar"]
293  api_req = convert_ros_message_to_dictionary(ros_req)
294  assert ros_req.load_carrier_ids == api_req["load_carrier_ids"]
295 
296  api_res = {
297  "load_carriers": [
298  {
299  "id": "test",
300  "inner_dimensions": {"x": 0.8, "y": 0.2, "z": 0.8},
301  "outer_dimensions": {"x": 1.0, "y": 0.6, "z": 1.0},
302  "pose": {
303  "orientation": {"w": 0.0, "x": 0.0, "y": 0.0, "z": 0.0},
304  "position": {"x": 0.0, "y": 0.0, "z": 0.0},
305  },
306  "pose_frame": "",
307  "rim_thickness": {"x": 0.0, "y": 0.0},
308  }
309  ],
310  "return_code": {"message": "test", "value": 1234},
311  }
312  ros_res = convert_dictionary_to_ros_message(GetLoadCarriers._response_class(), api_res)
313  assert_lc(ros_res.load_carriers[0], api_res["load_carriers"][0])
314  assert ros_res.return_code.value == api_res["return_code"]["value"]
315  assert ros_res.return_code.message == api_res["return_code"]["message"]
316 
317 
318 def test_set_lc():
319  ros_req = SetLoadCarrier._request_class()
320  ros_req.load_carrier.id = "mylc"
321  ros_req.load_carrier.inner_dimensions = Box(x=0.8, y=0.2, z=0.8)
322  ros_req.load_carrier.outer_dimensions = Box(x=1.0, y=0.6, z=1.0)
323  api_req = convert_ros_message_to_dictionary(ros_req)
324  assert_lc(ros_req.load_carrier, api_req["load_carrier"], pose_required=False)
325  # don't send overfilled flag in set_load_carrier
326  assert "overfilled" not in api_req["load_carrier"]
327  # don't send pose (as prior) if frame_id is not set
328  assert "pose" not in api_req["load_carrier"]
329 
330  # with prior
331  ros_req.load_carrier.pose.header.frame_id = 'camera'
332  api_req = convert_ros_message_to_dictionary(ros_req)
333  assert_pose(ros_req.load_carrier.pose.pose, api_req["load_carrier"]["pose"])
334 
335 
336 def test_detect_lcs():
337  ros_req = DetectLoadCarriers._request_class()
338  ros_req.load_carrier_ids = ["test"]
339  ros_req.pose_frame = "camera"
340  ros_req.region_of_interest_id = "my_roi"
341  api_req = convert_ros_message_to_dictionary(ros_req)
342  assert ros_req.load_carrier_ids == api_req["load_carrier_ids"]
343  assert ros_req.pose_frame == api_req["pose_frame"]
344  assert ros_req.region_of_interest_id == api_req["region_of_interest_id"]
345  assert "robot_pose" not in api_req
346 
347  # check that API request contains robot pose if pose_frame is external
348  ros_req.pose_frame = "external"
349  api_req = convert_ros_message_to_dictionary(ros_req)
350  assert "robot_pose" in api_req
351 
352  api_res = {
353  "timestamp": {"sec": 1581614679, "nsec": 917540309},
354  "load_carriers": [
355  {
356  "pose": {
357  "position": {
358  "y": -0.01687562098439693,
359  "x": 0.11888062561732585,
360  "z": 0.8873076959237803,
361  },
362  "orientation": {
363  "y": -0.046329159479736336,
364  "x": 0.9986625754609102,
365  "z": 0.006671112421383355,
366  "w": 0.021958331489780644,
367  },
368  },
369  "pose_frame": "camera",
370  "inner_dimensions": {"y": 0.27, "x": 0.37, "z": 0.14},
371  "outer_dimensions": {"y": 0.3, "x": 0.4, "z": 0.2},
372  "overfilled": True,
373  "rim_thickness": {"y": 0.0, "x": 0.0},
374  "id": "test",
375  }
376  ],
377  "return_code": {"message": "foo", "value": 123},
378  }
379  ros_res = convert_dictionary_to_ros_message(DetectLoadCarriers._response_class(), api_res)
380 
381  ros_lc = ros_res.load_carriers[0]
382  api_lc = api_res["load_carriers"][0]
383  assert_lc(ros_lc, api_lc, api_res["timestamp"])
384  assert ros_res.return_code.value == api_res["return_code"]["value"]
385  assert ros_res.return_code.message == api_res["return_code"]["message"]
386  assert_timestamp(ros_res.timestamp, api_res["timestamp"])
387 
388 
389 def test_set_roi_box():
390  ros_req = SetRegionOfInterest3D._request_class()
391  ros_roi = ros_req.region_of_interest
392  ros_roi.id = "myroi"
393  ros_roi.pose.header.frame_id = "camera"
394  ros_roi.pose.pose.position.x = 1.0
395  ros_roi.pose.pose.orientation.w = 1.0
396  ros_roi.primitive = SolidPrimitive(type=SolidPrimitive.BOX)
397  ros_roi.primitive.dimensions = [1.0, 2.2, 3.0]
398 
399  api_req = convert_ros_message_to_dictionary(ros_req)
400 
401  api_roi = api_req["region_of_interest"]
402  assert ros_roi.id == api_roi["id"]
403  assert ros_roi.pose.header.frame_id == api_roi["pose_frame"]
404  assert_pose(ros_roi.pose.pose, api_roi["pose"])
405  assert api_roi["type"] == "BOX"
406  assert ros_roi.primitive.dimensions[0] == api_roi["box"]["x"]
407  assert ros_roi.primitive.dimensions[1] == api_roi["box"]["y"]
408  assert ros_roi.primitive.dimensions[2] == api_roi["box"]["z"]
409 
410 
411 def test_set_roi_sphere():
412  ros_req = SetRegionOfInterest3D._request_class()
413  ros_roi = ros_req.region_of_interest
414  ros_roi.id = "myroi"
415  ros_roi.pose.header.frame_id = "camera"
416  ros_roi.pose.pose.position.x = 1.0
417  ros_roi.pose.pose.orientation.w = 1.0
418  ros_roi.primitive = SolidPrimitive(type=SolidPrimitive.SPHERE)
419  ros_roi.primitive.dimensions = [1.1]
420 
421  api_req = convert_ros_message_to_dictionary(ros_req)
422 
423  api_roi = api_req["region_of_interest"]
424  assert ros_roi.id == api_roi["id"]
425  assert ros_roi.pose.header.frame_id == api_roi["pose_frame"]
426  assert_pose(ros_roi.pose.pose, api_roi["pose"])
427  assert api_roi["type"] == "SPHERE"
428  assert ros_roi.primitive.dimensions[0] == api_roi["sphere"]["radius"]
429 
430 
431 def test_get_roi_box():
432  ros_req = GetRegionsOfInterest3D._request_class()
433  ros_req.region_of_interest_ids = ["foo", "bar", "baz"]
434  api_req = convert_ros_message_to_dictionary(ros_req)
435  assert ros_req.region_of_interest_ids == api_req["region_of_interest_ids"]
436 
437  api_res = {
438  "regions_of_interest": [
439  {
440  "box": {"x": 0.6, "y": 0.37, "z": 0.23},
441  "id": "test",
442  "pose": {
443  "orientation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0},
444  "position": {"x": 0.06, "y": 0.0, "z": 0.83},
445  },
446  "pose_frame": "camera",
447  "type": "BOX",
448  },
449  {
450  "sphere": {"radius": 0.8},
451  "id": "test_external",
452  "pose": {
453  "orientation": {
454  "w": 0.7071067811865476,
455  "x": 0.0,
456  "y": 0.0,
457  "z": 0.7071067811865475,
458  },
459  "position": {"x": -0.1, "y": -0.5, "z": 0.5},
460  },
461  "pose_frame": "external",
462  "type": "SPHERE",
463  },
464  ],
465  "return_code": {"message": "", "value": 0},
466  }
467  ros_res = convert_dictionary_to_ros_message(GetRegionsOfInterest3D._response_class(), api_res)
468  for i, ros_roi in enumerate(ros_res.regions_of_interest):
469  api_roi = api_res["regions_of_interest"][i]
470  assert ros_roi.id == api_roi["id"]
471  assert_pose(ros_roi.pose.pose, api_roi["pose"])
472  if api_roi["type"] == "BOX":
473  assert ros_roi.primitive.type == SolidPrimitive.BOX
474  assert len(ros_roi.primitive.dimensions) == 3
475  assert ros_roi.primitive.dimensions[0] == api_roi["box"]["x"]
476  assert ros_roi.primitive.dimensions[1] == api_roi["box"]["y"]
477  assert ros_roi.primitive.dimensions[2] == api_roi["box"]["z"]
478  elif api_roi["type"] == "SPHERE":
479  assert ros_roi.primitive.type == SolidPrimitive.SPHERE
480  assert len(ros_roi.primitive.dimensions) == 1
481  assert ros_roi.primitive.dimensions[0] == api_roi["sphere"]["radius"]
482 
483 
484 def test_filling_level():
485  ros_req = DetectFillingLevel._request_class()
486  ros_req.pose_frame = "camera"
487  ros_req.region_of_interest_id = "testroi"
488  ros_req.load_carrier_ids = ["mylc"]
489  ros_req.filling_level_cell_count.x = 2
490  ros_req.filling_level_cell_count.y = 3
491  api_req = convert_ros_message_to_dictionary(ros_req)
492  assert ros_req.pose_frame == api_req["pose_frame"]
493  assert ros_req.region_of_interest_id == api_req["region_of_interest_id"]
494  assert ros_req.load_carrier_ids == api_req["load_carrier_ids"]
495  assert_primitives(
496  ros_req.filling_level_cell_count, api_req["filling_level_cell_count"]
497  )
498  # don't send robot_pose if pose_frame is camera
499  assert "robot_pose" not in api_req
500 
501  ros_req.pose_frame = "external"
502  ros_req.robot_pose.position.y = 1.0
503  ros_req.robot_pose.orientation.z = 1.0
504  api_req = convert_ros_message_to_dictionary(ros_req)
505  assert "robot_pose" in api_req
506  assert_pose(ros_req.robot_pose, api_req["robot_pose"])
507 
508  api_res = {
509  "timestamp": {"sec": 1586451776, "nsec": 640984219},
510  "load_carriers": [
511  {
512  "pose": {
513  "position": {
514  "y": -0.016895702313915,
515  "x": 0.11865983503829655,
516  "z": 0.88755382218002,
517  },
518  "orientation": {
519  "y": -0.04670608639905016,
520  "x": 0.99864717766196,
521  "z": 0.006390199364671242,
522  "w": 0.021943839675236384,
523  },
524  },
525  "filling_level_cell_count": {"y": 1, "x": 1},
526  "pose_frame": "camera",
527  "inner_dimensions": {"y": 0.27, "x": 0.37, "z": 0.14},
528  "outer_dimensions": {"y": 0.3, "x": 0.4, "z": 0.2},
529  "overall_filling_level": {
530  "level_free_in_meters": {"max": 0.14, "mean": 0.131, "min": 0.085},
531  "cell_size": {"y": 0.26, "x": 0.36},
532  "coverage": 1.0,
533  "level_in_percent": {"max": 39.3, "mean": 6.4, "min": 0.0},
534  "cell_position": {
535  "y": -0.021338225361242996,
536  "x": 0.11973116380121526,
537  "z": 0.7876582912409178,
538  },
539  },
540  "overfilled": False,
541  "rim_thickness": {"y": 0.0, "x": 0.0},
542  "cells_filling_levels": [
543  {
544  "level_free_in_meters": {
545  "max": 0.14,
546  "mean": 0.131,
547  "min": 0.085,
548  },
549  "cell_size": {"y": 0.26, "x": 0.36},
550  "coverage": 1.0,
551  "level_in_percent": {"max": 39.3, "mean": 6.4, "min": 0.0},
552  "cell_position": {
553  "y": -0.021338225361242996,
554  "x": 0.11973116380121526,
555  "z": 0.7876582912409178,
556  },
557  }
558  ],
559  "id": "auer_40x30",
560  }
561  ],
562  "return_code": {"message": "", "value": 0},
563  }
564  ros_res = convert_dictionary_to_ros_message(DetectFillingLevel._response_class(), api_res)
565  ros_lc = ros_res.load_carriers[0]
566  api_lc = api_res["load_carriers"][0]
567  assert_lc(ros_lc, api_lc)
568  assert ros_lc.overfilled == api_lc["overfilled"]
569  assert_primitives(ros_lc.overall_filling_level, api_lc["overall_filling_level"])
570  for i, ros_l in enumerate(ros_lc.cells_filling_levels):
571  assert_primitives(ros_l, api_lc["cells_filling_levels"][i])
572  assert ros_res.return_code.value == api_res["return_code"]["value"]
573  assert ros_res.return_code.message == api_res["return_code"]["message"]
574  assert_timestamp(ros_res.timestamp, api_res["timestamp"])
575 
576 
577 def test_compute_grasps():
578  ros_req = ComputeGrasps._request_class()
579  ros_req.pose_frame = "camera"
580  ros_req.suction_surface_length = 0.05
581  ros_req.suction_surface_width = 0.02
582 
583  api_req = convert_ros_message_to_dictionary(ros_req)
584  assert ros_req.pose_frame == api_req["pose_frame"]
585  assert ros_req.suction_surface_length == api_req["suction_surface_length"]
586  assert ros_req.suction_surface_width == api_req["suction_surface_width"]
587 
588  # don't send robot_pose if pose_frame is camera
589  assert "robot_pose" not in api_req
590 
591  ros_req.pose_frame = "external"
592  ros_req.robot_pose.position.y = 1.0
593  ros_req.robot_pose.orientation.z = 1.0
594  api_req = convert_ros_message_to_dictionary(ros_req)
595  assert "robot_pose" in api_req
596  assert_pose(ros_req.robot_pose, api_req["robot_pose"])
597 
598  assert len(api_req["item_models"]) == 0
599  assert "region_of_interest_id" not in api_req
600  assert "load_carrier_id" not in api_req
601  assert "load_carrier_compartment" not in api_req
602  assert "collision_detection" not in api_req
603 
604  ros_req.region_of_interest_id = "testroi"
605  ros_req.load_carrier_id = "mylc"
606  api_req = convert_ros_message_to_dictionary(ros_req)
607  assert ros_req.region_of_interest_id == api_req["region_of_interest_id"]
608  assert ros_req.load_carrier_id == api_req["load_carrier_id"]
609  assert "load_carrier_compartment" not in api_req
610  assert "collision_detection" not in api_req
611 
612  # add valid compartment
613  ros_req.load_carrier_compartment.box.x = 0.1
614  ros_req.load_carrier_compartment.box.y = 0.2
615  ros_req.load_carrier_compartment.box.z = 0.3
616  api_req = convert_ros_message_to_dictionary(ros_req)
617  assert_primitives(
618  ros_req.load_carrier_compartment, api_req["load_carrier_compartment"]
619  )
620  assert "collision_detection" not in api_req
621 
622  ros_req.collision_detection.gripper_id = "mygripper"
623  ros_req.collision_detection.pre_grasp_offset.x = 1.0
624  ros_req.collision_detection.pre_grasp_offset.y = 0.1
625  ros_req.collision_detection.pre_grasp_offset.z = -0.5
626  api_req = convert_ros_message_to_dictionary(ros_req)
627  assert_primitives(ros_req.collision_detection, api_req["collision_detection"])
628 
629  api_res = {
630  "timestamp": {"sec": 1587033051, "nsec": 179231838},
631  "grasps": [
632  {
633  "uuid": "589ce564-7fcd-11ea-a789-00142d2cd4ce",
634  "quality": 0.6812791396682123,
635  "pose_frame": "camera",
636  "item_uuid": "",
637  "timestamp": {"sec": 1587033051, "nsec": 179231838},
638  "pose": {
639  "position": {
640  "y": 0.04363611955261629,
641  "x": 0.07198067450728357,
642  "z": 0.8879013030941614,
643  },
644  "orientation": {
645  "y": -0.18422641602024026,
646  "x": 0.03447462246434434,
647  "z": 0.5485609394110762,
648  "w": 0.8148331263508596,
649  },
650  },
651  "max_suction_surface_width": 0.051409365319609185,
652  "max_suction_surface_length": 0.09779866352968565,
653  "type": "SUCTION",
654  },
655  {
656  "uuid": "58a02a80-7fcd-11ea-a789-00142d2cd4ce",
657  "quality": 0.7596309125250751,
658  "pose_frame": "camera",
659  "item_uuid": "",
660  "timestamp": {"sec": 1587033051, "nsec": 179231838},
661  "pose": {
662  "position": {
663  "y": 0.010809225849663155,
664  "x": 0.13863803337363131,
665  "z": 0.904460429033145,
666  },
667  "orientation": {
668  "y": -0.00608028855302178,
669  "x": -0.02148840363751206,
670  "z": 0.5712238542641026,
671  "w": 0.8204904551058998,
672  },
673  },
674  "max_suction_surface_width": 0.0546039270399626,
675  "max_suction_surface_length": 0.1002807889119014,
676  "type": "SUCTION",
677  },
678  ],
679  "load_carriers": [
680  {
681  "pose": {
682  "position": {
683  "y": -0.0168824336375496,
684  "x": 0.1189406043995812,
685  "z": 0.8875302697155399,
686  },
687  "orientation": {
688  "y": -0.04632486703729271,
689  "x": 0.998664879978751,
690  "z": 0.006342615882086765,
691  "w": 0.02195985184108778,
692  },
693  },
694  "pose_frame": "camera",
695  "inner_dimensions": {"y": 0.27, "x": 0.37, "z": 0.14},
696  "outer_dimensions": {"y": 0.3, "x": 0.4, "z": 0.2},
697  "overfilled": True,
698  "rim_thickness": {"y": 0.0, "x": 0.0},
699  "id": "mylc",
700  }
701  ],
702  "return_code": {"message": "", "value": 0},
703  }
704  ros_res = convert_dictionary_to_ros_message(ComputeGrasps._response_class(), api_res)
705  ros_lc = ros_res.load_carriers[0]
706  api_lc = api_res["load_carriers"][0]
707  assert_lc(ros_lc, api_lc, api_res["timestamp"])
708  for i, ros_grasp in enumerate(ros_res.grasps):
709  api_grasp = api_res["grasps"][i]
710  assert_suction_grasp(ros_grasp, api_grasp)
711 
712 
713 def test_detect_items():
714  ros_req = DetectItems._request_class()
715  ros_req.pose_frame = "camera"
716  im = ItemModel(type=ItemModel.RECTANGLE)
717  im.rectangle.min_dimensions.x = 0.2
718  im.rectangle.min_dimensions.y = 0.3
719  im.rectangle.max_dimensions.x = 0.4
720  im.rectangle.max_dimensions.y = 0.5
721  ros_req.item_models.append(im)
722 
723  api_req = convert_ros_message_to_dictionary(ros_req)
724  assert ros_req.pose_frame == api_req["pose_frame"]
725  for i, ros_im in enumerate(ros_req.item_models):
726  api_im = api_req["item_models"][i]
727  assert_primitives(ros_im, api_im)
728 
729  # don't send robot_pose if pose_frame is camera
730  assert "robot_pose" not in api_req
731 
732  ros_req.pose_frame = "external"
733  ros_req.robot_pose.position.y = 1.0
734  ros_req.robot_pose.orientation.z = 1.0
735  api_req = convert_ros_message_to_dictionary(ros_req)
736  assert "robot_pose" in api_req
737  assert_pose(ros_req.robot_pose, api_req["robot_pose"])
738 
739  assert len(api_req["item_models"]) == 1
740  assert "region_of_interest_id" not in api_req
741  assert "load_carrier_id" not in api_req
742  assert "load_carrier_compartment" not in api_req
743 
744  ros_req.region_of_interest_id = "testroi"
745  ros_req.load_carrier_id = "mylc"
746  api_req = convert_ros_message_to_dictionary(ros_req)
747  assert ros_req.region_of_interest_id == api_req["region_of_interest_id"]
748  assert ros_req.load_carrier_id == api_req["load_carrier_id"]
749  assert "load_carrier_compartment" not in api_req
750 
751  # add valid compartment
752  ros_req.load_carrier_compartment.box.x = 0.1
753  ros_req.load_carrier_compartment.box.y = 0.2
754  ros_req.load_carrier_compartment.box.z = 0.3
755  api_req = convert_ros_message_to_dictionary(ros_req)
756  assert_primitives(
757  ros_req.load_carrier_compartment, api_req["load_carrier_compartment"]
758  )
759 
760  api_res = {
761  "timestamp": {"sec": 1587035449, "nsec": 321465164},
762  "return_code": {"message": "abcdef", "value": 1234},
763  "load_carriers": [
764  {
765  "pose": {
766  "position": {
767  "y": -0.0168824336375496,
768  "x": 0.1189406043995812,
769  "z": 0.8875302697155399,
770  },
771  "orientation": {
772  "y": -0.04632486703729271,
773  "x": 0.998664879978751,
774  "z": 0.006342615882086765,
775  "w": 0.02195985184108778,
776  },
777  },
778  "pose_frame": "camera",
779  "inner_dimensions": {"y": 0.27, "x": 0.37, "z": 0.14},
780  "outer_dimensions": {"y": 0.3, "x": 0.4, "z": 0.2},
781  "overfilled": True,
782  "rim_thickness": {"y": 0.0, "x": 0.0},
783  "id": "xyz",
784  }
785  ],
786  "items": [
787  {
788  "uuid": "ee0b2bc4-7fd2-11ea-a789-00142d2cd4ce",
789  "pose_frame": "camera",
790  "timestamp": {"sec": 1587035449, "nsec": 321465164},
791  "pose": {
792  "position": {
793  "y": 0.0015526703948114742,
794  "x": 0.018541338820357283,
795  "z": 0.9062130178042246,
796  },
797  "orientation": {
798  "y": -0.29071987884727973,
799  "x": 0.9563493015403196,
800  "z": 0.019845952916433495,
801  "w": 0.02200235531039019,
802  },
803  },
804  "type": "RECTANGLE",
805  "rectangle": {"y": 0.05698329755083764, "x": 0.10302717395647137},
806  },
807  {
808  "uuid": "ee0b2f34-7fd2-11ea-a789-00142d2cd4ce",
809  "pose_frame": "camera",
810  "timestamp": {"sec": 1587035449, "nsec": 321465164},
811  "pose": {
812  "position": {
813  "y": -0.07512277927447901,
814  "x": 0.01837425822827067,
815  "z": 0.9083435428699417,
816  },
817  "orientation": {
818  "y": -0.10244608240444944,
819  "x": 0.9947325521529258,
820  "z": 0.0022848846348791315,
821  "w": 0.0025940681395760523,
822  },
823  },
824  "type": "RECTANGLE",
825  "rectangle": {"y": 0.05739744695533501, "x": 0.10506054260132827},
826  },
827  ],
828  }
829  ros_res = convert_dictionary_to_ros_message(DetectItems._response_class(), api_res)
830  ros_lc = ros_res.load_carriers[0]
831  api_lc = api_res["load_carriers"][0]
832  assert_lc(ros_lc, api_lc, api_res["timestamp"])
833  assert ros_lc.overfilled == api_lc["overfilled"]
834  for i, ros_item in enumerate(ros_res.items):
835  api_item = api_res["items"][i]
836  assert_item(ros_item, api_item)
837 
838 
839 def test_silhouettematch_detect_object():
840  ros_req = SilhouetteMatchDetectObject._request_class()
841  ros_req.pose_frame = "camera"
842  ros_req.object_to_detect.object_id = "foo_template"
843  ros_req.object_to_detect.region_of_interest_2d_id = "bar_roi"
844  ros_req.offset = 0.1
845 
846  api_req = convert_ros_message_to_dictionary(ros_req)
847  assert ros_req.pose_frame == api_req["pose_frame"]
848  assert ros_req.object_to_detect.object_id == api_req["object_to_detect"]["object_id"]
849  assert ros_req.object_to_detect.region_of_interest_2d_id == api_req["object_to_detect"]["region_of_interest_2d_id"]
850  assert ros_req.offset == api_req["offset"]
851 
852  # don't send robot_pose if pose_frame is camera
853  assert "robot_pose" not in api_req
854 
855  ros_req.pose_frame = "external"
856  ros_req.robot_pose.position.y = 1.0
857  ros_req.robot_pose.orientation.z = 1.0
858  api_req = convert_ros_message_to_dictionary(ros_req)
859  assert "robot_pose" in api_req
860  assert_pose(ros_req.robot_pose, api_req["robot_pose"])
861 
862  api_res = {
863  "timestamp": {"sec": 1587035449, "nsec": 321465164},
864  "return_code": {"message": "abcdef", "value": 1234},
865  "load_carriers": [
866  {
867  "pose": {
868  "position": {
869  "y": -0.0168824336375496,
870  "x": 0.1189406043995812,
871  "z": 0.8875302697155399,
872  },
873  "orientation": {
874  "y": -0.04632486703729271,
875  "x": 0.998664879978751,
876  "z": 0.006342615882086765,
877  "w": 0.02195985184108778,
878  },
879  },
880  "pose_frame": "camera",
881  "inner_dimensions": {"y": 0.27, "x": 0.37, "z": 0.14},
882  "outer_dimensions": {"y": 0.3, "x": 0.4, "z": 0.2},
883  "overfilled": True,
884  "rim_thickness": {"y": 0.0, "x": 0.0},
885  "id": "xyz",
886  }
887  ],
888  "object_id": "foo_template",
889  "grasps": [
890  {
891  "instance_uuid": "666ab9d5-613d-4ab5-8150-e0f57bc64f0b",
892  "uuid": "8566591f-a973-48dc-868d-d88dd36b1c2f",
893  "pose_frame": "camera",
894  "timestamp": {
895  "sec": 1618242033,
896  "nsec": 580367964
897  },
898  "pose": {
899  "position": {
900  "y": -0.10296900579068122,
901  "x": -0.0266098059453003,
902  "z": 0.7429549952451179
903  },
904  "orientation": {
905  "y": -0.03440297300901497,
906  "x": -0.0521801300739437,
907  "z": -0.5059767984782052,
908  "w": 0.86027969223698
909  }
910  },
911  "id": "grasp1"
912  },
913  {
914  "instance_uuid": "666ab9d5-613d-4ab5-8150-e0f57bc64f0b",
915  "uuid": "032d85c9-897f-45bc-b6c0-962c72d77ff5",
916  "pose_frame": "camera",
917  "timestamp": {
918  "sec": 1618242033,
919  "nsec": 580367964
920  },
921  "pose": {
922  "position": {
923  "y": -0.12056604440014489,
924  "x": -0.017118856731590644,
925  "z": 0.74522584119423
926  },
927  "orientation": {
928  "y": 0.0521801300739437,
929  "x": -0.03440297300901496,
930  "z": 0.8602796922369801,
931  "w": 0.5059767984782052
932  }
933  },
934  "id": "grasp1"
935  }
936  ],
937  "instances": [
938  {
939  "uuid": "666ab9d5-613d-4ab5-8150-e0f57bc64f0b",
940  "pose_frame": "camera",
941  "timestamp": {
942  "sec": 1605085203,
943  "nsec": 437828567
944  },
945  "pose": {
946  "position": {
947  "y": -0.030490136926486944,
948  "x": 0.11629137366368486,
949  "z": 0.750721261686269
950  },
951  "orientation": {
952  "y": 0.00888023218635928,
953  "x": 0.01306525525093437,
954  "z": 0.2290564806473834,
955  "w": 0.973284937341054
956  }
957  },
958  "object_id": "foo_template",
959  "grasp_uuids": [
960  "8566591f-a973-48dc-868d-d88dd36b1c2f",
961  "032d85c9-897f-45bc-b6c0-962c72d77ff5"
962  ],
963  "id": "666ab9d5-613d-4ab5-8150-e0f57bc64f0b"
964  },
965  {
966  "uuid": "1b9a8b0d-1add-4c2b-81e7-240a21ec65c5",
967  "pose_frame": "camera",
968  "timestamp": {
969  "sec": 1605085203,
970  "nsec": 437828567
971  },
972  "pose": {
973  "position": {
974  "y": 0.10677343069137911,
975  "x": 0.021505663098054306,
976  "z": 0.7558621572235659
977  },
978  "orientation": {
979  "y": 0.010262094705195637,
980  "x": 0.012010363471490575,
981  "z": 0.12072303580908399,
982  "w": 0.9925605216844882
983  }
984  },
985  "object_id": "foo_template",
986  "grasp_uuids": [],
987  "id": "1b9a8b0d-1add-4c2b-81e7-240a21ec65c5"
988  }
989  ]
990  }
991  ros_res = convert_dictionary_to_ros_message(SilhouetteMatchDetectObject._response_class(), api_res)
992  ros_lc = ros_res.load_carriers[0]
993  api_lc = api_res["load_carriers"][0]
994  assert_lc(ros_lc, api_lc, api_res["timestamp"])
995  assert ros_lc.overfilled == api_lc["overfilled"]
996  for i, ros_match in enumerate(ros_res.matches):
997  api_match = api_res["instances"][i]
998  assert_match(ros_match, api_match)
999  for i, ros_grasp in enumerate(ros_res.grasps):
1000  api_grasp = api_res["grasps"][i]
1001  assert_grasp(ros_grasp, api_grasp)
1002 
test_mappings.test_detect_items
def test_detect_items()
Definition: test_mappings.py:713
test_mappings.assert_pose
def assert_pose(ros_pose, api_pose)
Definition: test_mappings.py:60
test_mappings.test_set_roi_sphere
def test_set_roi_sphere()
Definition: test_mappings.py:411
test_mappings.test_get_lcs
def test_get_lcs()
Definition: test_mappings.py:290
test_mappings.assert_timestamp
def assert_timestamp(ros_ts, ts)
Definition: test_mappings.py:50
test_mappings.assert_suction_grasp
def assert_suction_grasp(ros_grasp, api_grasp)
Definition: test_mappings.py:99
test_mappings.test_filling_level
def test_filling_level()
Definition: test_mappings.py:484
test_mappings.test_get_base_plane_calibration
def test_get_base_plane_calibration()
Definition: test_mappings.py:205
test_mappings.assert_header
def assert_header(ros_header, timestamp, frame_id)
Definition: test_mappings.py:55
test_mappings.assert_match
def assert_match(ros_match, api_match)
Definition: test_mappings.py:126
rc_reason_clients.message_converter
Definition: message_converter.py:1
test_mappings.test_set_lc
def test_set_lc()
Definition: test_mappings.py:318
test_mappings.test_compute_grasps
def test_compute_grasps()
Definition: test_mappings.py:577
test_mappings.assert_grasp
def assert_grasp(ros_grasp, api_grasp)
Definition: test_mappings.py:111
test_mappings.test_plane
def test_plane()
Definition: test_mappings.py:199
test_mappings.assert_plane
def assert_plane(ros_plane, api_plane)
Definition: test_mappings.py:70
test_mappings.test_get_roi_box
def test_get_roi_box()
Definition: test_mappings.py:431
test_mappings.test_detect_tags
def test_detect_tags()
Definition: test_mappings.py:137
test_mappings.test_silhouettematch_detect_object
def test_silhouettematch_detect_object()
Definition: test_mappings.py:839
test_mappings.test_set_roi_box
def test_set_roi_box()
Definition: test_mappings.py:389
test_mappings.assert_item
def assert_item(ros_item, api_item)
Definition: test_mappings.py:121
test_mappings.assert_lc
def assert_lc(ros_lc, api_lc, timestamp=None, pose_required=True)
Definition: test_mappings.py:85
test_mappings.test_calibrate_base_plane_response
def test_calibrate_base_plane_response()
Definition: test_mappings.py:238
test_mappings.assert_primitives
def assert_primitives(ros, api)
Definition: test_mappings.py:77
test_mappings.test_calibrate_base_plane_request
def test_calibrate_base_plane_request(method)
Definition: test_mappings.py:261
test_mappings.test_detect_lcs
def test_detect_lcs()
Definition: test_mappings.py:336

rc_reason_clients
Author(s):
autogenerated on Sat Jun 17 2023 02:48:57