From 15ab45aea97cb8051d50dfb6958db1d6b6b03749 Mon Sep 17 00:00:00 2001 From: KingMaZito <matteo.aneddama@icloud.com> Date: Mon, 14 Nov 2022 12:10:54 +0100 Subject: [PATCH] some fixes --- .../profiles/default/devel_collisions.txt | 2 +- .../robots/ceti_double.urdf.xacro | 2 +- src/mtc/README.md | 7 ------ src/mtc/launch/base_routine.launch | 8 +++--- src/mtc/launch/cell_routine.launch | 5 ++-- src/mtc/launch/mtc2taskspace.launch | 6 ++++- src/mtc/results/dummy.yaml | 25 ------------------- .../{1002613186.yaml => -1113960400.yaml} | 0 .../{945520390.yaml => -1130060311.yaml} | 0 .../{934553240.yaml => -1141587806.yaml} | 0 .../{957818188.yaml => -1164134400.yaml} | 0 .../{990058227.yaml => -1177867363.yaml} | 0 .../{978772872.yaml => -1190597504.yaml} | 0 .../{841127331.yaml => -1241254685.yaml} | 0 .../{890568488.yaml => -1287858113.yaml} | 0 .../{409146028.yaml => -1725746253.yaml} | 0 .../{396809119.yaml => -1739448485.yaml} | 0 .../{385732106.yaml => -1751589910.yaml} | 0 .../{453123235.yaml => -1775227539.yaml} | 0 .../{440798129.yaml => -1789367903.yaml} | 0 .../{429872256.yaml => -1801493263.yaml} | 0 .../{512151887.yaml => -2019620283.yaml} | 0 .../{568064102.yaml => -2070819246.yaml} | 0 .../{-953422313.yaml => 1086409496.yaml} | 0 .../{-551819248.yaml => 1086505991.yaml} | 0 .../{-540664156.yaml => 1097760815.yaml} | 0 .../{-997664293.yaml => 1132989736.yaml} | 0 .../{-598701477.yaml => 1138917448.yaml} | 0 .../{-587407747.yaml => 1150205447.yaml} | 0 .../{-239267840.yaml => 1420108742.yaml} | 0 .../{-288597854.yaml => 1467312061.yaml} | 0 .../dummy/{-26465509.yaml => 1634357786.yaml} | 0 .../dummy/{-73418491.yaml => 1681783699.yaml} | 0 .../dummy/{106540234.yaml => 1830642082.yaml} | 0 .../dummy/{161620969.yaml => 1878182050.yaml} | 0 .../{-118381120.yaml => 1927303313.yaml} | 0 .../{-107328901.yaml => 1938979366.yaml} | 0 .../dummy/{241047948.yaml => 1951197037.yaml} | 0 .../{-139322185.yaml => 1952346695.yaml} | 0 .../{-163087766.yaml => 1974030854.yaml} | 0 .../{-151902566.yaml => 1985613525.yaml} | 0 .../dummy/{-94891628.yaml => 1998093904.yaml} | 0 .../dummy/{288860765.yaml => 1998619737.yaml} | 0 .../{-1299660620.yaml => 287216712.yaml} | 0 .../{-1287907953.yaml => 299277607.yaml} | 0 .../{-1274604634.yaml => 312857611.yaml} | 0 .../{-1346543685.yaml => 335131185.yaml} | 0 .../{-1334648679.yaml => 347069224.yaml} | 0 .../{-1321395857.yaml => 360590070.yaml} | 0 .../{-1191626777.yaml => 397572609.yaml} | 0 .../{-1179960295.yaml => 409649008.yaml} | 0 .../{-1166700698.yaml => 423450698.yaml} | 0 .../{-1238289141.yaml => 445682784.yaml} | 0 .../{-1226601008.yaml => 461533561.yaml} | 0 .../{-1213390021.yaml => 477108873.yaml} | 0 .../{-1083227167.yaml => 518554172.yaml} | 0 .../{-1071524157.yaml => 530725211.yaml} | 0 .../{-1058266167.yaml => 544173349.yaml} | 0 .../{-1130182242.yaml => 571021364.yaml} | 0 .../{-1118397307.yaml => 582894029.yaml} | 0 .../{-1105096712.yaml => 597217384.yaml} | 0 .../{-1280463185.yaml => 759067436.yaml} | 0 .../{-1269289091.yaml => 770754525.yaml} | 0 .../{-1256829190.yaml => 784016529.yaml} | 0 .../{-1325476019.yaml => 807557424.yaml} | 0 .../{-1313991264.yaml => 827720657.yaml} | 0 .../{-1301462009.yaml => 840878174.yaml} | 0 .../{-1164235259.yaml => 886296696.yaml} | 0 .../{-1213000891.yaml => 931879073.yaml} | 0 src/mtc/src/impl/mediator.cpp | 8 +++--- src/mtc/src/mtc2taskspace.cpp | 23 ++++++----------- 71 files changed, 26 insertions(+), 60 deletions(-) delete mode 100644 src/mtc/results/dummy.yaml rename src/mtc/results/dummy/{1002613186.yaml => -1113960400.yaml} (100%) rename src/mtc/results/dummy/{945520390.yaml => -1130060311.yaml} (100%) rename src/mtc/results/dummy/{934553240.yaml => -1141587806.yaml} (100%) rename src/mtc/results/dummy/{957818188.yaml => -1164134400.yaml} (100%) rename src/mtc/results/dummy/{990058227.yaml => -1177867363.yaml} (100%) rename src/mtc/results/dummy/{978772872.yaml => -1190597504.yaml} (100%) rename src/mtc/results/dummy/{841127331.yaml => -1241254685.yaml} (100%) rename src/mtc/results/dummy/{890568488.yaml => -1287858113.yaml} (100%) rename src/mtc/results/dummy/{409146028.yaml => -1725746253.yaml} (100%) rename src/mtc/results/dummy/{396809119.yaml => -1739448485.yaml} (100%) rename src/mtc/results/dummy/{385732106.yaml => -1751589910.yaml} (100%) rename src/mtc/results/dummy/{453123235.yaml => -1775227539.yaml} (100%) rename src/mtc/results/dummy/{440798129.yaml => -1789367903.yaml} (100%) rename src/mtc/results/dummy/{429872256.yaml => -1801493263.yaml} (100%) rename src/mtc/results/dummy/{512151887.yaml => -2019620283.yaml} (100%) rename src/mtc/results/dummy/{568064102.yaml => -2070819246.yaml} (100%) rename src/mtc/results/dummy/{-953422313.yaml => 1086409496.yaml} (100%) rename src/mtc/results/dummy/{-551819248.yaml => 1086505991.yaml} (100%) rename src/mtc/results/dummy/{-540664156.yaml => 1097760815.yaml} (100%) rename src/mtc/results/dummy/{-997664293.yaml => 1132989736.yaml} (100%) rename src/mtc/results/dummy/{-598701477.yaml => 1138917448.yaml} (100%) rename src/mtc/results/dummy/{-587407747.yaml => 1150205447.yaml} (100%) rename src/mtc/results/dummy/{-239267840.yaml => 1420108742.yaml} (100%) rename src/mtc/results/dummy/{-288597854.yaml => 1467312061.yaml} (100%) rename src/mtc/results/dummy/{-26465509.yaml => 1634357786.yaml} (100%) rename src/mtc/results/dummy/{-73418491.yaml => 1681783699.yaml} (100%) rename src/mtc/results/dummy/{106540234.yaml => 1830642082.yaml} (100%) rename src/mtc/results/dummy/{161620969.yaml => 1878182050.yaml} (100%) rename src/mtc/results/dummy/{-118381120.yaml => 1927303313.yaml} (100%) rename src/mtc/results/dummy/{-107328901.yaml => 1938979366.yaml} (100%) rename src/mtc/results/dummy/{241047948.yaml => 1951197037.yaml} (100%) rename src/mtc/results/dummy/{-139322185.yaml => 1952346695.yaml} (100%) rename src/mtc/results/dummy/{-163087766.yaml => 1974030854.yaml} (100%) rename src/mtc/results/dummy/{-151902566.yaml => 1985613525.yaml} (100%) rename src/mtc/results/dummy/{-94891628.yaml => 1998093904.yaml} (100%) rename src/mtc/results/dummy/{288860765.yaml => 1998619737.yaml} (100%) rename src/mtc/results/dummy/{-1299660620.yaml => 287216712.yaml} (100%) rename src/mtc/results/dummy/{-1287907953.yaml => 299277607.yaml} (100%) rename src/mtc/results/dummy/{-1274604634.yaml => 312857611.yaml} (100%) rename src/mtc/results/dummy/{-1346543685.yaml => 335131185.yaml} (100%) rename src/mtc/results/dummy/{-1334648679.yaml => 347069224.yaml} (100%) rename src/mtc/results/dummy/{-1321395857.yaml => 360590070.yaml} (100%) rename src/mtc/results/dummy/{-1191626777.yaml => 397572609.yaml} (100%) rename src/mtc/results/dummy/{-1179960295.yaml => 409649008.yaml} (100%) rename src/mtc/results/dummy/{-1166700698.yaml => 423450698.yaml} (100%) rename src/mtc/results/dummy/{-1238289141.yaml => 445682784.yaml} (100%) rename src/mtc/results/dummy/{-1226601008.yaml => 461533561.yaml} (100%) rename src/mtc/results/dummy/{-1213390021.yaml => 477108873.yaml} (100%) rename src/mtc/results/dummy/{-1083227167.yaml => 518554172.yaml} (100%) rename src/mtc/results/dummy/{-1071524157.yaml => 530725211.yaml} (100%) rename src/mtc/results/dummy/{-1058266167.yaml => 544173349.yaml} (100%) rename src/mtc/results/dummy/{-1130182242.yaml => 571021364.yaml} (100%) rename src/mtc/results/dummy/{-1118397307.yaml => 582894029.yaml} (100%) rename src/mtc/results/dummy/{-1105096712.yaml => 597217384.yaml} (100%) rename src/mtc/results/dummy/{-1280463185.yaml => 759067436.yaml} (100%) rename src/mtc/results/dummy/{-1269289091.yaml => 770754525.yaml} (100%) rename src/mtc/results/dummy/{-1256829190.yaml => 784016529.yaml} (100%) rename src/mtc/results/dummy/{-1325476019.yaml => 807557424.yaml} (100%) rename src/mtc/results/dummy/{-1313991264.yaml => 827720657.yaml} (100%) rename src/mtc/results/dummy/{-1301462009.yaml => 840878174.yaml} (100%) rename src/mtc/results/dummy/{-1164235259.yaml => 886296696.yaml} (100%) rename src/mtc/results/dummy/{-1213000891.yaml => 931879073.yaml} (100%) diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 461c7ae..6e11520 100644 --- a/.catkin_tools/profiles/default/devel_collisions.txt +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -1,4 +1,4 @@ /home/matteo/ws_panda/devel/./cmake.lock 42 -/home/matteo/reachability/devel/./cmake.lock 23953 +/home/matteo/reachability/devel/./cmake.lock 24055 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 diff --git a/src/franka_description/robots/ceti_double.urdf.xacro b/src/franka_description/robots/ceti_double.urdf.xacro index 50104db..9619220 100644 --- a/src/franka_description/robots/ceti_double.urdf.xacro +++ b/src/franka_description/robots/ceti_double.urdf.xacro @@ -13,7 +13,7 @@ <link name="world" /> - <xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-151902566.yaml" /> + <xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-1801493263.yaml" /> <xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" /> <xacro:property name="x1" value="${props['objects'][8]['pos']['x']}" /> diff --git a/src/mtc/README.md b/src/mtc/README.md index 5b726e3..f2997d8 100644 --- a/src/mtc/README.md +++ b/src/mtc/README.md @@ -35,10 +35,3 @@ State: in progress. Every executable is working with some explained downfalls th :black_square_button: include ferdinants work (main focus) Im working non stop, so don't forget to pull. - - - -:ballot_box_with_check: - - -:ballot_box_with_check: diff --git a/src/mtc/launch/base_routine.launch b/src/mtc/launch/base_routine.launch index f13e2e6..7522368 100644 --- a/src/mtc/launch/base_routine.launch +++ b/src/mtc/launch/base_routine.launch @@ -1,11 +1,11 @@ <launch> - <arg name="map" default="dummy" /> - <arg name="description" default="dummy" /> + <arg name="map" default="dummy.yaml" /> + <arg name="description" default="dummy.yaml" /> <arg name="resource" default="dummy" /> - <rosparam command="load" file="$(find mtc)/maps/$(arg map).yaml"/> - <rosparam command="load" file="$(find mtc)/descriptions/$(arg description).yaml"/> + <rosparam command="load" file="$(find mtc)/maps/$(arg map)"/> + <rosparam command="load" file="$(find mtc)/descriptions/$(arg description)"/> <rosparam command="load" file="$(find mtc)/resources/$(arg resource).yaml"/> <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam> diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch index 82f43ba..f5c4b99 100644 --- a/src/mtc/launch/cell_routine.launch +++ b/src/mtc/launch/cell_routine.launch @@ -1,10 +1,11 @@ <launch> + <arg name="result" default="dummy.yaml" /> <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> --> <!-- this is to change--> - <rosparam command="load" file="$(find mtc)/results/dummy/-151902566.yaml"/> + <rosparam command="load" file="$(find mtc)/results/dummy/-1801493263.yaml"/> + - <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" /> <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/> <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/> diff --git a/src/mtc/launch/mtc2taskspace.launch b/src/mtc/launch/mtc2taskspace.launch index c165f18..d969135 100644 --- a/src/mtc/launch/mtc2taskspace.launch +++ b/src/mtc/launch/mtc2taskspace.launch @@ -1,4 +1,8 @@ <launch> - <rosparam command="load" file="$(find mtc)/resources/dummy.yaml"/> + <arg name="resource" default="dummy.yaml" /> + + <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam> + + <rosparam command="load" file="$(find mtc)/resources/$(arg resource)"/> <node pkg="mtc" type="mtc2taskspace" name="mtc2taskspace" output="screen" required="true"/> </launch> \ No newline at end of file diff --git a/src/mtc/results/dummy.yaml b/src/mtc/results/dummy.yaml deleted file mode 100644 index d93ffb7..0000000 --- a/src/mtc/results/dummy.yaml +++ /dev/null @@ -1,25 +0,0 @@ -{ '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.000002 , '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.000005, '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_right_panel' , 'pos': { 'x': -0.000004 , '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_left_panel' , 'pos': { 'x': -0.000000 , '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_right_panel', 'pos': { 'x': -0.000007, '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.220002, '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.220005, '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 } } -]} diff --git a/src/mtc/results/dummy/1002613186.yaml b/src/mtc/results/dummy/-1113960400.yaml similarity index 100% rename from src/mtc/results/dummy/1002613186.yaml rename to src/mtc/results/dummy/-1113960400.yaml diff --git a/src/mtc/results/dummy/945520390.yaml b/src/mtc/results/dummy/-1130060311.yaml similarity index 100% rename from src/mtc/results/dummy/945520390.yaml rename to src/mtc/results/dummy/-1130060311.yaml diff --git a/src/mtc/results/dummy/934553240.yaml b/src/mtc/results/dummy/-1141587806.yaml similarity index 100% rename from src/mtc/results/dummy/934553240.yaml rename to src/mtc/results/dummy/-1141587806.yaml diff --git a/src/mtc/results/dummy/957818188.yaml b/src/mtc/results/dummy/-1164134400.yaml similarity index 100% rename from src/mtc/results/dummy/957818188.yaml rename to src/mtc/results/dummy/-1164134400.yaml diff --git a/src/mtc/results/dummy/990058227.yaml b/src/mtc/results/dummy/-1177867363.yaml similarity index 100% rename from src/mtc/results/dummy/990058227.yaml rename to src/mtc/results/dummy/-1177867363.yaml diff --git a/src/mtc/results/dummy/978772872.yaml b/src/mtc/results/dummy/-1190597504.yaml similarity index 100% rename from src/mtc/results/dummy/978772872.yaml rename to src/mtc/results/dummy/-1190597504.yaml diff --git a/src/mtc/results/dummy/841127331.yaml b/src/mtc/results/dummy/-1241254685.yaml similarity index 100% rename from src/mtc/results/dummy/841127331.yaml rename to src/mtc/results/dummy/-1241254685.yaml diff --git a/src/mtc/results/dummy/890568488.yaml b/src/mtc/results/dummy/-1287858113.yaml similarity index 100% rename from src/mtc/results/dummy/890568488.yaml rename to src/mtc/results/dummy/-1287858113.yaml diff --git a/src/mtc/results/dummy/409146028.yaml b/src/mtc/results/dummy/-1725746253.yaml similarity index 100% rename from src/mtc/results/dummy/409146028.yaml rename to src/mtc/results/dummy/-1725746253.yaml diff --git a/src/mtc/results/dummy/396809119.yaml b/src/mtc/results/dummy/-1739448485.yaml similarity index 100% rename from src/mtc/results/dummy/396809119.yaml rename to src/mtc/results/dummy/-1739448485.yaml diff --git a/src/mtc/results/dummy/385732106.yaml b/src/mtc/results/dummy/-1751589910.yaml similarity index 100% rename from src/mtc/results/dummy/385732106.yaml rename to src/mtc/results/dummy/-1751589910.yaml diff --git a/src/mtc/results/dummy/453123235.yaml b/src/mtc/results/dummy/-1775227539.yaml similarity index 100% rename from src/mtc/results/dummy/453123235.yaml rename to src/mtc/results/dummy/-1775227539.yaml diff --git a/src/mtc/results/dummy/440798129.yaml b/src/mtc/results/dummy/-1789367903.yaml similarity index 100% rename from src/mtc/results/dummy/440798129.yaml rename to src/mtc/results/dummy/-1789367903.yaml diff --git a/src/mtc/results/dummy/429872256.yaml b/src/mtc/results/dummy/-1801493263.yaml similarity index 100% rename from src/mtc/results/dummy/429872256.yaml rename to src/mtc/results/dummy/-1801493263.yaml diff --git a/src/mtc/results/dummy/512151887.yaml b/src/mtc/results/dummy/-2019620283.yaml similarity index 100% rename from src/mtc/results/dummy/512151887.yaml rename to src/mtc/results/dummy/-2019620283.yaml diff --git a/src/mtc/results/dummy/568064102.yaml b/src/mtc/results/dummy/-2070819246.yaml similarity index 100% rename from src/mtc/results/dummy/568064102.yaml rename to src/mtc/results/dummy/-2070819246.yaml diff --git a/src/mtc/results/dummy/-953422313.yaml b/src/mtc/results/dummy/1086409496.yaml similarity index 100% rename from src/mtc/results/dummy/-953422313.yaml rename to src/mtc/results/dummy/1086409496.yaml diff --git a/src/mtc/results/dummy/-551819248.yaml b/src/mtc/results/dummy/1086505991.yaml similarity index 100% rename from src/mtc/results/dummy/-551819248.yaml rename to src/mtc/results/dummy/1086505991.yaml diff --git a/src/mtc/results/dummy/-540664156.yaml b/src/mtc/results/dummy/1097760815.yaml similarity index 100% rename from src/mtc/results/dummy/-540664156.yaml rename to src/mtc/results/dummy/1097760815.yaml diff --git a/src/mtc/results/dummy/-997664293.yaml b/src/mtc/results/dummy/1132989736.yaml similarity index 100% rename from src/mtc/results/dummy/-997664293.yaml rename to src/mtc/results/dummy/1132989736.yaml diff --git a/src/mtc/results/dummy/-598701477.yaml b/src/mtc/results/dummy/1138917448.yaml similarity index 100% rename from src/mtc/results/dummy/-598701477.yaml rename to src/mtc/results/dummy/1138917448.yaml diff --git a/src/mtc/results/dummy/-587407747.yaml b/src/mtc/results/dummy/1150205447.yaml similarity index 100% rename from src/mtc/results/dummy/-587407747.yaml rename to src/mtc/results/dummy/1150205447.yaml diff --git a/src/mtc/results/dummy/-239267840.yaml b/src/mtc/results/dummy/1420108742.yaml similarity index 100% rename from src/mtc/results/dummy/-239267840.yaml rename to src/mtc/results/dummy/1420108742.yaml diff --git a/src/mtc/results/dummy/-288597854.yaml b/src/mtc/results/dummy/1467312061.yaml similarity index 100% rename from src/mtc/results/dummy/-288597854.yaml rename to src/mtc/results/dummy/1467312061.yaml diff --git a/src/mtc/results/dummy/-26465509.yaml b/src/mtc/results/dummy/1634357786.yaml similarity index 100% rename from src/mtc/results/dummy/-26465509.yaml rename to src/mtc/results/dummy/1634357786.yaml diff --git a/src/mtc/results/dummy/-73418491.yaml b/src/mtc/results/dummy/1681783699.yaml similarity index 100% rename from src/mtc/results/dummy/-73418491.yaml rename to src/mtc/results/dummy/1681783699.yaml diff --git a/src/mtc/results/dummy/106540234.yaml b/src/mtc/results/dummy/1830642082.yaml similarity index 100% rename from src/mtc/results/dummy/106540234.yaml rename to src/mtc/results/dummy/1830642082.yaml diff --git a/src/mtc/results/dummy/161620969.yaml b/src/mtc/results/dummy/1878182050.yaml similarity index 100% rename from src/mtc/results/dummy/161620969.yaml rename to src/mtc/results/dummy/1878182050.yaml diff --git a/src/mtc/results/dummy/-118381120.yaml b/src/mtc/results/dummy/1927303313.yaml similarity index 100% rename from src/mtc/results/dummy/-118381120.yaml rename to src/mtc/results/dummy/1927303313.yaml diff --git a/src/mtc/results/dummy/-107328901.yaml b/src/mtc/results/dummy/1938979366.yaml similarity index 100% rename from src/mtc/results/dummy/-107328901.yaml rename to src/mtc/results/dummy/1938979366.yaml diff --git a/src/mtc/results/dummy/241047948.yaml b/src/mtc/results/dummy/1951197037.yaml similarity index 100% rename from src/mtc/results/dummy/241047948.yaml rename to src/mtc/results/dummy/1951197037.yaml diff --git a/src/mtc/results/dummy/-139322185.yaml b/src/mtc/results/dummy/1952346695.yaml similarity index 100% rename from src/mtc/results/dummy/-139322185.yaml rename to src/mtc/results/dummy/1952346695.yaml diff --git a/src/mtc/results/dummy/-163087766.yaml b/src/mtc/results/dummy/1974030854.yaml similarity index 100% rename from src/mtc/results/dummy/-163087766.yaml rename to src/mtc/results/dummy/1974030854.yaml diff --git a/src/mtc/results/dummy/-151902566.yaml b/src/mtc/results/dummy/1985613525.yaml similarity index 100% rename from src/mtc/results/dummy/-151902566.yaml rename to src/mtc/results/dummy/1985613525.yaml diff --git a/src/mtc/results/dummy/-94891628.yaml b/src/mtc/results/dummy/1998093904.yaml similarity index 100% rename from src/mtc/results/dummy/-94891628.yaml rename to src/mtc/results/dummy/1998093904.yaml diff --git a/src/mtc/results/dummy/288860765.yaml b/src/mtc/results/dummy/1998619737.yaml similarity index 100% rename from src/mtc/results/dummy/288860765.yaml rename to src/mtc/results/dummy/1998619737.yaml diff --git a/src/mtc/results/dummy/-1299660620.yaml b/src/mtc/results/dummy/287216712.yaml similarity index 100% rename from src/mtc/results/dummy/-1299660620.yaml rename to src/mtc/results/dummy/287216712.yaml diff --git a/src/mtc/results/dummy/-1287907953.yaml b/src/mtc/results/dummy/299277607.yaml similarity index 100% rename from src/mtc/results/dummy/-1287907953.yaml rename to src/mtc/results/dummy/299277607.yaml diff --git a/src/mtc/results/dummy/-1274604634.yaml b/src/mtc/results/dummy/312857611.yaml similarity index 100% rename from src/mtc/results/dummy/-1274604634.yaml rename to src/mtc/results/dummy/312857611.yaml diff --git a/src/mtc/results/dummy/-1346543685.yaml b/src/mtc/results/dummy/335131185.yaml similarity index 100% rename from src/mtc/results/dummy/-1346543685.yaml rename to src/mtc/results/dummy/335131185.yaml diff --git a/src/mtc/results/dummy/-1334648679.yaml b/src/mtc/results/dummy/347069224.yaml similarity index 100% rename from src/mtc/results/dummy/-1334648679.yaml rename to src/mtc/results/dummy/347069224.yaml diff --git a/src/mtc/results/dummy/-1321395857.yaml b/src/mtc/results/dummy/360590070.yaml similarity index 100% rename from src/mtc/results/dummy/-1321395857.yaml rename to src/mtc/results/dummy/360590070.yaml diff --git a/src/mtc/results/dummy/-1191626777.yaml b/src/mtc/results/dummy/397572609.yaml similarity index 100% rename from src/mtc/results/dummy/-1191626777.yaml rename to src/mtc/results/dummy/397572609.yaml diff --git a/src/mtc/results/dummy/-1179960295.yaml b/src/mtc/results/dummy/409649008.yaml similarity index 100% rename from src/mtc/results/dummy/-1179960295.yaml rename to src/mtc/results/dummy/409649008.yaml diff --git a/src/mtc/results/dummy/-1166700698.yaml b/src/mtc/results/dummy/423450698.yaml similarity index 100% rename from src/mtc/results/dummy/-1166700698.yaml rename to src/mtc/results/dummy/423450698.yaml diff --git a/src/mtc/results/dummy/-1238289141.yaml b/src/mtc/results/dummy/445682784.yaml similarity index 100% rename from src/mtc/results/dummy/-1238289141.yaml rename to src/mtc/results/dummy/445682784.yaml diff --git a/src/mtc/results/dummy/-1226601008.yaml b/src/mtc/results/dummy/461533561.yaml similarity index 100% rename from src/mtc/results/dummy/-1226601008.yaml rename to src/mtc/results/dummy/461533561.yaml diff --git a/src/mtc/results/dummy/-1213390021.yaml b/src/mtc/results/dummy/477108873.yaml similarity index 100% rename from src/mtc/results/dummy/-1213390021.yaml rename to src/mtc/results/dummy/477108873.yaml diff --git a/src/mtc/results/dummy/-1083227167.yaml b/src/mtc/results/dummy/518554172.yaml similarity index 100% rename from src/mtc/results/dummy/-1083227167.yaml rename to src/mtc/results/dummy/518554172.yaml diff --git a/src/mtc/results/dummy/-1071524157.yaml b/src/mtc/results/dummy/530725211.yaml similarity index 100% rename from src/mtc/results/dummy/-1071524157.yaml rename to src/mtc/results/dummy/530725211.yaml diff --git a/src/mtc/results/dummy/-1058266167.yaml b/src/mtc/results/dummy/544173349.yaml similarity index 100% rename from src/mtc/results/dummy/-1058266167.yaml rename to src/mtc/results/dummy/544173349.yaml diff --git a/src/mtc/results/dummy/-1130182242.yaml b/src/mtc/results/dummy/571021364.yaml similarity index 100% rename from src/mtc/results/dummy/-1130182242.yaml rename to src/mtc/results/dummy/571021364.yaml diff --git a/src/mtc/results/dummy/-1118397307.yaml b/src/mtc/results/dummy/582894029.yaml similarity index 100% rename from src/mtc/results/dummy/-1118397307.yaml rename to src/mtc/results/dummy/582894029.yaml diff --git a/src/mtc/results/dummy/-1105096712.yaml b/src/mtc/results/dummy/597217384.yaml similarity index 100% rename from src/mtc/results/dummy/-1105096712.yaml rename to src/mtc/results/dummy/597217384.yaml diff --git a/src/mtc/results/dummy/-1280463185.yaml b/src/mtc/results/dummy/759067436.yaml similarity index 100% rename from src/mtc/results/dummy/-1280463185.yaml rename to src/mtc/results/dummy/759067436.yaml diff --git a/src/mtc/results/dummy/-1269289091.yaml b/src/mtc/results/dummy/770754525.yaml similarity index 100% rename from src/mtc/results/dummy/-1269289091.yaml rename to src/mtc/results/dummy/770754525.yaml diff --git a/src/mtc/results/dummy/-1256829190.yaml b/src/mtc/results/dummy/784016529.yaml similarity index 100% rename from src/mtc/results/dummy/-1256829190.yaml rename to src/mtc/results/dummy/784016529.yaml diff --git a/src/mtc/results/dummy/-1325476019.yaml b/src/mtc/results/dummy/807557424.yaml similarity index 100% rename from src/mtc/results/dummy/-1325476019.yaml rename to src/mtc/results/dummy/807557424.yaml diff --git a/src/mtc/results/dummy/-1313991264.yaml b/src/mtc/results/dummy/827720657.yaml similarity index 100% rename from src/mtc/results/dummy/-1313991264.yaml rename to src/mtc/results/dummy/827720657.yaml diff --git a/src/mtc/results/dummy/-1301462009.yaml b/src/mtc/results/dummy/840878174.yaml similarity index 100% rename from src/mtc/results/dummy/-1301462009.yaml rename to src/mtc/results/dummy/840878174.yaml diff --git a/src/mtc/results/dummy/-1164235259.yaml b/src/mtc/results/dummy/886296696.yaml similarity index 100% rename from src/mtc/results/dummy/-1164235259.yaml rename to src/mtc/results/dummy/886296696.yaml diff --git a/src/mtc/results/dummy/-1213000891.yaml b/src/mtc/results/dummy/931879073.yaml similarity index 100% rename from src/mtc/results/dummy/-1213000891.yaml rename to src/mtc/results/dummy/931879073.yaml diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp index 0730159..a530e8a 100644 --- a/src/mtc/src/impl/mediator.cpp +++ b/src/mtc/src/impl/mediator.cpp @@ -301,7 +301,7 @@ void Mediator::write_file(Robot* A, Robot* B){ float pos_x = A->tf().getOrigin().getX(); float pos_y = A->tf().getOrigin().getY(); - float pos_z = A->tf().getOrigin().getZ() *2 - size_z/2; + float pos_z = A->tf().getOrigin().getZ() *2 ; float rot_x = A->tf().getRotation().getX(); float rot_y = A->tf().getRotation().getY(); float rot_z = A->tf().getRotation().getZ(); @@ -332,7 +332,7 @@ void Mediator::write_file(Robot* A, Robot* B){ pos_x = B->tf().getOrigin().getX(); pos_y = B->tf().getOrigin().getY(); - pos_z = B->tf().getOrigin().getZ() *2 - size_z/2; + pos_z = B->tf().getOrigin().getZ() *2 ; rot_x = B->tf().getRotation().getX(); rot_y = B->tf().getRotation().getY(); rot_z = B->tf().getRotation().getZ(); @@ -364,7 +364,7 @@ void Mediator::write_file(Robot* A, Robot* B){ float width = w->size().getY(); float height = w->size().getZ(); - ss << "{ 'id': '" << 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"; + ss << "{ 'id': '" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " } , '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"; } @@ -383,7 +383,7 @@ void Mediator::write_file(Robot* A, Robot* B){ float width = w->size().getY(); float height = w->size().getZ(); - ss << "{ 'id': '" << 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"; + ss << "{ 'id': '" << w->name() << "', 'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " },'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(); float arm_x = tf_arm.getOrigin().getX(); diff --git a/src/mtc/src/mtc2taskspace.cpp b/src/mtc/src/mtc2taskspace.cpp index db30d16..312acab 100644 --- a/src/mtc/src/mtc2taskspace.cpp +++ b/src/mtc/src/mtc2taskspace.cpp @@ -6,7 +6,9 @@ int main(int argc, char **argv){ ros::init(argc, argv, "mtc2taskspace"); ros::NodeHandle n; - std::string path; + std::string filename; + n.getParam("/resource_name", filename); + XmlRpc::XmlRpcValue resources; n.getParam("/objects", resources); @@ -60,34 +62,25 @@ int main(int argc, char **argv){ // lets make some result data ROS_INFO("writing file..."); - std::ofstream o(ros::package::getPath("mtc") + "/descriptions/dummy2.yaml"); + std::ofstream o(ros::package::getPath("mtc") + "/descriptions/" + filename); YAML::Node node; ROS_INFO("writing items for robot1..."); for(tf2::Vector3& vec : robot_1){ - node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ())); + node["task"]["groups"]["panda_arm1"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ())); } ROS_INFO("writing items for robot2..."); for(tf2::Vector3& vec : robot_2){ - node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ())); + node["task"]["groups"]["panda_arm2"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ())); } ROS_INFO("writing intersection..."); for(tf2::Vector3& vec : robot_middle){ - node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));; - node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));; + node["task"]["groups"]["panda_arm1"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));; + node["task"]["groups"]["panda_arm2"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));; } - ROS_INFO("writing intersection..."); - - for(tf2::Vector3& vec : robot_middle){ - node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));; - node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));; - } - - node.SetStyle(YAML::EmitterStyle::Flow); - o << node; return 0; } \ No newline at end of file -- GitLab