From dafccf8c34ba724701541b14eec5dd01c6e66988 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Wed, 19 Apr 2023 20:42:52 +0200
Subject: [PATCH] ...
---
README.md | 35 +++-
include/mediator/base_calculation_mediator.h | 41 +++++
.../-2045918175.yaml} | 9 +-
.../configs/-2045918175_panda_arm1.yaml} | 0
.../configs/-2045918175_panda_arm2.yaml} | 0
.../execute-2045918175.launch} | 4 +-
results/dummy/-2045918175/jobs/dummy.yaml | 56 ++++++
.../launch/panda_arm1_-2045918175.launch} | 4 +-
.../launch/panda_arm2_-2045918175.launch} | 4 +-
.../-2047444178.yaml} | 0
.../configs/-2047444178_panda_arm1.yaml} | 0
.../configs/-2047444178_panda_arm2.yaml} | 0
.../execute-2047444178.launch} | 4 +-
.../launch/panda_arm1_-2047444178.launch} | 4 +-
.../launch/panda_arm2_-2047444178.launch} | 4 +-
.../-2048784050.yaml} | 0
.../configs/-2048784050_panda_arm1.yaml} | 0
.../configs/-2048784050_panda_arm2.yaml} | 0
.../execute-2048784050.launch} | 4 +-
.../launch/panda_arm1_-2048784050.launch} | 4 +-
.../launch/panda_arm2_-2048784050.launch} | 4 +-
.../-2050138938.yaml} | 0
.../configs/-2050138938_panda_arm1.yaml} | 0
.../configs/-2050138938_panda_arm2.yaml} | 0
.../execute-2050138938.launch} | 4 +-
.../launch/panda_arm1_-2050138938.launch} | 4 +-
.../launch/panda_arm2_-2050138938.launch} | 4 +-
.../-2051644478.yaml} | 0
.../configs/-2051644478_panda_arm1.yaml} | 0
.../configs/-2051644478_panda_arm2.yaml} | 0
.../-2051644478/execute-2051644478.launch | 20 +++
.../launch/panda_arm1_-2051644478.launch} | 4 +-
.../launch/panda_arm2_-2051644478.launch} | 4 +-
.../-2053542187.yaml} | 0
.../configs/-2053542187_panda_arm1.yaml} | 0
.../configs/-2053542187_panda_arm2.yaml} | 0
.../-2053542187/execute-2053542187.launch | 20 +++
.../launch/panda_arm1_-2053542187.launch} | 4 +-
.../launch/panda_arm2_-2053542187.launch} | 4 +-
.../-2055046679.yaml} | 0
.../configs/-2055046679_panda_arm1.yaml} | 0
.../configs/-2055046679_panda_arm2.yaml} | 0
.../-2055046679/execute-2055046679.launch | 20 +++
.../launch/panda_arm1_-2055046679.launch} | 4 +-
.../launch/panda_arm2_-2055046679.launch} | 4 +-
.../-2056643993.yaml} | 0
.../configs/-2056643993_panda_arm1.yaml} | 0
.../configs/-2056643993_panda_arm2.yaml} | 0
.../-2056643993/execute-2056643993.launch | 20 +++
.../launch/panda_arm1_-2056643993.launch} | 4 +-
.../launch/panda_arm2_-2056643993.launch} | 4 +-
.../dummy/440481311/execute440481311.launch | 20 ---
.../dummy/442867108/execute442867108.launch | 20 ---
.../dummy/444768468/execute444768468.launch | 20 ---
.../dummy/446956887/execute446956887.launch | 20 ---
src/mediator/base_calculation_mediator.cpp | 32 +---
test/mediator/test_base.cpp | 160 ++++++++----------
test/reader/test_bt.cpp | 74 +++-----
58 files changed, 352 insertions(+), 295 deletions(-)
rename results/dummy/{446956887/446956887.yaml => -2045918175/-2045918175.yaml} (82%)
rename results/dummy/{440481311/configs/440481311_panda_arm1.yaml => -2045918175/configs/-2045918175_panda_arm1.yaml} (100%)
rename results/dummy/{440481311/configs/440481311_panda_arm2.yaml => -2045918175/configs/-2045918175_panda_arm2.yaml} (100%)
rename results/dummy/{430537148/execute430537148.launch => -2045918175/execute-2045918175.launch} (88%)
create mode 100644 results/dummy/-2045918175/jobs/dummy.yaml
rename results/dummy/{440481311/launch/panda_arm1_440481311.launch => -2045918175/launch/panda_arm1_-2045918175.launch} (96%)
rename results/dummy/{440481311/launch/panda_arm2_440481311.launch => -2045918175/launch/panda_arm2_-2045918175.launch} (96%)
rename results/dummy/{440481311/440481311.yaml => -2047444178/-2047444178.yaml} (100%)
rename results/dummy/{442867108/configs/442867108_panda_arm1.yaml => -2047444178/configs/-2047444178_panda_arm1.yaml} (100%)
rename results/dummy/{442867108/configs/442867108_panda_arm2.yaml => -2047444178/configs/-2047444178_panda_arm2.yaml} (100%)
rename results/dummy/{435978895/execute435978895.launch => -2047444178/execute-2047444178.launch} (88%)
rename results/dummy/{442867108/launch/panda_arm1_442867108.launch => -2047444178/launch/panda_arm1_-2047444178.launch} (96%)
rename results/dummy/{442867108/launch/panda_arm2_442867108.launch => -2047444178/launch/panda_arm2_-2047444178.launch} (96%)
rename results/dummy/{442867108/442867108.yaml => -2048784050/-2048784050.yaml} (100%)
rename results/dummy/{444768468/configs/444768468_panda_arm1.yaml => -2048784050/configs/-2048784050_panda_arm1.yaml} (100%)
rename results/dummy/{444768468/configs/444768468_panda_arm2.yaml => -2048784050/configs/-2048784050_panda_arm2.yaml} (100%)
rename results/dummy/{438078262/execute438078262.launch => -2048784050/execute-2048784050.launch} (88%)
rename results/dummy/{444768468/launch/panda_arm1_444768468.launch => -2048784050/launch/panda_arm1_-2048784050.launch} (96%)
rename results/dummy/{444768468/launch/panda_arm2_444768468.launch => -2048784050/launch/panda_arm2_-2048784050.launch} (96%)
rename results/dummy/{444768468/444768468.yaml => -2050138938/-2050138938.yaml} (100%)
rename results/dummy/{446956887/configs/446956887_panda_arm1.yaml => -2050138938/configs/-2050138938_panda_arm1.yaml} (100%)
rename results/dummy/{446956887/configs/446956887_panda_arm2.yaml => -2050138938/configs/-2050138938_panda_arm2.yaml} (100%)
rename results/dummy/{433826865/execute433826865.launch => -2050138938/execute-2050138938.launch} (88%)
rename results/dummy/{446956887/launch/panda_arm1_446956887.launch => -2050138938/launch/panda_arm1_-2050138938.launch} (96%)
rename results/dummy/{446956887/launch/panda_arm2_446956887.launch => -2050138938/launch/panda_arm2_-2050138938.launch} (96%)
rename results/dummy/{430537148/430537148.yaml => -2051644478/-2051644478.yaml} (100%)
rename results/dummy/{430537148/configs/430537148_panda_arm1.yaml => -2051644478/configs/-2051644478_panda_arm1.yaml} (100%)
rename results/dummy/{430537148/configs/430537148_panda_arm2.yaml => -2051644478/configs/-2051644478_panda_arm2.yaml} (100%)
create mode 100644 results/dummy/-2051644478/execute-2051644478.launch
rename results/dummy/{433826865/launch/panda_arm1_433826865.launch => -2051644478/launch/panda_arm1_-2051644478.launch} (96%)
rename results/dummy/{435978895/launch/panda_arm2_435978895.launch => -2051644478/launch/panda_arm2_-2051644478.launch} (96%)
rename results/dummy/{433826865/433826865.yaml => -2053542187/-2053542187.yaml} (100%)
rename results/dummy/{433826865/configs/433826865_panda_arm1.yaml => -2053542187/configs/-2053542187_panda_arm1.yaml} (100%)
rename results/dummy/{433826865/configs/433826865_panda_arm2.yaml => -2053542187/configs/-2053542187_panda_arm2.yaml} (100%)
create mode 100644 results/dummy/-2053542187/execute-2053542187.launch
rename results/dummy/{430537148/launch/panda_arm1_430537148.launch => -2053542187/launch/panda_arm1_-2053542187.launch} (96%)
rename results/dummy/{438078262/launch/panda_arm2_438078262.launch => -2053542187/launch/panda_arm2_-2053542187.launch} (96%)
rename results/dummy/{435978895/435978895.yaml => -2055046679/-2055046679.yaml} (100%)
rename results/dummy/{435978895/configs/435978895_panda_arm1.yaml => -2055046679/configs/-2055046679_panda_arm1.yaml} (100%)
rename results/dummy/{435978895/configs/435978895_panda_arm2.yaml => -2055046679/configs/-2055046679_panda_arm2.yaml} (100%)
create mode 100644 results/dummy/-2055046679/execute-2055046679.launch
rename results/dummy/{435978895/launch/panda_arm1_435978895.launch => -2055046679/launch/panda_arm1_-2055046679.launch} (96%)
rename results/dummy/{430537148/launch/panda_arm2_430537148.launch => -2055046679/launch/panda_arm2_-2055046679.launch} (96%)
rename results/dummy/{438078262/438078262.yaml => -2056643993/-2056643993.yaml} (100%)
rename results/dummy/{438078262/configs/438078262_panda_arm1.yaml => -2056643993/configs/-2056643993_panda_arm1.yaml} (100%)
rename results/dummy/{438078262/configs/438078262_panda_arm2.yaml => -2056643993/configs/-2056643993_panda_arm2.yaml} (100%)
create mode 100644 results/dummy/-2056643993/execute-2056643993.launch
rename results/dummy/{438078262/launch/panda_arm1_438078262.launch => -2056643993/launch/panda_arm1_-2056643993.launch} (96%)
rename results/dummy/{433826865/launch/panda_arm2_433826865.launch => -2056643993/launch/panda_arm2_-2056643993.launch} (96%)
delete mode 100644 results/dummy/440481311/execute440481311.launch
delete mode 100644 results/dummy/442867108/execute442867108.launch
delete mode 100644 results/dummy/444768468/execute444768468.launch
delete mode 100644 results/dummy/446956887/execute446956887.launch
diff --git a/README.md b/README.md
index f5724fe..ac682ec 100644
--- a/README.md
+++ b/README.md
@@ -2,8 +2,40 @@
multi_cell_builder is an approach that combines both the work of gb-auerswald and the earlier work of accessibility into a package that allows dynamic changes to the startup configuration and parallel execution.
Submodule of ['dynamc_modular_workcells'](git@git-st.inf.tu-dresden.de:Matteo_Anedda/dynamic_modular_workcells.git "GitLab")
+## workflow
+### Positioning
+The .launch file of the "Positioning" part should contain a reachability map (/maps), the protobuf (/resources) and a task description (/descriptions), based on which all robot positions are calculated and stored in the /results folder. Each result is named after its timestamp, including additional .launch files and configuration files for later use.
+
+```bash
+roslaunch multi_cell_builder base_calculation_approach.launch
+```
+### Startconfiguration
+The .launch files including the configurations are stored in the specific result folder, one for each robot that overwrites the result scene model. All overwritten box positions were marked with a comment (#modified robot->name()).
+
+### Execution
+The result folder also contains an execute_....launch file associated with the empty /jobs folder in this hierarchy. To execute a job, create the dummy.launch file in this /jobs folder following this scheme.
+
+```bash
+{ 'tasks':
+ {'groups' : [
+ { 'name': 'panda_arm1', 'jobs':[[
+ { 'pos': { 'x': ,'y': ,'z': }, 'orientation': { 'w': } },
+ { 'pos': { 'x': ,'y': ,'z': }, 'orientation': { 'w': } }
+ ]]},
+ { 'name': 'panda_arm2', 'jobs':[[
+ { 'pos': { 'x': ,'y': ,'z': }, 'orientation': { 'w': } },
+ { 'pos': { 'x': ,'y': ,'z': }, 'orientation': { 'w': } }
+ ]]}
+ ]
+ }
+}
+```
+
+Make sure that the first position in a job matches a box position in the timestamp.yaml file of that folder. A simple example would be the /jobs/dummy.yaml file, which is not associated with any result.
+
+
## Current state
-State: in progress. Implementing or omitting ideas because some modules do not work as expected took a lot of time. The actual results can be loaded and run. I tested some myself, with surprisingly good results. The following list shows open and closed ToDos.<br/>
+The following list shows open and closed ToDos.<br/>
:ballot_box_with_check: present a faster work-space execution <br/>
:ballot_box_with_check: generate more solutions for 1 protobuf reference file <br/>
@@ -11,6 +43,7 @@ State: in progress. Implementing or omitting ideas because some modules do not w
:ballot_box_with_check: elaborate the moveit task constructor <br/>
:ballot_box_with_check: present more general task files <br/>
:black_square_button: include cosmetic values <br/>
+:black_square_button: resolve endless-loop bug <br/>
:ballot_box_with_check: passing yaml in xacro (franca_description) <br/>
:ballot_box_with_check: parallel task execution in container <br/>
:ballot_box_with_check: fix acm <br/>
diff --git a/include/mediator/base_calculation_mediator.h b/include/mediator/base_calculation_mediator.h
index 05e6d88..1bb4206 100644
--- a/include/mediator/base_calculation_mediator.h
+++ b/include/mediator/base_calculation_mediator.h
@@ -149,6 +149,47 @@ static bool hasRobot(const protocol& proto, const std::string& robotName) {
return false;
}
+static std::vector<std::vector<std::string>> compareMapEntries(std::map<const std::string, std::vector<object_data>>& inputMap) {
+ std::vector<std::vector<std::string>> outputVec;
+
+ std::map<std::string, bool> matchedMap; // Map to keep track of matched keys
+
+ // Loop through each map entry
+ for (const auto& entry1 : inputMap) {
+ // Skip if the key has already been matched
+ if (matchedMap[entry1.first]) continue;
+
+ std::vector<std::string> keysVec;
+ keysVec.push_back(entry1.first); // Add the current key to the vector
+ matchedMap[entry1.first] = true; // Mark the key as matched
+
+ // Loop through remaining map entries
+ for (const auto& entry2 : inputMap) {
+ // Skip comparing the same map entry or if the key has already been matched
+ if (entry1.first == entry2.first || matchedMap[entry2.first]) continue;
+
+ bool matched = false;
+ // Loop through each transform in entry1's value
+ for (const auto& tf1 : entry1.second) {
+ // Loop through each transform in entry2's value
+ for (const auto& tf2 : entry2.second) {
+ // Compare the transform values
+ if (tf1.pose_ == tf2.pose_) {
+ matched = true;
+ break;
+ }
+ }
+ if (matched) break;
+ }
+ if (matched) {
+ keysVec.push_back(entry2.first); // Add the matching key to the vector
+ matchedMap[entry2.first] = true; // Mark the key as matched
+ }
+ }
+ outputVec.push_back(keysVec); // Add the vector of keys to the output vector
+ }
+ return outputVec;
+}
};
diff --git a/results/dummy/446956887/446956887.yaml b/results/dummy/-2045918175/-2045918175.yaml
similarity index 82%
rename from results/dummy/446956887/446956887.yaml
rename to results/dummy/-2045918175/-2045918175.yaml
index df60e80..c82b215 100644
--- a/results/dummy/446956887/446956887.yaml
+++ b/results/dummy/-2045918175/-2045918175.yaml
@@ -20,13 +20,8 @@
{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , '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.652502 , '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': 1.957498 , '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': 'blue1', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
-{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
-{ 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.1, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
-{ 'id': 'green1', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': -0.3, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
-{ 'id': 'green2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.91, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
-{ 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.6, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
-{ 'id': 'red2', 'type': 'BOX', 'pos': { 'x': 0.3, 'y': 1.41, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
+{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
+{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.11, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.000003, '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.304997, '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
diff --git a/results/dummy/440481311/configs/440481311_panda_arm1.yaml b/results/dummy/-2045918175/configs/-2045918175_panda_arm1.yaml
similarity index 100%
rename from results/dummy/440481311/configs/440481311_panda_arm1.yaml
rename to results/dummy/-2045918175/configs/-2045918175_panda_arm1.yaml
diff --git a/results/dummy/440481311/configs/440481311_panda_arm2.yaml b/results/dummy/-2045918175/configs/-2045918175_panda_arm2.yaml
similarity index 100%
rename from results/dummy/440481311/configs/440481311_panda_arm2.yaml
rename to results/dummy/-2045918175/configs/-2045918175_panda_arm2.yaml
diff --git a/results/dummy/430537148/execute430537148.launch b/results/dummy/-2045918175/execute-2045918175.launch
similarity index 88%
rename from results/dummy/430537148/execute430537148.launch
rename to results/dummy/-2045918175/execute-2045918175.launch
index 262c627..6f6f9b3 100644
--- a/results/dummy/430537148/execute430537148.launch
+++ b/results/dummy/-2045918175/execute-2045918175.launch
@@ -1,6 +1,6 @@
<launch>
-<arg name="result" default="dummy/430537148/430537148.yaml" />
-<arg name="jobs" default="dummy/430537148/jobs/dummy.yaml" />
+<arg name="result" default="dummy/-2045918175/-2045918175.yaml" />
+<arg name="jobs" default="dummy/-2045918175/jobs/dummy.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
diff --git a/results/dummy/-2045918175/jobs/dummy.yaml b/results/dummy/-2045918175/jobs/dummy.yaml
new file mode 100644
index 0000000..680c878
--- /dev/null
+++ b/results/dummy/-2045918175/jobs/dummy.yaml
@@ -0,0 +1,56 @@
+{ 'tasks':
+ {'groups' : [
+ { 'name': 'panda_arm1', 'jobs':[[
+ { 'pos': { 'x': -0.300000 ,'y': -0.700000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.300000 ,'y': -0.600000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.200000 ,'y': -0.700000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.200000 ,'y': -0.600000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': -0.700000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': -0.600000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': -0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': -0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': -0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': -0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': -0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': -0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': -0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': -0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': -0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': 0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': 0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': 0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': 0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': 0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': 0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': 0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': 0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': 0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ]]},
+ { 'name': 'panda_arm2', 'jobs':[[
+ { 'pos': { 'x': 0.100000 ,'y': 1.110000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': 1.210000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': 1.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': 1.110000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': 1.210000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': 1.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': 1.110000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': 1.210000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': 1.410000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': 1.510000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': 1.610000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': 1.410000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': 1.510000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.200000 ,'y': 1.610000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': 1.410000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': 1.510000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.300000 ,'y': 1.610000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.300000 ,'y': 1.910000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.300000 ,'y': 2.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.200000 ,'y': 1.910000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': -0.200000 ,'y': 2.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': 1.910000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.100000 ,'y': 2.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ]]}
+ ]
+ }
+}
\ No newline at end of file
diff --git a/results/dummy/440481311/launch/panda_arm1_440481311.launch b/results/dummy/-2045918175/launch/panda_arm1_-2045918175.launch
similarity index 96%
rename from results/dummy/440481311/launch/panda_arm1_440481311.launch
rename to results/dummy/-2045918175/launch/panda_arm1_-2045918175.launch
index c447d0c..5c30634 100644
--- a/results/dummy/440481311/launch/panda_arm1_440481311.launch
+++ b/results/dummy/-2045918175/launch/panda_arm1_-2045918175.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm1" />
<arg name="referenceXYZ" default="-0.220002 -2.59133e-06 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.49943e-06"/>
-<arg name="result" default="dummy/440481311/440481311.yaml" />
+<arg name="result" default="dummy/-2045918175/-2045918175.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/440481311/configs/440481311_panda_arm1.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2045918175/configs/-2045918175_panda_arm1.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/440481311/launch/panda_arm2_440481311.launch b/results/dummy/-2045918175/launch/panda_arm2_-2045918175.launch
similarity index 96%
rename from results/dummy/440481311/launch/panda_arm2_440481311.launch
rename to results/dummy/-2045918175/launch/panda_arm2_-2045918175.launch
index 8bc0385..774bf3f 100644
--- a/results/dummy/440481311/launch/panda_arm2_440481311.launch
+++ b/results/dummy/-2045918175/launch/panda_arm2_-2045918175.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm2" />
<arg name="referenceXYZ" default="-0.220005 1.305 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.67427e-06"/>
-<arg name="result" default="dummy/440481311/440481311.yaml" />
+<arg name="result" default="dummy/-2045918175/-2045918175.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/440481311/configs/440481311_panda_arm2.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2045918175/configs/-2045918175_panda_arm2.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/440481311/440481311.yaml b/results/dummy/-2047444178/-2047444178.yaml
similarity index 100%
rename from results/dummy/440481311/440481311.yaml
rename to results/dummy/-2047444178/-2047444178.yaml
diff --git a/results/dummy/442867108/configs/442867108_panda_arm1.yaml b/results/dummy/-2047444178/configs/-2047444178_panda_arm1.yaml
similarity index 100%
rename from results/dummy/442867108/configs/442867108_panda_arm1.yaml
rename to results/dummy/-2047444178/configs/-2047444178_panda_arm1.yaml
diff --git a/results/dummy/442867108/configs/442867108_panda_arm2.yaml b/results/dummy/-2047444178/configs/-2047444178_panda_arm2.yaml
similarity index 100%
rename from results/dummy/442867108/configs/442867108_panda_arm2.yaml
rename to results/dummy/-2047444178/configs/-2047444178_panda_arm2.yaml
diff --git a/results/dummy/435978895/execute435978895.launch b/results/dummy/-2047444178/execute-2047444178.launch
similarity index 88%
rename from results/dummy/435978895/execute435978895.launch
rename to results/dummy/-2047444178/execute-2047444178.launch
index dbe7244..22ba806 100644
--- a/results/dummy/435978895/execute435978895.launch
+++ b/results/dummy/-2047444178/execute-2047444178.launch
@@ -1,6 +1,6 @@
<launch>
-<arg name="result" default="dummy/435978895/435978895.yaml" />
-<arg name="jobs" default="dummy/435978895/jobs/dummy.yaml" />
+<arg name="result" default="dummy/-2047444178/-2047444178.yaml" />
+<arg name="jobs" default="dummy/-2047444178/jobs/dummy.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
diff --git a/results/dummy/442867108/launch/panda_arm1_442867108.launch b/results/dummy/-2047444178/launch/panda_arm1_-2047444178.launch
similarity index 96%
rename from results/dummy/442867108/launch/panda_arm1_442867108.launch
rename to results/dummy/-2047444178/launch/panda_arm1_-2047444178.launch
index caee851..44a2808 100644
--- a/results/dummy/442867108/launch/panda_arm1_442867108.launch
+++ b/results/dummy/-2047444178/launch/panda_arm1_-2047444178.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm1" />
<arg name="referenceXYZ" default="-0.220002 -2.59133e-06 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.49943e-06"/>
-<arg name="result" default="dummy/442867108/442867108.yaml" />
+<arg name="result" default="dummy/-2047444178/-2047444178.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/442867108/configs/442867108_panda_arm1.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2047444178/configs/-2047444178_panda_arm1.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/442867108/launch/panda_arm2_442867108.launch b/results/dummy/-2047444178/launch/panda_arm2_-2047444178.launch
similarity index 96%
rename from results/dummy/442867108/launch/panda_arm2_442867108.launch
rename to results/dummy/-2047444178/launch/panda_arm2_-2047444178.launch
index 5419b56..7275ffb 100644
--- a/results/dummy/442867108/launch/panda_arm2_442867108.launch
+++ b/results/dummy/-2047444178/launch/panda_arm2_-2047444178.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm2" />
<arg name="referenceXYZ" default="-0.220005 1.305 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.67427e-06"/>
-<arg name="result" default="dummy/442867108/442867108.yaml" />
+<arg name="result" default="dummy/-2047444178/-2047444178.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/442867108/configs/442867108_panda_arm2.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2047444178/configs/-2047444178_panda_arm2.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/442867108/442867108.yaml b/results/dummy/-2048784050/-2048784050.yaml
similarity index 100%
rename from results/dummy/442867108/442867108.yaml
rename to results/dummy/-2048784050/-2048784050.yaml
diff --git a/results/dummy/444768468/configs/444768468_panda_arm1.yaml b/results/dummy/-2048784050/configs/-2048784050_panda_arm1.yaml
similarity index 100%
rename from results/dummy/444768468/configs/444768468_panda_arm1.yaml
rename to results/dummy/-2048784050/configs/-2048784050_panda_arm1.yaml
diff --git a/results/dummy/444768468/configs/444768468_panda_arm2.yaml b/results/dummy/-2048784050/configs/-2048784050_panda_arm2.yaml
similarity index 100%
rename from results/dummy/444768468/configs/444768468_panda_arm2.yaml
rename to results/dummy/-2048784050/configs/-2048784050_panda_arm2.yaml
diff --git a/results/dummy/438078262/execute438078262.launch b/results/dummy/-2048784050/execute-2048784050.launch
similarity index 88%
rename from results/dummy/438078262/execute438078262.launch
rename to results/dummy/-2048784050/execute-2048784050.launch
index 93f6fe4..a76e61c 100644
--- a/results/dummy/438078262/execute438078262.launch
+++ b/results/dummy/-2048784050/execute-2048784050.launch
@@ -1,6 +1,6 @@
<launch>
-<arg name="result" default="dummy/438078262/438078262.yaml" />
-<arg name="jobs" default="dummy/438078262/jobs/dummy.yaml" />
+<arg name="result" default="dummy/-2048784050/-2048784050.yaml" />
+<arg name="jobs" default="dummy/-2048784050/jobs/dummy.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
diff --git a/results/dummy/444768468/launch/panda_arm1_444768468.launch b/results/dummy/-2048784050/launch/panda_arm1_-2048784050.launch
similarity index 96%
rename from results/dummy/444768468/launch/panda_arm1_444768468.launch
rename to results/dummy/-2048784050/launch/panda_arm1_-2048784050.launch
index d43ece0..1d5f00a 100644
--- a/results/dummy/444768468/launch/panda_arm1_444768468.launch
+++ b/results/dummy/-2048784050/launch/panda_arm1_-2048784050.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm1" />
<arg name="referenceXYZ" default="-0.220002 -2.59133e-06 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.49943e-06"/>
-<arg name="result" default="dummy/444768468/444768468.yaml" />
+<arg name="result" default="dummy/-2048784050/-2048784050.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/444768468/configs/444768468_panda_arm1.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2048784050/configs/-2048784050_panda_arm1.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/444768468/launch/panda_arm2_444768468.launch b/results/dummy/-2048784050/launch/panda_arm2_-2048784050.launch
similarity index 96%
rename from results/dummy/444768468/launch/panda_arm2_444768468.launch
rename to results/dummy/-2048784050/launch/panda_arm2_-2048784050.launch
index 26a864c..9fe4d62 100644
--- a/results/dummy/444768468/launch/panda_arm2_444768468.launch
+++ b/results/dummy/-2048784050/launch/panda_arm2_-2048784050.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm2" />
<arg name="referenceXYZ" default="-0.220005 1.305 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.67427e-06"/>
-<arg name="result" default="dummy/444768468/444768468.yaml" />
+<arg name="result" default="dummy/-2048784050/-2048784050.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/444768468/configs/444768468_panda_arm2.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2048784050/configs/-2048784050_panda_arm2.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/444768468/444768468.yaml b/results/dummy/-2050138938/-2050138938.yaml
similarity index 100%
rename from results/dummy/444768468/444768468.yaml
rename to results/dummy/-2050138938/-2050138938.yaml
diff --git a/results/dummy/446956887/configs/446956887_panda_arm1.yaml b/results/dummy/-2050138938/configs/-2050138938_panda_arm1.yaml
similarity index 100%
rename from results/dummy/446956887/configs/446956887_panda_arm1.yaml
rename to results/dummy/-2050138938/configs/-2050138938_panda_arm1.yaml
diff --git a/results/dummy/446956887/configs/446956887_panda_arm2.yaml b/results/dummy/-2050138938/configs/-2050138938_panda_arm2.yaml
similarity index 100%
rename from results/dummy/446956887/configs/446956887_panda_arm2.yaml
rename to results/dummy/-2050138938/configs/-2050138938_panda_arm2.yaml
diff --git a/results/dummy/433826865/execute433826865.launch b/results/dummy/-2050138938/execute-2050138938.launch
similarity index 88%
rename from results/dummy/433826865/execute433826865.launch
rename to results/dummy/-2050138938/execute-2050138938.launch
index c3f5703..531269d 100644
--- a/results/dummy/433826865/execute433826865.launch
+++ b/results/dummy/-2050138938/execute-2050138938.launch
@@ -1,6 +1,6 @@
<launch>
-<arg name="result" default="dummy/433826865/433826865.yaml" />
-<arg name="jobs" default="dummy/433826865/jobs/dummy.yaml" />
+<arg name="result" default="dummy/-2050138938/-2050138938.yaml" />
+<arg name="jobs" default="dummy/-2050138938/jobs/dummy.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
diff --git a/results/dummy/446956887/launch/panda_arm1_446956887.launch b/results/dummy/-2050138938/launch/panda_arm1_-2050138938.launch
similarity index 96%
rename from results/dummy/446956887/launch/panda_arm1_446956887.launch
rename to results/dummy/-2050138938/launch/panda_arm1_-2050138938.launch
index 94cf6b7..5919908 100644
--- a/results/dummy/446956887/launch/panda_arm1_446956887.launch
+++ b/results/dummy/-2050138938/launch/panda_arm1_-2050138938.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm1" />
<arg name="referenceXYZ" default="-0.220002 -2.59133e-06 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.49943e-06"/>
-<arg name="result" default="dummy/446956887/446956887.yaml" />
+<arg name="result" default="dummy/-2050138938/-2050138938.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/446956887/configs/446956887_panda_arm1.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2050138938/configs/-2050138938_panda_arm1.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/446956887/launch/panda_arm2_446956887.launch b/results/dummy/-2050138938/launch/panda_arm2_-2050138938.launch
similarity index 96%
rename from results/dummy/446956887/launch/panda_arm2_446956887.launch
rename to results/dummy/-2050138938/launch/panda_arm2_-2050138938.launch
index 52f9aaf..df00b97 100644
--- a/results/dummy/446956887/launch/panda_arm2_446956887.launch
+++ b/results/dummy/-2050138938/launch/panda_arm2_-2050138938.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm2" />
<arg name="referenceXYZ" default="-0.220005 1.305 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.67427e-06"/>
-<arg name="result" default="dummy/446956887/446956887.yaml" />
+<arg name="result" default="dummy/-2050138938/-2050138938.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/446956887/configs/446956887_panda_arm2.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2050138938/configs/-2050138938_panda_arm2.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/430537148/430537148.yaml b/results/dummy/-2051644478/-2051644478.yaml
similarity index 100%
rename from results/dummy/430537148/430537148.yaml
rename to results/dummy/-2051644478/-2051644478.yaml
diff --git a/results/dummy/430537148/configs/430537148_panda_arm1.yaml b/results/dummy/-2051644478/configs/-2051644478_panda_arm1.yaml
similarity index 100%
rename from results/dummy/430537148/configs/430537148_panda_arm1.yaml
rename to results/dummy/-2051644478/configs/-2051644478_panda_arm1.yaml
diff --git a/results/dummy/430537148/configs/430537148_panda_arm2.yaml b/results/dummy/-2051644478/configs/-2051644478_panda_arm2.yaml
similarity index 100%
rename from results/dummy/430537148/configs/430537148_panda_arm2.yaml
rename to results/dummy/-2051644478/configs/-2051644478_panda_arm2.yaml
diff --git a/results/dummy/-2051644478/execute-2051644478.launch b/results/dummy/-2051644478/execute-2051644478.launch
new file mode 100644
index 0000000..0d65602
--- /dev/null
+++ b/results/dummy/-2051644478/execute-2051644478.launch
@@ -0,0 +1,20 @@
+<launch>
+<arg name="result" default="dummy/-2051644478/-2051644478.yaml" />
+<arg name="jobs" default="dummy/-2051644478/jobs/dummy.yaml" />
+<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
+<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+<include file="$(find ceti_double)/launch/demo.launch">
+<arg name="use_rviz" value="false"/>
+<arg name="scene" value="$(arg result)" />
+</include>
+<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
+<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability"/>
+<include file="$(find ceti_double)/launch/moveit_rviz.launch">
+<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+</include>
+<node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
+</node>
+<node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
+</node>
+</launch>
diff --git a/results/dummy/433826865/launch/panda_arm1_433826865.launch b/results/dummy/-2051644478/launch/panda_arm1_-2051644478.launch
similarity index 96%
rename from results/dummy/433826865/launch/panda_arm1_433826865.launch
rename to results/dummy/-2051644478/launch/panda_arm1_-2051644478.launch
index 699d5bc..b3f558c 100644
--- a/results/dummy/433826865/launch/panda_arm1_433826865.launch
+++ b/results/dummy/-2051644478/launch/panda_arm1_-2051644478.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm1" />
<arg name="referenceXYZ" default="-0.220002 -0.100003 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.49943e-06"/>
-<arg name="result" default="dummy/433826865/433826865.yaml" />
+<arg name="result" default="dummy/-2051644478/-2051644478.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/433826865/configs/433826865_panda_arm1.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2051644478/configs/-2051644478_panda_arm1.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/435978895/launch/panda_arm2_435978895.launch b/results/dummy/-2051644478/launch/panda_arm2_-2051644478.launch
similarity index 96%
rename from results/dummy/435978895/launch/panda_arm2_435978895.launch
rename to results/dummy/-2051644478/launch/panda_arm2_-2051644478.launch
index 94be3b1..8123f89 100644
--- a/results/dummy/435978895/launch/panda_arm2_435978895.launch
+++ b/results/dummy/-2051644478/launch/panda_arm2_-2051644478.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm2" />
<arg name="referenceXYZ" default="-0.220005 1.205 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.67427e-06"/>
-<arg name="result" default="dummy/435978895/435978895.yaml" />
+<arg name="result" default="dummy/-2051644478/-2051644478.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/435978895/configs/435978895_panda_arm2.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2051644478/configs/-2051644478_panda_arm2.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/433826865/433826865.yaml b/results/dummy/-2053542187/-2053542187.yaml
similarity index 100%
rename from results/dummy/433826865/433826865.yaml
rename to results/dummy/-2053542187/-2053542187.yaml
diff --git a/results/dummy/433826865/configs/433826865_panda_arm1.yaml b/results/dummy/-2053542187/configs/-2053542187_panda_arm1.yaml
similarity index 100%
rename from results/dummy/433826865/configs/433826865_panda_arm1.yaml
rename to results/dummy/-2053542187/configs/-2053542187_panda_arm1.yaml
diff --git a/results/dummy/433826865/configs/433826865_panda_arm2.yaml b/results/dummy/-2053542187/configs/-2053542187_panda_arm2.yaml
similarity index 100%
rename from results/dummy/433826865/configs/433826865_panda_arm2.yaml
rename to results/dummy/-2053542187/configs/-2053542187_panda_arm2.yaml
diff --git a/results/dummy/-2053542187/execute-2053542187.launch b/results/dummy/-2053542187/execute-2053542187.launch
new file mode 100644
index 0000000..b029ced
--- /dev/null
+++ b/results/dummy/-2053542187/execute-2053542187.launch
@@ -0,0 +1,20 @@
+<launch>
+<arg name="result" default="dummy/-2053542187/-2053542187.yaml" />
+<arg name="jobs" default="dummy/-2053542187/jobs/dummy.yaml" />
+<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
+<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+<include file="$(find ceti_double)/launch/demo.launch">
+<arg name="use_rviz" value="false"/>
+<arg name="scene" value="$(arg result)" />
+</include>
+<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
+<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability"/>
+<include file="$(find ceti_double)/launch/moveit_rviz.launch">
+<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+</include>
+<node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
+</node>
+<node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
+</node>
+</launch>
diff --git a/results/dummy/430537148/launch/panda_arm1_430537148.launch b/results/dummy/-2053542187/launch/panda_arm1_-2053542187.launch
similarity index 96%
rename from results/dummy/430537148/launch/panda_arm1_430537148.launch
rename to results/dummy/-2053542187/launch/panda_arm1_-2053542187.launch
index 31573df..6c5be68 100644
--- a/results/dummy/430537148/launch/panda_arm1_430537148.launch
+++ b/results/dummy/-2053542187/launch/panda_arm1_-2053542187.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm1" />
<arg name="referenceXYZ" default="-0.220002 -0.100003 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.49943e-06"/>
-<arg name="result" default="dummy/430537148/430537148.yaml" />
+<arg name="result" default="dummy/-2053542187/-2053542187.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/430537148/configs/430537148_panda_arm1.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2053542187/configs/-2053542187_panda_arm1.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/438078262/launch/panda_arm2_438078262.launch b/results/dummy/-2053542187/launch/panda_arm2_-2053542187.launch
similarity index 96%
rename from results/dummy/438078262/launch/panda_arm2_438078262.launch
rename to results/dummy/-2053542187/launch/panda_arm2_-2053542187.launch
index 2495496..9e8f7a0 100644
--- a/results/dummy/438078262/launch/panda_arm2_438078262.launch
+++ b/results/dummy/-2053542187/launch/panda_arm2_-2053542187.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm2" />
<arg name="referenceXYZ" default="-0.220005 1.205 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.67427e-06"/>
-<arg name="result" default="dummy/438078262/438078262.yaml" />
+<arg name="result" default="dummy/-2053542187/-2053542187.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/438078262/configs/438078262_panda_arm2.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2053542187/configs/-2053542187_panda_arm2.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/435978895/435978895.yaml b/results/dummy/-2055046679/-2055046679.yaml
similarity index 100%
rename from results/dummy/435978895/435978895.yaml
rename to results/dummy/-2055046679/-2055046679.yaml
diff --git a/results/dummy/435978895/configs/435978895_panda_arm1.yaml b/results/dummy/-2055046679/configs/-2055046679_panda_arm1.yaml
similarity index 100%
rename from results/dummy/435978895/configs/435978895_panda_arm1.yaml
rename to results/dummy/-2055046679/configs/-2055046679_panda_arm1.yaml
diff --git a/results/dummy/435978895/configs/435978895_panda_arm2.yaml b/results/dummy/-2055046679/configs/-2055046679_panda_arm2.yaml
similarity index 100%
rename from results/dummy/435978895/configs/435978895_panda_arm2.yaml
rename to results/dummy/-2055046679/configs/-2055046679_panda_arm2.yaml
diff --git a/results/dummy/-2055046679/execute-2055046679.launch b/results/dummy/-2055046679/execute-2055046679.launch
new file mode 100644
index 0000000..57b63a5
--- /dev/null
+++ b/results/dummy/-2055046679/execute-2055046679.launch
@@ -0,0 +1,20 @@
+<launch>
+<arg name="result" default="dummy/-2055046679/-2055046679.yaml" />
+<arg name="jobs" default="dummy/-2055046679/jobs/dummy.yaml" />
+<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
+<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+<include file="$(find ceti_double)/launch/demo.launch">
+<arg name="use_rviz" value="false"/>
+<arg name="scene" value="$(arg result)" />
+</include>
+<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
+<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability"/>
+<include file="$(find ceti_double)/launch/moveit_rviz.launch">
+<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+</include>
+<node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
+</node>
+<node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
+</node>
+</launch>
diff --git a/results/dummy/435978895/launch/panda_arm1_435978895.launch b/results/dummy/-2055046679/launch/panda_arm1_-2055046679.launch
similarity index 96%
rename from results/dummy/435978895/launch/panda_arm1_435978895.launch
rename to results/dummy/-2055046679/launch/panda_arm1_-2055046679.launch
index ac9b03e..803da56 100644
--- a/results/dummy/435978895/launch/panda_arm1_435978895.launch
+++ b/results/dummy/-2055046679/launch/panda_arm1_-2055046679.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm1" />
<arg name="referenceXYZ" default="-0.220002 -0.100003 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.49943e-06"/>
-<arg name="result" default="dummy/435978895/435978895.yaml" />
+<arg name="result" default="dummy/-2055046679/-2055046679.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/435978895/configs/435978895_panda_arm1.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2055046679/configs/-2055046679_panda_arm1.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/430537148/launch/panda_arm2_430537148.launch b/results/dummy/-2055046679/launch/panda_arm2_-2055046679.launch
similarity index 96%
rename from results/dummy/430537148/launch/panda_arm2_430537148.launch
rename to results/dummy/-2055046679/launch/panda_arm2_-2055046679.launch
index a306f8f..c4dedd1 100644
--- a/results/dummy/430537148/launch/panda_arm2_430537148.launch
+++ b/results/dummy/-2055046679/launch/panda_arm2_-2055046679.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm2" />
<arg name="referenceXYZ" default="-0.220005 1.205 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.67427e-06"/>
-<arg name="result" default="dummy/430537148/430537148.yaml" />
+<arg name="result" default="dummy/-2055046679/-2055046679.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/430537148/configs/430537148_panda_arm2.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2055046679/configs/-2055046679_panda_arm2.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/438078262/438078262.yaml b/results/dummy/-2056643993/-2056643993.yaml
similarity index 100%
rename from results/dummy/438078262/438078262.yaml
rename to results/dummy/-2056643993/-2056643993.yaml
diff --git a/results/dummy/438078262/configs/438078262_panda_arm1.yaml b/results/dummy/-2056643993/configs/-2056643993_panda_arm1.yaml
similarity index 100%
rename from results/dummy/438078262/configs/438078262_panda_arm1.yaml
rename to results/dummy/-2056643993/configs/-2056643993_panda_arm1.yaml
diff --git a/results/dummy/438078262/configs/438078262_panda_arm2.yaml b/results/dummy/-2056643993/configs/-2056643993_panda_arm2.yaml
similarity index 100%
rename from results/dummy/438078262/configs/438078262_panda_arm2.yaml
rename to results/dummy/-2056643993/configs/-2056643993_panda_arm2.yaml
diff --git a/results/dummy/-2056643993/execute-2056643993.launch b/results/dummy/-2056643993/execute-2056643993.launch
new file mode 100644
index 0000000..9d4e3c4
--- /dev/null
+++ b/results/dummy/-2056643993/execute-2056643993.launch
@@ -0,0 +1,20 @@
+<launch>
+<arg name="result" default="dummy/-2056643993/-2056643993.yaml" />
+<arg name="jobs" default="dummy/-2056643993/jobs/dummy.yaml" />
+<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
+<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+<include file="$(find ceti_double)/launch/demo.launch">
+<arg name="use_rviz" value="false"/>
+<arg name="scene" value="$(arg result)" />
+</include>
+<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
+<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability"/>
+<include file="$(find ceti_double)/launch/moveit_rviz.launch">
+<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+</include>
+<node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
+</node>
+<node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
+</node>
+</launch>
diff --git a/results/dummy/438078262/launch/panda_arm1_438078262.launch b/results/dummy/-2056643993/launch/panda_arm1_-2056643993.launch
similarity index 96%
rename from results/dummy/438078262/launch/panda_arm1_438078262.launch
rename to results/dummy/-2056643993/launch/panda_arm1_-2056643993.launch
index 483342a..39fc3f4 100644
--- a/results/dummy/438078262/launch/panda_arm1_438078262.launch
+++ b/results/dummy/-2056643993/launch/panda_arm1_-2056643993.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm1" />
<arg name="referenceXYZ" default="-0.220002 -0.100003 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.49943e-06"/>
-<arg name="result" default="dummy/438078262/438078262.yaml" />
+<arg name="result" default="dummy/-2056643993/-2056643993.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/438078262/configs/438078262_panda_arm1.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2056643993/configs/-2056643993_panda_arm1.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/433826865/launch/panda_arm2_433826865.launch b/results/dummy/-2056643993/launch/panda_arm2_-2056643993.launch
similarity index 96%
rename from results/dummy/433826865/launch/panda_arm2_433826865.launch
rename to results/dummy/-2056643993/launch/panda_arm2_-2056643993.launch
index a6e4f70..1e46d1d 100644
--- a/results/dummy/433826865/launch/panda_arm2_433826865.launch
+++ b/results/dummy/-2056643993/launch/panda_arm2_-2056643993.launch
@@ -2,7 +2,7 @@
<arg name="referenceRobot" default="panda_arm2" />
<arg name="referenceXYZ" default="-0.220005 1.205 0.8875"/>
<arg name="referenceRPY" default="0 -0 2.67427e-06"/>
-<arg name="result" default="dummy/433826865/433826865.yaml" />
+<arg name="result" default="dummy/-2056643993/-2056643993.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam param="referenceRobot" subst_value="True"> $(arg referenceRobot)</rosparam>
<rosparam param="resultPath" subst_value="True"> $(arg result)</rosparam>
@@ -48,7 +48,7 @@
<param name="planning_group_name" value="panda_arm_hand" />
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/433826865/configs/433826865_panda_arm2.yaml"/>
+<rosparam command="load" file="$(find multi_cell_builder)/results/dummy/-2056643993/configs/-2056643993_panda_arm2.yaml"/>
</node>
<arg name="planner" default="ompl" />
<include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
diff --git a/results/dummy/440481311/execute440481311.launch b/results/dummy/440481311/execute440481311.launch
deleted file mode 100644
index f556234..0000000
--- a/results/dummy/440481311/execute440481311.launch
+++ /dev/null
@@ -1,20 +0,0 @@
-<launch>
-<arg name="result" default="dummy/440481311/440481311.yaml" />
-<arg name="jobs" default="dummy/440481311/jobs/dummy.yaml" />
-<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
-<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
-<include file="$(find ceti_double)/launch/demo.launch">
-<arg name="use_rviz" value="false"/>
-<arg name="scene" value="$(arg result)" />
-</include>
-<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
-<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability"/>
-<include file="$(find ceti_double)/launch/moveit_rviz.launch">
-<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
-</include>
-<node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
-</node>
-<node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
-</node>
-</launch>
diff --git a/results/dummy/442867108/execute442867108.launch b/results/dummy/442867108/execute442867108.launch
deleted file mode 100644
index ca53db9..0000000
--- a/results/dummy/442867108/execute442867108.launch
+++ /dev/null
@@ -1,20 +0,0 @@
-<launch>
-<arg name="result" default="dummy/442867108/442867108.yaml" />
-<arg name="jobs" default="dummy/442867108/jobs/dummy.yaml" />
-<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
-<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
-<include file="$(find ceti_double)/launch/demo.launch">
-<arg name="use_rviz" value="false"/>
-<arg name="scene" value="$(arg result)" />
-</include>
-<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
-<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability"/>
-<include file="$(find ceti_double)/launch/moveit_rviz.launch">
-<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
-</include>
-<node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
-</node>
-<node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
-</node>
-</launch>
diff --git a/results/dummy/444768468/execute444768468.launch b/results/dummy/444768468/execute444768468.launch
deleted file mode 100644
index 8c2965a..0000000
--- a/results/dummy/444768468/execute444768468.launch
+++ /dev/null
@@ -1,20 +0,0 @@
-<launch>
-<arg name="result" default="dummy/444768468/444768468.yaml" />
-<arg name="jobs" default="dummy/444768468/jobs/dummy.yaml" />
-<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
-<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
-<include file="$(find ceti_double)/launch/demo.launch">
-<arg name="use_rviz" value="false"/>
-<arg name="scene" value="$(arg result)" />
-</include>
-<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
-<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability"/>
-<include file="$(find ceti_double)/launch/moveit_rviz.launch">
-<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
-</include>
-<node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
-</node>
-<node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
-</node>
-</launch>
diff --git a/results/dummy/446956887/execute446956887.launch b/results/dummy/446956887/execute446956887.launch
deleted file mode 100644
index f5870f9..0000000
--- a/results/dummy/446956887/execute446956887.launch
+++ /dev/null
@@ -1,20 +0,0 @@
-<launch>
-<arg name="result" default="dummy/446956887/446956887.yaml" />
-<arg name="jobs" default="dummy/446956887/jobs/dummy.yaml" />
-<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
-<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg jobs)"/>
-<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
-<include file="$(find ceti_double)/launch/demo.launch">
-<arg name="use_rviz" value="false"/>
-<arg name="scene" value="$(arg result)" />
-</include>
-<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
-<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability"/>
-<include file="$(find ceti_double)/launch/moveit_rviz.launch">
-<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
-</include>
-<node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
-</node>
-<node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
-</node>
-</launch>
diff --git a/src/mediator/base_calculation_mediator.cpp b/src/mediator/base_calculation_mediator.cpp
index 69bd9de..be4d6eb 100644
--- a/src/mediator/base_calculation_mediator.cpp
+++ b/src/mediator/base_calculation_mediator.cpp
@@ -287,37 +287,7 @@ void BaseCalculationMediator::mediate(){
// estimate workcells
auto ts = task_space_reader_->dropOffData();
- std::vector<std::vector<std::string>> wcs;
-
- std::map<std::string, bool> matchedMap;
-
- for (const auto& entry1 : ts) {
- if (matchedMap[entry1.first]) continue;
-
- std::vector<std::string> keysVec;
- keysVec.push_back(entry1.first);
- matchedMap[entry1.first] = true;
-
- for (const auto& entry2 : ts) {
- if (entry1.first == entry2.first || matchedMap[entry2.first]) continue;
-
- bool matched = false;
- for (const auto& tf1 : entry1.second) {
- for (const auto& tf2 : entry2.second) {
- if (tf1.pose_ == tf2.pose_) {
- matched = true;
- break;
- }
- }
- if (matched) break;
- }
- if (matched) {
- keysVec.push_back(entry2.first);
- matchedMap[entry2.first] = true;
- }
- }
- wcs.push_back(keysVec);
- }
+ std::vector<std::vector<std::string>> wcs = compareMapEntries(ts);
/* wcs
[[panda_arm1, panda_arm2], [] ...]
diff --git a/test/mediator/test_base.cpp b/test/mediator/test_base.cpp
index b6ffaa0..447d4e1 100644
--- a/test/mediator/test_base.cpp
+++ b/test/mediator/test_base.cpp
@@ -13,12 +13,8 @@
#include <boost/circular_buffer.hpp>
-TEST(BaseTestSuit, workcellCompositionLogicTest){
- /* Workcell Logic
- A taskspace defines the shared workspace between manipulators.
- */
-
- std::map<std::string, std::vector<object_data>> inputMap = {
+TEST(BaseTestSuit, workcellCompositionLogicTest1){
+ std::map<const std::string, std::vector<object_data>> inputMap = {
{"key3", {
{"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(10,11, 12)), tf2::Vector3(0,0,0)},
{"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(13,14, 15)), tf2::Vector3(0,0,0)},
@@ -31,104 +27,92 @@ TEST(BaseTestSuit, workcellCompositionLogicTest){
}}
};
- std::vector<std::vector<std::string>> outputVec;
- std::map<std::string, bool> matchedMap;
-
- for (const auto& entry1 : inputMap) {
- if (matchedMap[entry1.first]) continue;
-
- std::vector<std::string> keysVec;
- keysVec.push_back(entry1.first);
- matchedMap[entry1.first] = true;
-
- for (const auto& entry2 : inputMap) {
- if (entry1.first == entry2.first || matchedMap[entry2.first]) continue;
-
- bool matched = false;
- for (const auto& tf1 : entry1.second) {
- for (const auto& tf2 : entry2.second) {
- if (tf1.pose_ == tf2.pose_) {
- matched = true;
- break;
- }
- }
- if (matched) break;
- }
- if (matched) {
- keysVec.push_back(entry2.first);
- matchedMap[entry2.first] = true;
- }
- }
- outputVec.push_back(keysVec);
- }
-
+ std::vector<std::vector<std::string>> outputVec = BaseCalculationMediator::compareMapEntries(inputMap);
ASSERT_EQ(outputVec.size(), 1);
}
-TEST(BaseTestSuit, workcellCalculationTest){
- /* Workcell Logic
- The workcell container structure looks like that: [[ws0], [ws1], ...], while ws[0-1]+ could contain 1 or more robot keys. A Key redirects to a map with base positions.
- */
-
- std::map<std::string, std::vector<object_data>> inputTaskspace = {
- {"panda_arm1", {
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,1, 1)), tf2::Vector3(0,0,0)},
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(2,2, 2)), tf2::Vector3(0,0,0)},
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(3,3, 3)), tf2::Vector3(0,0,0)},
+TEST(BaseTestSuit, workcellCompositionLogicTest2){
+ std::map<const std::string, std::vector<object_data>> inputMap = {
+ {"key1", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(9,99, 999)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,11, 111)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(2,22, 222)), tf2::Vector3(0,0,0)},
}},
- {"panda_arm2", {
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(4, 4, 4)), tf2::Vector3(0,0,0)},
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(5, 5, 5)), tf2::Vector3(0,0,0)},
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(6, 6, 6)), tf2::Vector3(0,0,0)}
- }},
- {"panda_arm3", {
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(7, 7, 7)), tf2::Vector3(0,0,0)},
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(5, 5, 5)), tf2::Vector3(0,0,0)},
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(6, 6, 6)), tf2::Vector3(0,0,0)}
- }},
- {"panda_arm4", {
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(8, 8, 8)), tf2::Vector3(0,0,0)},
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(9, 9, 9)), tf2::Vector3(0,0,0)},
- {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(10, 10, 10)), tf2::Vector3(0,0,0)}
+ {"key3", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(10,11, 12)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(13,14, 15)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(16,17, 18)), tf2::Vector3(0,0,0)},
+ }},
+ {"key2", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(19, 20, 21)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(22, 23, 24)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(16, 17, 18)), tf2::Vector3(0,0,0)}
}}
};
- std::vector<std::vector<std::string>> inputVector = {
- {"panda_arm1"},
- {"panda_arm2", "panda_arm3"},
- {"panda_arm4"}
- };
+ std::vector<std::vector<std::string>> outputVec = BaseCalculationMediator::compareMapEntries(inputMap);
+ ASSERT_EQ(outputVec.size(), 2);
+}
- std::map<std::string, std::vector<tf2::Vector3>> base_positions = {
- {"panda_arm1", {tf2::Vector3(0,0,0), tf2::Vector3(0,1,0), tf2::Vector3(0,2,0), tf2::Vector3(0,3,0), tf2::Vector3(0,4,0)}},
- {"panda_arm2", {tf2::Vector3(0,0,0), tf2::Vector3(0,1,0), tf2::Vector3(0,2,0), tf2::Vector3(0,3,0), tf2::Vector3(0,4,0)}},
- {"panda_arm3", {tf2::Vector3(0,0,0), tf2::Vector3(0,1,0), tf2::Vector3(0,2,0), tf2::Vector3(0,3,0), tf2::Vector3(0,4,0)}},
- {"panda_arm4", {tf2::Vector3(0,0,0), tf2::Vector3(0,1,0), tf2::Vector3(0,2,0), tf2::Vector3(0,3,0), tf2::Vector3(0,4,0)}}
+TEST(BaseTestSuit, workcellCompositionLogicTest3){
+ std::map<const std::string, std::vector<object_data>> inputMap = {
+ {"key1", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(9,99, 999)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,11, 111)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(2,22, 222)), tf2::Vector3(0,0,0)},
+ }},
+ {"key3", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(10,11, 12)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(13,14, 15)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(16,17, 18)), tf2::Vector3(0,0,0)},
+ }},
+ {"key3", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(10,11, 12)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,11, 111)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(2,22, 222)), tf2::Vector3(0,0,0)},
+ }},
+ {"key2", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(19, 20, 21)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(22, 23, 24)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(16, 17, 18)), tf2::Vector3(0,0,0)}
+ }}
};
+ std::vector<std::vector<std::string>> outputVec = BaseCalculationMediator::compareMapEntries(inputMap);
+ ASSERT_EQ(outputVec.size(), 2);
+}
- for (const auto& ws : inputVector){
- /*
- if ws contains more robots, a container structure should be build. For that, the order of placing robots is important.
- */
- for (const auto& robot : ws){
- // set any robot position
- // First Robot should run a 360 degree turn
- for (int i = 0; i < 360 ; i++){
- // set and check task space collision
- if (ws.size()>1){
- // call approximate(), which d
- }
- }
- }
+TEST(BaseTestSuit, workcellCompositionLogicTest4){
+ std::map<const std::string, std::vector<object_data>> inputMap = {
+ {"key1", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(10,11, 12)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,11, 111)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(2,22, 222)), tf2::Vector3(0,0,0)},
+ }},
+ {"key3", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(10,11, 12)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(13,14, 15)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(16,17, 18)), tf2::Vector3(0,0,0)},
+ }},
+ {"key4", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(10,11, 12)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,11, 111)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(2,22, 222)), tf2::Vector3(0,0,0)},
+ }},
+ {"key2", {
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(19, 20, 21)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(22, 23, 24)), tf2::Vector3(0,0,0)},
+ {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(2, 22, 222)), tf2::Vector3(0,0,0)}
+ }}
+ };
- }
+
+ std::vector<std::vector<std::string>> outputVec = BaseCalculationMediator::compareMapEntries(inputMap);
+ ASSERT_EQ(outputVec.size(), 1);
}
TEST(BaseTestSuit, workcellSolutionTest){
-
-
/* Workcell Logic
A Solution should contain all possible robots, if in a cluster or not, so its a concatenation.
*/
diff --git a/test/reader/test_bt.cpp b/test/reader/test_bt.cpp
index f38580d..6e522c0 100644
--- a/test/reader/test_bt.cpp
+++ b/test/reader/test_bt.cpp
@@ -22,55 +22,33 @@ TEST(BtTestSuit, basicParallelTest){
job2.jobs_ = {tf2::Transform(tf2::Quaternion(2,2,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(2,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,2,0))};
std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
- cb_jd.push_back({"panda_arm1", job1});
- cb_jd.push_back({"panda_arm2", job2});
-
-
- while(!cb_jd.empty()){
- for(auto& box : boxes){
- std::pair<std::string, job_data> temp = cb_jd.front();
- if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box.second) == 0) {
- for (int k = 1; k < temp.second.jobs_.size(); k++){
- box.second = temp.second.jobs_[k].getOrigin();
- }
- cb_jd.pop_front();
- } else {cb_jd.push_back(temp);}
- }
- }
- ASSERT_EQ(cb_jd.size(), 0); //All jobs
-}
-
-TEST(BtTestSuit, basicCooperativeTest){
- // BT::BehaviorTreeFactory factory;
- // factory.registerNodeType<Execution>("Execution");
- // factory.registerNodeType<PositionCondition>("PositionCondition");
- // factory.registerNodeType<Parallel_robot>("Parallel_robot");
- std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
-
- boost::circular_buffer<std::pair<std::string, job_data>> cb_jd;
- job_data job1, job2, job3;
- job1.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,0,0))};
- job2.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(2,2,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(2,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,2,0))};
- job3.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0))};
-
- std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
- cb_jd.push_back({"panda_arm1", job1});
- cb_jd.push_back({"panda_arm2", job2});
- cb_jd.push_back({"panda_arm1", job3});
-
-
- while(!cb_jd.empty()){
- for(auto& box : boxes){
- std::pair<std::string, job_data> temp = cb_jd.front();
- if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box.second) == 0) {
- for (int k = 1; k < temp.second.jobs_.size(); k++){
- box.second = temp.second.jobs_[k].getOrigin();
- }
- cb_jd.pop_front();
- } else {cb_jd.push_back(temp);}
- }
- }
+ bool jobs_remaining = true;
+ while (jobs_remaining && !cb_jd.empty() && !boxes.empty()) {
+ bool moved = false;
+ for (auto it = cb_jd.begin(); it != cb_jd.end();) {
+ auto& temp = *it;
+ bool ready_to_move = true;
+ for (auto& box : boxes) {
+ if (temp.second.jobs_.front().getOrigin() == box.second) {
+ for (int k = 1; k < temp.second.jobs_.size(); k++) {
+ box.second = temp.second.jobs_[k].getOrigin();
+ }
+ if (ready_to_move) {
+ moved = true;
+ it = cb_jd.erase(it);
+ break;
+ }
+ } else {
+ ready_to_move = false;
+ }
+ }
+ if (!ready_to_move) {
+ ++it;
+ }
+ }
+ jobs_remaining = moved;
+ }
ASSERT_EQ(cb_jd.size(), 0); //All jobs
}
--
GitLab