diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 35d97a11fafa59e6bc88b583aefdff3f1c5e2f54..3ad86a5ae91f2674325bfa42894e302dbcae4675 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 26851 +/home/matteo/reachability/devel/./cmake.lock 26877 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 diff --git a/src/mtc/README.md b/src/mtc/README.md index bb2ded4410674ea881b33a25b0b5d2e2d26c0458..8e6a611ff3169ff62298372dd55d68101cdbb806 100644 --- a/src/mtc/README.md +++ b/src/mtc/README.md @@ -6,18 +6,25 @@ MTC consists of 3 executables, with each computing .yaml files for the next subs > mtc2taskspace >> base_routine >> cell_routine ### mtc2taskspace -From a Ceti dump file representing an arbitrary scene with specific locations, located in the /resources directory. Mtc2taskspace computes a taskspace.yaml file and associates each object with a drop-of-location to a robot. The shared space between robots is expressed by identical location entries for each robot involved. -> resulting .yaml files in /descriptions +From a bare Ceti-resource file representing an arbitrary scene with specific locations, located in the /resources directory. Mtc2taskspace computes a new file and associates each object with a drop-of-location to a robot. The shared space between robots is expressed by identical location entries for each robot involved. <br/> +mtc/mtc/mtc2taskspace.cpp (/resources/{resource}) -> (/descriptions/{resource}) <br/> +roslaunch mtc mtc2taskspace.launch resource='dummy.yaml' + ### base_routine -Calculates the workspace from a /descriptions file (mtc2taskspace) and a precomputed reachability map (/map), combined with information from a dump file (/resources). Creates the directory results/${dump_file_name} with the resulting dump files as contents. -> Replace the resulting .yaml files in the results/${dump_file_name} directory if they already exist ->> Is strictly programmed, which means that the shared space is given by the first robot, which leads to no result if the first robot is not able to carry the shared space. Also resulting dump files can break the yaml schema, because each robot has an extra rpy element which is important for franca_description -> setting robot rotation... for placing, each 8th entry must be a table[0-9]+_table_top. Cosmetic data (table_wheel...) included as placeholder ->>> Solutions: switching robots, soon to be implemented. Cosmetic data by relative definitions. +Calculates the workspace from a descriptions file (/descriptions/{description}) and a precomputed reachability map (/map/{map}), combined with information of a resource file (/resources/{resource}). Creates the directory (results/{resource}/ros::time.yaml}) with the resulting files as contents. +> Replace already existing result folder if exists. +>> Resulting files can break the yaml schema, because each robot has an extra rpy element which is important for setting robot pose in franca_description. Also each 8th entry must be a table[0-9]+_table_top id. Cosmetic data (table_wheel...) included as placeholder + +mtc/src/base_routine.cpp (/map/{map}, /resources/{resource}, /descriptions/{descriptions}) -> (/results/{resource}/ros::time.yaml) <br/> +roslaunch mtc base_routine.launch resource='dummy.yaml' map='dummy.yaml' description='dummy.yaml' ### cell_routine -Designed to load a dump_file (+ with additional rpy component) and execute a task with moveit task constructor. A task description can also be the /descriptions file of mtc2taskspace, which is the case now. +Designed to load a resource file (+ with additional rpy component in the right order) and execute a task with moveit task constructor. A task description can also be the /descriptions/{description} in the example. > Christoph's parser could not be integrated because there is no way to have multiple robots compute a task. +mtc/src/cell_routine.cpp (/results/{result}, /descriptions/{task}) -> () <br/> +roslaunch mtc base_routine.launch result='/dummy/dummy.yaml' task='dummy.yaml' (is coop) +roslaunch mtc base_routine.launch result='/dummy/dummy.yaml' task='dummy2.yaml' (is parallel) ## Current state diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch index 91bcb4c106ce1feedc08ce9d7f515534f61cc030..5b3d3e516ee2a2aec2275655501f4a3c99e56f82 100644 --- a/src/mtc/launch/cell_routine.launch +++ b/src/mtc/launch/cell_routine.launch @@ -1,5 +1,5 @@ <launch> - <arg name="result" default="dummy/-211114391.yaml" /> + <arg name="result" default="dummy/-562289182.yaml" /> <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> --> <!-- this is to change--> diff --git a/src/mtc/results/dummy/-222544024.yaml b/src/mtc/results/dummy/-14976433.yaml similarity index 100% rename from src/mtc/results/dummy/-222544024.yaml rename to src/mtc/results/dummy/-14976433.yaml diff --git a/src/mtc/results/dummy/1295853078.yaml b/src/mtc/results/dummy/-1656836011.yaml similarity index 100% rename from src/mtc/results/dummy/1295853078.yaml rename to src/mtc/results/dummy/-1656836011.yaml diff --git a/src/mtc/results/dummy/1283302731.yaml b/src/mtc/results/dummy/-1671801501.yaml similarity index 100% rename from src/mtc/results/dummy/1283302731.yaml rename to src/mtc/results/dummy/-1671801501.yaml diff --git a/src/mtc/results/dummy/1272105605.yaml b/src/mtc/results/dummy/-1683824376.yaml similarity index 100% rename from src/mtc/results/dummy/1272105605.yaml rename to src/mtc/results/dummy/-1683824376.yaml diff --git a/src/mtc/results/dummy/1340801804.yaml b/src/mtc/results/dummy/-1706168228.yaml similarity index 100% rename from src/mtc/results/dummy/1340801804.yaml rename to src/mtc/results/dummy/-1706168228.yaml diff --git a/src/mtc/results/dummy/1328208922.yaml b/src/mtc/results/dummy/-1719483923.yaml similarity index 100% rename from src/mtc/results/dummy/1328208922.yaml rename to src/mtc/results/dummy/-1719483923.yaml diff --git a/src/mtc/results/dummy/1316801078.yaml b/src/mtc/results/dummy/-1731052042.yaml similarity index 100% rename from src/mtc/results/dummy/1316801078.yaml rename to src/mtc/results/dummy/-1731052042.yaml diff --git a/src/mtc/results/dummy/1183415122.yaml b/src/mtc/results/dummy/-1779240138.yaml similarity index 100% rename from src/mtc/results/dummy/1183415122.yaml rename to src/mtc/results/dummy/-1779240138.yaml diff --git a/src/mtc/results/dummy/1227695304.yaml b/src/mtc/results/dummy/-1825479086.yaml similarity index 100% rename from src/mtc/results/dummy/1227695304.yaml rename to src/mtc/results/dummy/-1825479086.yaml diff --git a/src/mtc/results/dummy/-211114391.yaml b/src/mtc/results/dummy/-2642749.yaml similarity index 100% rename from src/mtc/results/dummy/-211114391.yaml rename to src/mtc/results/dummy/-2642749.yaml diff --git a/src/mtc/results/dummy/-742232139.yaml b/src/mtc/results/dummy/-535885623.yaml similarity index 100% rename from src/mtc/results/dummy/-742232139.yaml rename to src/mtc/results/dummy/-535885623.yaml diff --git a/src/mtc/results/dummy/-755774193.yaml b/src/mtc/results/dummy/-549849544.yaml similarity index 100% rename from src/mtc/results/dummy/-755774193.yaml rename to src/mtc/results/dummy/-549849544.yaml diff --git a/src/mtc/results/dummy/-767674851.yaml b/src/mtc/results/dummy/-562289182.yaml similarity index 100% rename from src/mtc/results/dummy/-767674851.yaml rename to src/mtc/results/dummy/-562289182.yaml diff --git a/src/mtc/results/dummy/-789859288.yaml b/src/mtc/results/dummy/-586491524.yaml similarity index 100% rename from src/mtc/results/dummy/-789859288.yaml rename to src/mtc/results/dummy/-586491524.yaml diff --git a/src/mtc/results/dummy/-803422225.yaml b/src/mtc/results/dummy/-606671318.yaml similarity index 100% rename from src/mtc/results/dummy/-803422225.yaml rename to src/mtc/results/dummy/-606671318.yaml diff --git a/src/mtc/results/dummy/-815321487.yaml b/src/mtc/results/dummy/-620298450.yaml similarity index 100% rename from src/mtc/results/dummy/-815321487.yaml rename to src/mtc/results/dummy/-620298450.yaml diff --git a/src/mtc/results/dummy/-852264234.yaml b/src/mtc/results/dummy/-657407479.yaml similarity index 100% rename from src/mtc/results/dummy/-852264234.yaml rename to src/mtc/results/dummy/-657407479.yaml diff --git a/src/mtc/results/dummy/-865767525.yaml b/src/mtc/results/dummy/-671037754.yaml similarity index 100% rename from src/mtc/results/dummy/-865767525.yaml rename to src/mtc/results/dummy/-671037754.yaml diff --git a/src/mtc/results/dummy/-877673981.yaml b/src/mtc/results/dummy/-683068383.yaml similarity index 100% rename from src/mtc/results/dummy/-877673981.yaml rename to src/mtc/results/dummy/-683068383.yaml diff --git a/src/mtc/results/dummy/-899976593.yaml b/src/mtc/results/dummy/-706760442.yaml similarity index 100% rename from src/mtc/results/dummy/-899976593.yaml rename to src/mtc/results/dummy/-706760442.yaml diff --git a/src/mtc/results/dummy/-914020264.yaml b/src/mtc/results/dummy/-720385548.yaml similarity index 100% rename from src/mtc/results/dummy/-914020264.yaml rename to src/mtc/results/dummy/-720385548.yaml diff --git a/src/mtc/results/dummy/-925946624.yaml b/src/mtc/results/dummy/-732517591.yaml similarity index 100% rename from src/mtc/results/dummy/-925946624.yaml rename to src/mtc/results/dummy/-732517591.yaml diff --git a/src/mtc/results/dummy/-1010460506.yaml b/src/mtc/results/dummy/-774785188.yaml similarity index 100% rename from src/mtc/results/dummy/-1010460506.yaml rename to src/mtc/results/dummy/-774785188.yaml diff --git a/src/mtc/results/dummy/-1024001513.yaml b/src/mtc/results/dummy/-788555850.yaml similarity index 100% rename from src/mtc/results/dummy/-1024001513.yaml rename to src/mtc/results/dummy/-788555850.yaml diff --git a/src/mtc/results/dummy/-1036064907.yaml b/src/mtc/results/dummy/-811317163.yaml similarity index 100% rename from src/mtc/results/dummy/-1036064907.yaml rename to src/mtc/results/dummy/-811317163.yaml diff --git a/src/mtc/results/dummy/-962936586.yaml b/src/mtc/results/dummy/-834662166.yaml similarity index 100% rename from src/mtc/results/dummy/-962936586.yaml rename to src/mtc/results/dummy/-834662166.yaml diff --git a/src/mtc/results/dummy/-976487161.yaml b/src/mtc/results/dummy/-848791479.yaml similarity index 100% rename from src/mtc/results/dummy/-976487161.yaml rename to src/mtc/results/dummy/-848791479.yaml diff --git a/src/mtc/results/dummy/-988375807.yaml b/src/mtc/results/dummy/-861416902.yaml similarity index 100% rename from src/mtc/results/dummy/-988375807.yaml rename to src/mtc/results/dummy/-861416902.yaml diff --git a/src/mtc/results/dummy/-1003092792.yaml b/src/mtc/results/dummy/104202925.yaml similarity index 100% rename from src/mtc/results/dummy/-1003092792.yaml rename to src/mtc/results/dummy/104202925.yaml diff --git a/src/mtc/results/dummy/832741068.yaml b/src/mtc/results/dummy/1183733496.yaml similarity index 100% rename from src/mtc/results/dummy/832741068.yaml rename to src/mtc/results/dummy/1183733496.yaml diff --git a/src/mtc/results/dummy/-947074197.yaml b/src/mtc/results/dummy/123006295.yaml similarity index 100% rename from src/mtc/results/dummy/-947074197.yaml rename to src/mtc/results/dummy/123006295.yaml diff --git a/src/mtc/results/dummy/880346008.yaml b/src/mtc/results/dummy/1237434762.yaml similarity index 100% rename from src/mtc/results/dummy/880346008.yaml rename to src/mtc/results/dummy/1237434762.yaml diff --git a/src/mtc/results/dummy/144778443.yaml b/src/mtc/results/dummy/1353640318.yaml similarity index 100% rename from src/mtc/results/dummy/144778443.yaml rename to src/mtc/results/dummy/1353640318.yaml diff --git a/src/mtc/results/dummy/155931149.yaml b/src/mtc/results/dummy/1365341001.yaml similarity index 100% rename from src/mtc/results/dummy/155931149.yaml rename to src/mtc/results/dummy/1365341001.yaml diff --git a/src/mtc/results/dummy/-934521686.yaml b/src/mtc/results/dummy/137600073.yaml similarity index 100% rename from src/mtc/results/dummy/-934521686.yaml rename to src/mtc/results/dummy/137600073.yaml diff --git a/src/mtc/results/dummy/168492950.yaml b/src/mtc/results/dummy/1378781438.yaml similarity index 100% rename from src/mtc/results/dummy/168492950.yaml rename to src/mtc/results/dummy/1378781438.yaml diff --git a/src/mtc/results/dummy/189371734.yaml b/src/mtc/results/dummy/1401169642.yaml similarity index 100% rename from src/mtc/results/dummy/189371734.yaml rename to src/mtc/results/dummy/1401169642.yaml diff --git a/src/mtc/results/dummy/203146650.yaml b/src/mtc/results/dummy/1413511427.yaml similarity index 100% rename from src/mtc/results/dummy/203146650.yaml rename to src/mtc/results/dummy/1413511427.yaml diff --git a/src/mtc/results/dummy/215783882.yaml b/src/mtc/results/dummy/1427045874.yaml similarity index 100% rename from src/mtc/results/dummy/215783882.yaml rename to src/mtc/results/dummy/1427045874.yaml diff --git a/src/mtc/results/dummy/-958226134.yaml b/src/mtc/results/dummy/158824668.yaml similarity index 100% rename from src/mtc/results/dummy/-958226134.yaml rename to src/mtc/results/dummy/158824668.yaml diff --git a/src/mtc/results/dummy/-991922905.yaml b/src/mtc/results/dummy/170127937.yaml similarity index 100% rename from src/mtc/results/dummy/-991922905.yaml rename to src/mtc/results/dummy/170127937.yaml diff --git a/src/mtc/results/dummy/-979308652.yaml b/src/mtc/results/dummy/182858545.yaml similarity index 100% rename from src/mtc/results/dummy/-979308652.yaml rename to src/mtc/results/dummy/182858545.yaml diff --git a/src/mtc/results/dummy/702701396.yaml b/src/mtc/results/dummy/1961728642.yaml similarity index 100% rename from src/mtc/results/dummy/702701396.yaml rename to src/mtc/results/dummy/1961728642.yaml diff --git a/src/mtc/results/dummy/714102535.yaml b/src/mtc/results/dummy/1973258416.yaml similarity index 100% rename from src/mtc/results/dummy/714102535.yaml rename to src/mtc/results/dummy/1973258416.yaml diff --git a/src/mtc/results/dummy/726842647.yaml b/src/mtc/results/dummy/1986420314.yaml similarity index 100% rename from src/mtc/results/dummy/726842647.yaml rename to src/mtc/results/dummy/1986420314.yaml diff --git a/src/mtc/results/dummy/748047042.yaml b/src/mtc/results/dummy/2009012393.yaml similarity index 100% rename from src/mtc/results/dummy/748047042.yaml rename to src/mtc/results/dummy/2009012393.yaml diff --git a/src/mtc/results/dummy/759537023.yaml b/src/mtc/results/dummy/2020890621.yaml similarity index 100% rename from src/mtc/results/dummy/759537023.yaml rename to src/mtc/results/dummy/2020890621.yaml diff --git a/src/mtc/results/dummy/772496165.yaml b/src/mtc/results/dummy/2037862740.yaml similarity index 100% rename from src/mtc/results/dummy/772496165.yaml rename to src/mtc/results/dummy/2037862740.yaml diff --git a/src/mtc/results/dummy/-846087811.yaml b/src/mtc/results/dummy/235473380.yaml similarity index 100% rename from src/mtc/results/dummy/-846087811.yaml rename to src/mtc/results/dummy/235473380.yaml diff --git a/src/mtc/results/dummy/-890048247.yaml b/src/mtc/results/dummy/283969420.yaml similarity index 100% rename from src/mtc/results/dummy/-890048247.yaml rename to src/mtc/results/dummy/283969420.yaml diff --git a/src/mtc/results/dummy/40651904.yaml b/src/mtc/results/dummy/319613316.yaml similarity index 100% rename from src/mtc/results/dummy/40651904.yaml rename to src/mtc/results/dummy/319613316.yaml diff --git a/src/mtc/results/dummy/-270419468.yaml b/src/mtc/results/dummy/36088905.yaml similarity index 100% rename from src/mtc/results/dummy/-270419468.yaml rename to src/mtc/results/dummy/36088905.yaml diff --git a/src/mtc/results/dummy/88011482.yaml b/src/mtc/results/dummy/368603087.yaml similarity index 100% rename from src/mtc/results/dummy/88011482.yaml rename to src/mtc/results/dummy/368603087.yaml diff --git a/src/mtc/results/dummy/-652201118.yaml b/src/mtc/results/dummy/439026223.yaml similarity index 100% rename from src/mtc/results/dummy/-652201118.yaml rename to src/mtc/results/dummy/439026223.yaml diff --git a/src/mtc/results/dummy/-696648853.yaml b/src/mtc/results/dummy/484352725.yaml similarity index 100% rename from src/mtc/results/dummy/-696648853.yaml rename to src/mtc/results/dummy/484352725.yaml diff --git a/src/mtc/results/dummy/-258879761.yaml b/src/mtc/results/dummy/48584069.yaml similarity index 100% rename from src/mtc/results/dummy/-258879761.yaml rename to src/mtc/results/dummy/48584069.yaml diff --git a/src/mtc/results/dummy/254319817.yaml b/src/mtc/results/dummy/551994162.yaml similarity index 100% rename from src/mtc/results/dummy/254319817.yaml rename to src/mtc/results/dummy/551994162.yaml diff --git a/src/mtc/results/dummy/301530140.yaml b/src/mtc/results/dummy/606403650.yaml similarity index 100% rename from src/mtc/results/dummy/301530140.yaml rename to src/mtc/results/dummy/606403650.yaml diff --git a/src/mtc/results/dummy/434875766.yaml b/src/mtc/results/dummy/759458158.yaml similarity index 100% rename from src/mtc/results/dummy/434875766.yaml rename to src/mtc/results/dummy/759458158.yaml diff --git a/src/mtc/results/dummy/482378175.yaml b/src/mtc/results/dummy/810620021.yaml similarity index 100% rename from src/mtc/results/dummy/482378175.yaml rename to src/mtc/results/dummy/810620021.yaml diff --git a/src/mtc/results/dummy/560822576.yaml b/src/mtc/results/dummy/886121610.yaml similarity index 100% rename from src/mtc/results/dummy/560822576.yaml rename to src/mtc/results/dummy/886121610.yaml diff --git a/src/mtc/results/dummy/608106095.yaml b/src/mtc/results/dummy/939241980.yaml similarity index 100% rename from src/mtc/results/dummy/608106095.yaml rename to src/mtc/results/dummy/939241980.yaml diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp index b3c1ffcd5599c1c37734ccf9cdabca84cf386297..fadea82724903938b1feb8d1726f9cea826850ec 100644 --- a/src/mtc/src/impl/mediator.cpp +++ b/src/mtc/src/impl/mediator.cpp @@ -128,6 +128,15 @@ void Mediator::mediate(){ } calculate(ground_per_robot); + + //swap + Abstract_robot* ar = robots_[1]; + robots_[1] = robots_[0]; + robots_[0] = ar; + + calculate(ground_per_robot); + + } void Mediator::calculate(std::vector<tf2::Transform>& ground_per_robot){ diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index 2071a411b7239548b442ec985705d6d7f28ee78e..d5aeb048aec888f9e30208bdf3df1ca55a1dc9ce 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/src/mtc/src/impl/moveit_mediator.cpp @@ -74,7 +74,6 @@ void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){ for (std::size_t i = 0; i < result.size(); i++){ if (result[i]){ - ROS_INFO("%i", i); ceti->register_observers(wings_[robot][i]); } } @@ -93,8 +92,6 @@ void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>, void Moveit_mediator::setup_task(){ ROS_INFO("=> write Task Constructor Objects"); - ROS_INFO("=> "); - bool coop = false;