Skip to content
Snippets Groups Projects
Commit a15ac815 authored by KingMaZito's avatar KingMaZito
Browse files

searching for double planning solutions

parent 03cda6f4
Branches
No related tags found
No related merge requests found
Pipeline #14949 failed
Stage: build
Expand all Show whitespace changes
Inline Side-by-side
Showing
with 991 additions and 1132 deletions
src/mtc/results/dummy/651836672.yaml 0 → 100644
+ 25
− 0
View file @ a15ac815
{ 'objects' : [
{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'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' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'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' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
]}
\ No newline at end of file
src/mtc/results/dummy/657141845.yaml 0 → 100644
+ 27
− 0
View file @ a15ac815
{ 'objects' : [
{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'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' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_table_top', 'pos': { 'x': -0.200002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': 0.000000 , 'y': -0.087264 } },
{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'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' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_table_top', 'pos': { 'x': -0.086267, 'y': 1.400032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}},
{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.029400, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.563750, 'y': 1.343165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.119172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.419206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
]}
\ No newline at end of file
src/mtc/results/dummy/663611616.yaml 0 → 100644
+ 25
− 0
View file @ a15ac815
{ 'objects' : [
{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'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' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'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' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}},
{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 1.184998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
]}
\ No newline at end of file
src/mtc/results/dummy/676899603.yaml 0 → 100644
+ 26
− 0
View file @ a15ac815
{ 'objects' : [
{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'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' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'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' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'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' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'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' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}},
{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
]}
\ No newline at end of file
src/mtc/src/base_routine.cpp
+ 333
− 679
View file @ a15ac815
This diff is collapsed.
src/mtc/src/cell_routine.cpp
+ 268
− 340
View file @ a15ac815
This diff is collapsed.
src/mtc/src/impl/mediator.cpp
+ 223
− 39
View file @ a15ac815
...@@ -3,7 +3,52 @@ ...@@ -3,7 +3,52 @@
#include "impl/wing_rviz_decorator.h" #include "impl/wing_rviz_decorator.h"
#include <tf2/LinearMath/Scalar.h> #include <tf2/LinearMath/Scalar.h>
void Mediator::setup_rviz(){
visualization_msgs::MarkerArray ma;
for (int i = 0; i < wings_.size(); i++){
Robot* r = dynamic_cast<Robot*>(robots_[i]);
for (int j = 0; j < wings_[i].size(); j++){
Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(wings_[i][j]);
Wing* w = dynamic_cast<Wing*>(wrd->wing());
visualization_msgs::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time();
marker.ns = w->name();
marker.id = 1;
marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = w->world_tf().getOrigin().getX();
marker.pose.position.y = w->world_tf().getOrigin().getY();
marker.pose.position.z = w->world_tf().getOrigin().getZ();
marker.pose.orientation.x = w->world_tf().getRotation().getX();
marker.pose.orientation.y = w->world_tf().getRotation().getY();
marker.pose.orientation.z = w->world_tf().getRotation().getZ();
marker.pose.orientation.w = w->world_tf().getRotation().getW();
marker.scale.x = w->size().getX();
marker.scale.y = w->size().getY();
marker.scale.z = w->size().getZ();
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.color.a = 1.0;
ma.markers.push_back(marker);
}
}
//pub_->publish(ma);
}
void Mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){
for (int i =0; i < wbp.size(); i++){
std::vector<Abstract_robot_element*> v;
for (int j =0; j < wbp[i].size(); j++){
Abstract_robot_element* are = new Wing_rviz_decorator( new Wing(wbp[i][j].name_, wbp[i][j].pos_,wbp[i][j].size_));
v.push_back(are);
}
wings_.push_back(v);
}
}
bool Mediator::check_collision( const int& robot){ bool Mediator::check_collision( const int& robot){
bool succ = true; bool succ = true;
...@@ -12,6 +57,7 @@ bool Mediator::check_collision( const int& robot){ ...@@ -12,6 +57,7 @@ bool Mediator::check_collision( const int& robot){
count_v.resize(r->observers().size()+1); count_v.resize(r->observers().size()+1);
std::string str; std::string str;
visualization_msgs::MarkerArray ma;
for(long unsigned int j = 0; j < objects_[robot].size(); j++){ for(long unsigned int j = 0; j < objects_[robot].size(); j++){
visualization_msgs::Marker marker; visualization_msgs::Marker marker;
marker.header.frame_id = "map"; marker.header.frame_id = "map";
...@@ -42,10 +88,10 @@ bool Mediator::check_collision( const int& robot){ ...@@ -42,10 +88,10 @@ bool Mediator::check_collision( const int& robot){
marker.color.a = 1.0; marker.color.a = 1.0;
succ = false; succ = false;
} }
ma.markers.push_back(marker);
robots_[robot]->workload_checker(count_v, objects_[robot][j]); robots_[robot]->workload_checker(count_v, objects_[robot][j]);
rviz_markers_->markers.push_back(marker);
} }
pub_->publish(ma);
for (int& i : count_v){ for (int& i : count_v){
if(i == 0) {succ = false; break;} if(i == 0) {succ = false; break;}
...@@ -73,78 +119,177 @@ void Mediator::mediate(){ ...@@ -73,78 +119,177 @@ void Mediator::mediate(){
} }
} }
for (long unsigned int i = objects_[0].size()-1; i > 0; i--){ for(int i = 0; i < objects_.size(); i++){
if(objects_[0][i].getOrigin().distance(objects_[1].back().getOrigin()) == 0) objects_[1].pop_back(); if (i+1 < objects_.size()){
for (long unsigned int j = objects_[i].size()-1; j > 0; j--){
if(objects_[i][j].getOrigin().distance(objects_[i+1].back().getOrigin()) == 0) objects_[i+1].pop_back();
}
}
} }
calculate(ground_per_robot);
}
ros::Rate loop_rate(10); void Mediator::calculate(std::vector<tf2::Transform>& ground_per_robot){
visualization_msgs::MarkerArray ma;
int r1 = 0; int r1 = 0;
Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]); Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
for(int j = 0; j <= 7; j++){ for(int j = 0; j <= 7; j++){
std::bitset<3> wing_config(j); std::bitset<3> wing_config(j);
build_wings(wing_config, r1); build_wings(wing_config, r1);
visualization_msgs::Marker m;
m.header.frame_id = "map";
m.header.stamp = ros::Time();
m.ns = ceti1->name();
m.id = 1;
m.type = visualization_msgs::Marker::CUBE;
m.action = visualization_msgs::Marker::ADD;
m.pose.position.x = ceti1->tf().getOrigin().getX();
m.pose.position.y = ceti1->tf().getOrigin().getY();
m.pose.position.z = ceti1->tf().getOrigin().getZ();
m.pose.orientation.x = ceti1->tf().getRotation().getX();
m.pose.orientation.y = ceti1->tf().getRotation().getY();
m.pose.orientation.z = ceti1->tf().getRotation().getZ();
m.pose.orientation.w = ceti1->tf().getRotation().getW();
m.scale.x = ceti1->size().getX();
m.scale.y = ceti1->size().getY();
m.scale.z = ceti1->tf().getOrigin().getZ()*2;
m.color.r = 1.0;
m.color.g = 1.0;
m.color.b = 1.0;
m.color.a = 1.0;
ma.markers.push_back(m);
pub_->publish(ma);
ma.markers.clear();
ros::Duration timer(0.10);
for (long unsigned int i = 0; i < ground_per_robot.size(); i++){ for (long unsigned int i = 0; i < ground_per_robot.size(); i++){
robots_[0]->set_tf(ground_per_robot[i]); ceti1->set_tf(ground_per_robot[i]);
// vis. robot 1
for ( float p = 0; p < 2*M_PI; p += 0.0872665f){ for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) markers->markers.clear();
rviz_markers_->markers.clear();
ceti1->rotate(0.0872665f); ceti1->rotate(0.0872665f);
ceti1->notify(); ceti1->notify();
if (check_collision(0)) { m.action = visualization_msgs::Marker::MODIFY;
m.pose.position.x = ceti1->tf().getOrigin().getX();
m.pose.position.y = ceti1->tf().getOrigin().getY();
m.pose.position.z = ceti1->tf().getOrigin().getZ();
m.pose.orientation.x = ceti1->tf().getRotation().getX();
m.pose.orientation.y = ceti1->tf().getRotation().getY();
m.pose.orientation.z = ceti1->tf().getRotation().getZ();
m.pose.orientation.w = ceti1->tf().getRotation().getW();
ma.markers.push_back(m);
for (Abstract_robot_element* are : ceti1->observers()){
Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
ma.markers.push_back(*wrd->markers());
}
pub_->publish(ma);
ma.markers.clear();
if (check_collision(r1)) {
approximation(ceti1); approximation(ceti1);
} else { } else {
continue; continue;
} }
for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) pub_->publish(*markers);
pub_->publish(*rviz_markers_); //timer.sleep();
} }
} }
ceti1->reset(); ceti1->reset();
m.action = visualization_msgs::Marker::DELETEALL;
ma.markers.push_back(m);
pub_->publish(ma);
ma.markers.clear();
} }
} }
void Mediator::approximation(Robot* robot){ void Mediator::approximation(Robot* robot){
ROS_INFO("assigne result to first robot..."); Robot* ceti1 = dynamic_cast<Robot*>(robots_[1]);
Robot* ceti = dynamic_cast<Robot*>(robots_[1]);
int r1 = 1; int r1 = 1;
visualization_msgs::MarkerArray ma;
for (int i = 0; i <= 7; i++){ for (int i = 0; i <= 7; i++){
std::bitset<3> wing_config(i); std::bitset<3> wing_config(i);
build_wings(wing_config, r1); build_wings(wing_config, r1);
visualization_msgs::Marker m;
m.header.frame_id = "map";
m.header.stamp = ros::Time();
m.ns = ceti1->name();
m.id = 1;
m.type = visualization_msgs::Marker::CUBE;
m.action = visualization_msgs::Marker::ADD;
m.pose.position.x = ceti1->tf().getOrigin().getX();
m.pose.position.y = ceti1->tf().getOrigin().getY();
m.pose.position.z = ceti1->tf().getOrigin().getZ();
m.pose.orientation.x = ceti1->tf().getRotation().getX();
m.pose.orientation.y = ceti1->tf().getRotation().getY();
m.pose.orientation.z = ceti1->tf().getRotation().getZ();
m.pose.orientation.w = ceti1->tf().getRotation().getW();
m.scale.x = ceti1->size().getX();
m.scale.y = ceti1->size().getY();
m.scale.z = ceti1->tf().getOrigin().getZ()*2;
m.color.r = 1.0;
m.color.g = 1.0;
m.color.b = 1.0;
m.color.a = 1.0;
ma.markers.push_back(m);
pub_->publish(ma);
ma.markers.clear();
for (Abstract_robot_element* fields : robot->access_fields()) { for (Abstract_robot_element* fields : robot->access_fields()) {
ceti->set_tf(fields->world_tf()); ceti1->set_tf(fields->world_tf());
//ROS_INFO("field size %i", robot->access_fields().size());
for ( float p = 0; p < 2*M_PI; p += M_PI/2){ for ( float p = 0; p < 2*M_PI; p += M_PI/2){
for (visualization_msgs::MarkerArray* markers : robot->rviz_markers()) markers->markers.clear(); ceti1->rotate(M_PI/2);
for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear(); ceti1->notify();
rviz_markers_->markers.clear();
ceti->rotate(M_PI/2); m.action = visualization_msgs::Marker::MODIFY;
ceti->notify(); m.pose.position.x = ceti1->tf().getOrigin().getX();
m.pose.position.y = ceti1->tf().getOrigin().getY();
m.pose.position.z = ceti1->tf().getOrigin().getZ();
m.pose.orientation.x = ceti1->tf().getRotation().getX();
m.pose.orientation.y = ceti1->tf().getRotation().getY();
m.pose.orientation.z = ceti1->tf().getRotation().getZ();
m.pose.orientation.w = ceti1->tf().getRotation().getW();
ma.markers.push_back(m);
for (Abstract_robot_element* are : ceti1->observers()){
Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
ma.markers.push_back(*wrd->markers());
}
if (robot->check_robot_collision(ceti)) continue; pub_->publish(ma);
ma.markers.clear();
if (robot->check_robot_collision(ceti1)) continue;
if (check_collision(1)) { if (check_collision(r1)) {
ROS_INFO("should work"); write_file(robot, ceti1);
write_file(robot, ceti);
} else { } else {
continue; continue;
} }
for (visualization_msgs::MarkerArray* markers : robot->rviz_markers()) pub_->publish(*markers);
for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) pub_->publish(*markers);
pub_->publish(*rviz_markers_);
} }
//relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ())); //relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ()));
} }
ceti->reset(); ceti1->reset();
m.action = visualization_msgs::Marker::DELETEALL;
ma.markers.push_back(m);
pub_->publish(ma);
ma.markers.clear();
} }
} }
void Mediator::write_file(Robot* A, Robot* B){ void Mediator::write_file(Robot* A, Robot* B){
std::ofstream o(ros::package::getPath("mtc") + "/results/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml"); std::ofstream o(ros::package::getPath("mtc") + "/results/" + dirname_ + "/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
double r,p,y; double r,p,y;
tf2::Matrix3x3 m(A->tf().getRotation()); tf2::Matrix3x3 m(A->tf().getRotation());
m.getRPY(r,p,y); m.getRPY(r,p,y);
...@@ -215,7 +360,7 @@ void Mediator::write_file(Robot* A, Robot* B){ ...@@ -215,7 +360,7 @@ void Mediator::write_file(Robot* A, Robot* B){
float width = w->size().getY(); float width = w->size().getY();
float height = w->size().getZ(); float height = w->size().getZ();
ss << "{ 'id': 'table" << A->name().back() << "_" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n"; ss << "{ 'id': 'table" << A->name().back() << "_" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n";
} }
...@@ -234,7 +379,7 @@ void Mediator::write_file(Robot* A, Robot* B){ ...@@ -234,7 +379,7 @@ void Mediator::write_file(Robot* A, Robot* B){
float width = w->size().getY(); float width = w->size().getY();
float height = w->size().getZ(); float height = w->size().getZ();
ss << "{ 'id': 'table" << B->name().back() << "_" << w->name() << "', 'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n"; ss << "{ 'id': 'table" << B->name().back() << "_" << w->name() << "', 'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n";
} }
tf2::Transform tf_arm = A->tf() * A->root_tf(); tf2::Transform tf_arm = A->tf() * A->root_tf();
float arm_x = tf_arm.getOrigin().getX(); float arm_x = tf_arm.getOrigin().getX();
...@@ -268,10 +413,49 @@ void Mediator::write_file(Robot* A, Robot* B){ ...@@ -268,10 +413,49 @@ void Mediator::write_file(Robot* A, Robot* B){
void Mediator::build_wings(std::bitset<3>& wing, int& robot){ void Mediator::build_wings(std::bitset<3>& wing, int& robot){
std::bitset<3> result = robots_[robot]->observer_mask() & wing; std::bitset<3> result = robots_[robot]->observer_mask() & wing;
Robot* ceti = dynamic_cast<Robot*>(robots_[robot]); Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
for (std::size_t i = 0; i < result.size(); i++){ for (std::size_t i = 0; i < result.size(); i++){
if (result[i]){ if (result[i]){
Abstract_robot_element* moveit_right = new Wing_rviz_decorator(new Wing(wings_[robot][i].name_, wings_[robot][i].pos_, wings_[robot][i].size_), ceti->rviz_markers()[0]); ceti->register_observers(wings_[robot][i]);
ceti->register_observers(moveit_right);
} }
} }
visualization_msgs::MarkerArray ma;
for (Abstract_robot_element* are : ceti->observers()){
Wing_rviz_decorator* wad = dynamic_cast<Wing_rviz_decorator*>(are);
wad->markers()->action = visualization_msgs::Marker::ADD;
ma.markers.push_back(*wad->markers());
}
pub_->publish(ma);
}
void Mediator::publish(Robot* ar){
visualization_msgs::MarkerArray ma;
visualization_msgs::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time();
marker.ns = ar->name();
marker.id = 1;
marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::MODIFY;
marker.pose.position.x = ar->tf().getOrigin().getX();
marker.pose.position.y = ar->tf().getOrigin().getY();
marker.pose.position.z = ar->tf().getOrigin().getZ();
marker.pose.orientation.x = ar->tf().getRotation().getX();
marker.pose.orientation.y = ar->tf().getRotation().getY();
marker.pose.orientation.z = ar->tf().getRotation().getZ();
marker.pose.orientation.w = ar->tf().getRotation().getW();
marker.scale.x = ar->size().getX();
marker.scale.y = ar->size().getY();
marker.scale.z = ar->size().getZ();
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.color.a = 1.0;
for (auto w : ar->observers()){
Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(w);
ma.markers.push_back(*wrd->markers());
}
//pub_->publish(ma);
} }
src/mtc/src/impl/moveit_mediator.cpp
+ 37
− 39
View file @ a15ac815
...@@ -9,8 +9,9 @@ void Moveit_mediator::publish_tables(moveit::planning_interface::PlanningSceneIn ...@@ -9,8 +9,9 @@ void Moveit_mediator::publish_tables(moveit::planning_interface::PlanningSceneIn
ros::WallDuration sleep_time(1.0); ros::WallDuration sleep_time(1.0);
for(long unsigned int i = 0; i < robots_.size();i++){ for(long unsigned int i = 0; i < robots_.size();i++){
Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]); Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
for (moveit_msgs::CollisionObject* markers : ceti->coll_markers()) { for (Abstract_robot_element* are : ceti->observers()) {
psi.applyCollisionObject(*markers); Wing_moveit_decorator* wmd = dynamic_cast<Wing_moveit_decorator*>(are);
psi.applyCollisionObject(*wmd->markers());
sleep_time.sleep(); sleep_time.sleep();
} }
} }
...@@ -55,7 +56,9 @@ void Moveit_mediator::mediate(){ ...@@ -55,7 +56,9 @@ void Moveit_mediator::mediate(){
bottle.operation = bottle.ADD; bottle.operation = bottle.ADD;
psi.applyCollisionObject(bottle); psi.applyCollisionObject(bottle);
rewrite_task_template(robots_[0], bottle);
tf2::Transform target(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.1f, 0.6f, 0.9305f));
rewrite_task_template(robots_[0], psi.getObjects().at("bottle"), target);
Yaml_Mtc_Parser parser = Yaml_Mtc_Parser(); Yaml_Mtc_Parser parser = Yaml_Mtc_Parser();
...@@ -68,34 +71,10 @@ void Moveit_mediator::mediate(){ ...@@ -68,34 +71,10 @@ void Moveit_mediator::mediate(){
execute_result = task.execute(*task.solutions().front()); execute_result = task.execute(*task.solutions().front());
//task.introspection().publishSolution(*task.solutions().front()); //task.introspection().publishSolution(*task.solutions().front());
/*
if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
ROS_INFO("irgendwas ging schief");
}
*/
}
ROS_INFO("durch, print mich ");
} catch (const moveit::task_constructor::InitStageException& ex) {
std::cerr << "planning failed with exception" << std::endl << ex << task;
}
/*
ROS_INFO("durch, print mich daddy");
tf2::Vector3 source2(0.1, 1, 0.9305);
rewrite_task_template(robots_[1], source2);
task.clear();
task = parser.init_task(this->nh_);
this->nh_.getParam("max_planning_solutions", max_planning_solutions);
try {
if (task.plan(max_planning_solutions)) {
task.introspection().publishSolution(*task.solutions().front());
} }
} catch (const moveit::task_constructor::InitStageException& ex) { } catch (const moveit::task_constructor::InitStageException& ex) {
std::cerr << "planning failed with exception" << std::endl << ex << task; std::cerr << "planning failed with exception" << std::endl << ex << task;
} }
*/
ros::waitForShutdown(); ros::waitForShutdown();
}; };
...@@ -104,18 +83,27 @@ void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){ ...@@ -104,18 +83,27 @@ void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
Robot* ceti = dynamic_cast<Robot*>(robots_[robot]); Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
for (std::size_t i = 0; i < result.size(); i++){ for (std::size_t i = 0; i < result.size(); i++){
if (result[i]){ if (result[i]){
moveit_msgs::CollisionObject* marker = new moveit_msgs::CollisionObject(); ceti->register_observers(wings_[robot][i]);
ceti->add_coll_markers(marker); }
Abstract_robot_element* moveit_right = new Wing_moveit_decorator(new Wing(wings_[robot][i].name_, wings_[robot][i].pos_, wings_[robot][i].size_), marker);
ceti->register_observers(moveit_right);
} }
} }
void Moveit_mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){
for (int i =0; i < wbp.size(); i++){
std::vector<Abstract_robot_element*> v;
for (int j =0; j < wbp[i].size(); j++){
Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i][j].name_, wbp[i][j].pos_,wbp[i][j].size_));
v.push_back(are);
} }
wings_.push_back(v);
}
};
void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source){
tf2::Transform t(tf2::Quaternion(source.primitive_poses[0].orientation.x, source.primitive_poses[0].orientation.y, source.primitive_poses[0].orientation.z, source.primitive_poses[0].orientation.w), tf2::Vector3(source.primitive_poses[0].position.x, source.primitive_poses[0].position.y, source.primitive_poses[0].position.z));
tf2::Transform target(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.1f, 0.6f, 0.9305f));
void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){
tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
// unique ids cartesian, samplimg, interpolation, ready, hand_open, pick_up
setup_task();
XmlRpc::XmlRpcValue task; XmlRpc::XmlRpcValue task;
nh_.getParam("task/stages", task); nh_.getParam("task/stages", task);
...@@ -168,10 +156,9 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll ...@@ -168,10 +156,9 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll
c[4]["set"]["allow_collisions"]["first"] = source.id; c[4]["set"]["allow_collisions"]["first"] = source.id;
c[4]["set"]["allow_collisions"]["second"] = e; c[4]["set"]["allow_collisions"]["second"] = e;
h["x"] = static_cast<double>(0); c[2]["stage"]["set"]["pose"]["point"]["x"]= static_cast<double>(target.getOrigin().getX());
h["y"] = static_cast<double>(-0.5f); c[2]["stage"]["set"]["pose"]["point"]["y"]= static_cast<double>(target.getOrigin().getY());
h["z"] = static_cast<double>(0.9305f); c[2]["stage"]["set"]["pose"]["point"]["z"]= static_cast<double>(target.getOrigin().getZ());
c[2]["stage"]["set"]["pose"]["point"]= h;
task[2]["properties"] = a; task[2]["properties"] = a;
...@@ -214,7 +201,18 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll ...@@ -214,7 +201,18 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll
task[6]["stages"] = c; task[6]["stages"] = c;
task[4]["stages"] = b; task[4]["stages"] = b;
nh_.setParam("task/stages", task);
}
nh_.setParam("task/stages", task); void Moveit_mediator::setup_task (){
for (Abstract_robot* robot : robots_){
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
properties_[mr->map()["group"].first] = mr->map()["group"].second;
properties_[mr->map()["eef"].first] = mr->map()["eef"].second;
properties_[mr->map()["hand_grasping_frame"].first] = mr->map()["hand_grasping_frame"].second;
properties_[mr->map()["ik_frame"].first] = mr->map()["ik_frame"].second;
properties_[mr->map()["hand"].first] = mr->map()["hand"].second;
}
} }
src/mtc/src/impl/robot.cpp
+ 0
− 2
View file @ a15ac815
...@@ -104,7 +104,6 @@ bool Robot::check_single_object_collision(tf2::Transform& obj, std::string& str) ...@@ -104,7 +104,6 @@ bool Robot::check_single_object_collision(tf2::Transform& obj, std::string& str)
full_area = area_calculation(A,B,C) + area_calculation(A,D,C); full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A); sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
if ((std::floor(sum*100)/100.f) <= full_area) { if ((std::floor(sum*100)/100.f) <= full_area) {
str = ceti->name(); str = ceti->name();
return true; return true;
...@@ -120,7 +119,6 @@ void Robot::reset(){ ...@@ -120,7 +119,6 @@ void Robot::reset(){
observers_.clear(); observers_.clear();
access_fields_.clear(); access_fields_.clear();
generate_access_fields(); generate_access_fields();
tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0, tf().getOrigin().getZ())); tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0, tf().getOrigin().getZ()));
} }
......
src/mtc/src/impl/wing_rviz_decorator.cpp
+ 26
− 33
View file @ a15ac815
#include "impl/wing_rviz_decorator.h" #include "impl/wing_rviz_decorator.h"
void Wing_rviz_decorator::update(tf2::Transform& tf) { void Wing_rviz_decorator::update(tf2::Transform& tf) {
if(markers_->markers.empty()) { //input_filter(tf);
input_filter(tf);
}
Abstract_robot_element_decorator::update(tf); Abstract_robot_element_decorator::update(tf);
output_filter(); output_filter();
} }
...@@ -15,8 +13,8 @@ void Wing_rviz_decorator::input_filter(tf2::Transform& tf) { ...@@ -15,8 +13,8 @@ void Wing_rviz_decorator::input_filter(tf2::Transform& tf) {
visualization_msgs::Marker marker; visualization_msgs::Marker marker;
marker.header.frame_id = "map"; marker.header.frame_id = "map";
marker.header.stamp = ros::Time(); marker.header.stamp = ros::Time();
marker.ns = "ceti_table "; marker.ns = "";
marker.id = *((int*)(&markers_)); marker.id = 1;
marker.type = visualization_msgs::Marker::CUBE; marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::ADD; marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = world_origin.getX(); marker.pose.position.x = world_origin.getX();
...@@ -33,7 +31,6 @@ void Wing_rviz_decorator::input_filter(tf2::Transform& tf) { ...@@ -33,7 +31,6 @@ void Wing_rviz_decorator::input_filter(tf2::Transform& tf) {
marker.color.g = 1.0; marker.color.g = 1.0;
marker.color.b = 1.0; marker.color.b = 1.0;
marker.color.a = 1.0; // Don't forget to set the alpha marker.color.a = 1.0; // Don't forget to set the alpha
markers_->markers.push_back(marker);
} }
void Wing_rviz_decorator::output_filter() { void Wing_rviz_decorator::output_filter() {
...@@ -41,37 +38,34 @@ void Wing_rviz_decorator::output_filter() { ...@@ -41,37 +38,34 @@ void Wing_rviz_decorator::output_filter() {
tf2::Vector3 world_origin = wing->world_tf().getOrigin(); tf2::Vector3 world_origin = wing->world_tf().getOrigin();
tf2::Quaternion world_quat = wing->world_tf().getRotation().normalized(); tf2::Quaternion world_quat = wing->world_tf().getRotation().normalized();
visualization_msgs::Marker marker; marker_->header.frame_id = "map";
marker.header.frame_id = "map"; marker_->header.stamp = ros::Time();
marker.header.stamp = ros::Time(); marker_->ns = wing->name();
marker.ns = "wings and robo "; marker_->id = 1;
marker.id = *((int*)(&next_)); marker_->type = visualization_msgs::Marker::CUBE;
marker.type = visualization_msgs::Marker::CUBE; marker_->action = visualization_msgs::Marker::MODIFY;
marker.action = visualization_msgs::Marker::ADD; marker_->pose.position.x = world_origin.getX();
marker.pose.position.x = world_origin.getX(); marker_->pose.position.y = world_origin.getY();
marker.pose.position.y = world_origin.getY(); marker_->pose.position.z = world_origin.getZ();
marker.pose.position.z = world_origin.getZ(); marker_->pose.orientation.x = world_quat.getX();
marker.pose.orientation.x = world_quat.getX(); marker_->pose.orientation.y = world_quat.getY();
marker.pose.orientation.y = world_quat.getY(); marker_->pose.orientation.z = world_quat.getZ();
marker.pose.orientation.z = world_quat.getZ(); marker_->pose.orientation.w = world_quat.getW();
marker.pose.orientation.w = world_quat.getW(); marker_->scale.x = wing->size().getX();
marker.scale.x = wing->size().getX(); marker_->scale.y = wing->size().getY();
marker.scale.y = wing->size().getY(); marker_->scale.z = wing->size().getZ();
marker.scale.z = wing->size().getZ(); marker_->color.r = 1.0;
marker.color.r = 1.0; marker_->color.g = 1.0;
marker.color.g = 1.0; marker_->color.b = 1.0;
marker.color.b = 1.0; marker_->color.a = 1.0; // Don't forget to set the alpha!
marker.color.a = 1.0; // Don't forget to set the alpha! /*
markers_->markers.push_back(marker);
visualization_msgs::Marker bounds; visualization_msgs::Marker bounds;
bounds.header.frame_id = "map"; bounds.header.frame_id = "map";
bounds.header.stamp = ros::Time(); bounds.header.stamp = ros::Time();
bounds.ns = "bounds"; bounds.ns = "bounds";
bounds.id = *((int*)(&wing)); bounds.id = *((int*)(&wing));
bounds.type = visualization_msgs::Marker::POINTS; bounds.type = visualization_msgs::Marker::POINTS;
bounds.action = visualization_msgs::Marker::ADD; bounds.action = visualization_msgs::Marker::MODIFY;
bounds.scale.x = 0.01f; bounds.scale.x = 0.01f;
bounds.scale.y = 0.01f; bounds.scale.y = 0.01f;
bounds.scale.z = 0.01f; bounds.scale.z = 0.01f;
...@@ -89,7 +83,6 @@ void Wing_rviz_decorator::output_filter() { ...@@ -89,7 +83,6 @@ void Wing_rviz_decorator::output_filter() {
point.z = point_posistion.getOrigin().getZ(); point.z = point_posistion.getOrigin().getZ();
bounds.points.push_back(point); bounds.points.push_back(point);
} }
markers_->markers.push_back(bounds); */
} }
src/reachability/package.xml
+ 1
− 0
View file @ a15ac815
...@@ -36,6 +36,7 @@ ...@@ -36,6 +36,7 @@
<exec_depend>eigen_conversions</exec_depend> <exec_depend>eigen_conversions</exec_depend>
<exec_depend>YAML_CPP</exec_depend> <exec_depend>YAML_CPP</exec_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>
<!-- Other tools can request additional information be placed here --> <!-- Other tools can request additional information be placed here -->
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment