diff --git a/config/config_scene_ceti-table.json b/config/config_scene_ceti-table.json new file mode 100644 index 0000000000000000000000000000000000000000..597f6fa8724b33d5dc9fc70bcc2b55bbb17b432b --- /dev/null +++ b/config/config_scene_ceti-table.json @@ -0,0 +1,389 @@ +{ + "objects": [ + { + "id": "wheel_1", + "pos": { + "x": 0.28, + "y": 0.28, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "wheel_2", + "pos": { + "x": -0.28, + "y": -0.28, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "wheel_3", + "pos": { + "x": -0.28, + "y": 0.28, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "wheel_4", + "pos": { + "x": 0.28, + "y": -0.28, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table_body", + "pos": { + "z": 0.5 + }, + "size": { + "length": 0.69, + "width": 0.69, + "height": 0.76 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table-top", + "pos": { + "z": 0.885 + }, + "size": { + "length": 0.8, + "width": 0.8, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "left_panel", + "pos": { + "z": 0.885, + "y": -0.6525 + }, + "size": { + "length": 0.7, + "width": 0.5, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "right_panel", + "pos": { + "z": 0.885, + "y": 0.6525 + }, + "size": { + "length": 0.7, + "width": 0.5, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "ffront_panel", + "pos": { + "z": 0.885, + "x": 0.6525 + }, + "size": { + "length": 0.5, + "width": 0.7, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "binBlue", + "type": "BIN", + "pos": { + "x": -0.255, + "y": 0.5475, + "z": 0.9675 + }, + "size": { + "length": 0.19, + "width": 0.285, + "height": 0.155 + }, + "orientation": { + "w": 1 + }, + "color": { + "b": 1 + } + }, + { + "id": "binRed", + "type": "BIN", + "pos": { + "x": -0.055, + "y": 0.5475, + "z": 0.9675 + }, + "size": { + "length": 0.19, + "width": 0.285, + "height": 0.155 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 1 + } + }, + { + "id": "binGreen", + "type": "BIN", + "pos": { + "x": 0.145, + "y": 0.5475, + "z": 0.9675 + }, + "size": { + "length": 0.19, + "width": 0.285, + "height": 0.155 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 1 + } + }, + { + "id": "object4x2x1.5", + "type": "BOX", + "pos": { + "x": 0.3, + "y": -0.3, + "z": 0.9067 + }, + "size": { + "length": 0.0318, + "width": 0.0636, + "height": 0.0334 + }, + "orientation": { + "w": 1 + }, + "color": {} + }, + { + "id": "object4x2x2.0", + "type": "BOX", + "pos": { + "x": 0.3, + "y": -0.2, + "z": 0.9115 + }, + "size": { + "length": 0.0318, + "width": 0.0636, + "height": 0.043 + }, + "orientation": { + "w": 1 + }, + "color": {} + }, + { + "id": "object4x2x2.5", + "type": "BOX", + "pos": { + "x": 0.3, + "y": -0.1, + "z": 0.9163 + }, + "size": { + "length": 0.0318, + "width": 0.0636, + "height": 0.0526 + }, + "orientation": { + "w": 1 + }, + "color": {} + }, + { + "id": "object4x2x3.0", + "type": "BOX", + "pos": { + "x": 0.3, + "y": 0, + "z": 0.9211 + }, + "size": { + "length": 0.0318, + "width": 0.0636, + "height": 0.0622 + }, + "orientation": { + "w": 1 + }, + "color": {} + }, + { + "id": "object4x2x3.5", + "type": "BOX", + "pos": { + "x": 0.3, + "y": 0.1, + "z": 0.9259 + }, + "size": { + "length": 0.0318, + "width": 0.0636, + "height": 0.0718 + }, + "orientation": { + "w": 1 + }, + "color": {} + }, + { + "id": "object4x2x4.0", + "type": "BOX", + "pos": { + "x": 0.3, + "y": 0.2, + "z": 0.9307 + }, + "size": { + "length": 0.0318, + "width": 0.0636, + "height": 0.0814 + }, + "orientation": { + "w": 1 + }, + "color": {} + }, + { + "id": "object4x2x4.5", + "type": "BOX", + "pos": { + "x": 0.3, + "y": 0.3, + "z": 0.9355 + }, + "size": { + "length": 0.0318, + "width": 0.0636, + "height": 0.091 + }, + "orientation": { + "w": 1 + }, + "color": {} + }, + { + "id": "arm", + "type": "ARM", + "pos": { + "z": 0.89, + "x": -0.22 + }, + "size": {}, + "orientation": { + "w": 1 + }, + "color": { + "r": 1, + "g": 1, + "b": 1 + } + } + ] +} diff --git a/config/config_scene_ceti-table.yaml b/config/config_scene_ceti-table.yaml new file mode 100644 index 0000000000000000000000000000000000000000..99d579a4abefcfe6452a26292d03eb5aeacb566a --- /dev/null +++ b/config/config_scene_ceti-table.yaml @@ -0,0 +1,44 @@ +# create json file using +# yq eval -o=json config_scene_ceti-table.yaml > config_scene_ceti-table.json +{ 'objects': [ + # table + # height: 12 cm wheels, 76 cm body, 1 cm table-top = 89 cm total + # wheels width/depth/height 12*12*12 + # body width/depth/height 69*69*76 + # table-top width/depth/height 80*80*1 + # panels are 50*70, with 0.5 cm distance to the table-top + # height of both is 12 + 76 + .5 = 88.5 + # their centre is at 40 + 0.25 + 25 = 65.25 + { 'id': 'wheel_1','pos': { 'x': 0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'wheel_2','pos': { 'x': -0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'wheel_3','pos': { 'x': -0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'wheel_4','pos': { 'x': 0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table_body','pos': { 'z': 0.50 },'size': { 'length': .69,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'table-top', 'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'left_panel', 'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'right_panel', 'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + { 'id': 'ffront_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, + + # bins width/depth/height 28.5*19*15.5 + # their height is 89 + 7.75 = 0.9675 m + # the width is 31.8 or 63.6 + # if they stand on a panel edge, they stand at 40 + 0.5 + 14.25 = 54.75 + # their x location is N*19 + 9.5 - 35 = N*20 - 25.5 = -25.5 and -5.5 and 14.5 if they have 1 cm distance + { 'id': 'binBlue', 'type': 'BIN','pos': { 'x': -0.255,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, + { 'id': 'binRed', 'type': 'BIN','pos': { 'x': -0.055,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'r': 1 } }, + { 'id': 'binGreen','type': 'BIN','pos': { 'x': 0.145,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'r': 1 } }, + + # heights of objects (including pins): + # formula: half-height * 9.6 + 4.6 + # formula (python) + # for i in range(3,10): print("{ 'id': 'object4x2x%.1f','type': 'BOX','pos': { 'x': 0.3,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0636,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+8900)/10000, (i*96+46)/10000)); + { 'id': 'object4x2x1.5','type': 'BOX','pos': { 'x': 0.3,'y': -0.300000,'z': 0.906700 },'size': { 'length': .0318, 'width': .0636,'height': 0.033400 },'orientation': { 'w': 1 },'color': {} }, + { 'id': 'object4x2x2.0','type': 'BOX','pos': { 'x': 0.3,'y': -0.200000,'z': 0.911500 },'size': { 'length': .0318, 'width': .0636,'height': 0.043000 },'orientation': { 'w': 1 },'color': {} }, + { 'id': 'object4x2x2.5','type': 'BOX','pos': { 'x': 0.3,'y': -0.100000,'z': 0.916300 },'size': { 'length': .0318, 'width': .0636,'height': 0.052600 },'orientation': { 'w': 1 },'color': {} }, + { 'id': 'object4x2x3.0','type': 'BOX','pos': { 'x': 0.3,'y': 0.000000,'z': 0.921100 },'size': { 'length': .0318, 'width': .0636,'height': 0.062200 },'orientation': { 'w': 1 },'color': {} }, + { 'id': 'object4x2x3.5','type': 'BOX','pos': { 'x': 0.3,'y': 0.100000,'z': 0.925900 },'size': { 'length': .0318, 'width': .0636,'height': 0.071800 },'orientation': { 'w': 1 },'color': {} }, + { 'id': 'object4x2x4.0','type': 'BOX','pos': { 'x': 0.3,'y': 0.200000,'z': 0.930700 },'size': { 'length': .0318, 'width': .0636,'height': 0.081400 },'orientation': { 'w': 1 },'color': {} }, + { 'id': 'object4x2x4.5','type': 'BOX','pos': { 'x': 0.3,'y': 0.300000,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': {} }, + + { 'id': 'arm','type': 'ARM','pos': { 'z': 0.89 , 'x': -0.22 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } +] } diff --git a/launch/ceti-table_robot_main.launch b/launch/ceti-table_robot_main.launch new file mode 100644 index 0000000000000000000000000000000000000000..3c974b4e9e22531fe5c62a0ae691f7050fb25d7f --- /dev/null +++ b/launch/ceti-table_robot_main.launch @@ -0,0 +1,19 @@ +<launch> + + <arg name="robot_ip" default="172.31.1.13" /> + <arg name="load_gripper" default="true" /> + <arg name="connection_address" default="tcp://*:6576" /> + <arg name="client_controllers" default="[]" /> + + <include file="$(find ccf)/launch/robot_setup.launch" > + <arg name="robot_ip" value="$(arg robot_ip)" /> + <arg name="load_gripper" value="$(arg load_gripper)"/> + </include> + + <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" > + <param name="connection_address" type="string" value="$(arg connection_address)" /> + <param name="client_controllers" type="yaml" value="$(arg client_controllers)" /> + <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_ceti-table.json" /> + </node> + +</launch> diff --git a/launch/ceti-table_robot_main_dummy_selector.launch b/launch/ceti-table_robot_main_dummy_selector.launch new file mode 100644 index 0000000000000000000000000000000000000000..ae399802695a8b40d4fb8daa1ae7e7c88e65c2da --- /dev/null +++ b/launch/ceti-table_robot_main_dummy_selector.launch @@ -0,0 +1,4 @@ +<launch> + <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/> + <include file="$(find ccf_immersive_sorting)/launch/ceti-table_robot_main.launch" /> +</launch> diff --git a/launch/ceti-table_simulated_main.launch b/launch/ceti-table_simulated_main.launch new file mode 100644 index 0000000000000000000000000000000000000000..d4607d6d126f0c0fa62d29d3fe4e7a5aabede939 --- /dev/null +++ b/launch/ceti-table_simulated_main.launch @@ -0,0 +1,14 @@ +<launch> + + <arg name="connection_address" default="tcp://*:6576" /> + <arg name="client_controllers" default="[]" /> + + <include file="$(find ccf)/launch/simulation_setup.launch"/> + + <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" > + <param name="connection_address" type="string" value="$(arg connection_address)" /> + <param name="client_controllers" type="yaml" value="$(arg client_controllers)" /> + <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_ceti-table.json" /> + </node> + +</launch> diff --git a/launch/ceti-table_simulated_main_dummy_selector.launch b/launch/ceti-table_simulated_main_dummy_selector.launch new file mode 100644 index 0000000000000000000000000000000000000000..6ce6439b0ecaef20e16328ce9c1c8d8e6a7c861a --- /dev/null +++ b/launch/ceti-table_simulated_main_dummy_selector.launch @@ -0,0 +1,4 @@ +<launch> + <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/> + <include file="$(find ccf_immersive_sorting)/launch/ceti-table_simulated_main.launch" /> +</launch>