From 3718a9795df40b10de786949b34ecd99075a1434 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Tue, 15 Nov 2022 13:36:21 +0100
Subject: [PATCH] first parallel planning attempts
---
.../profiles/default/devel_collisions.txt | 2 +-
src/.vscode/c_cpp_properties.json | 76 +++++++------------
.../robots/ceti_double.urdf.xacro | 2 +-
src/mtc/descriptions/dummy2.yaml | 52 ++++++++++++-
src/mtc/include/impl/abstract_robot.h | 2 +
src/mtc/include/impl/moveit_mediator.h | 1 +
src/mtc/include/impl/moveit_robot.h | 1 +
src/mtc/launch/cell_routine.launch | 4 +-
.../{-1113960400.yaml => -1511245846.yaml} | 0
.../{-1130060311.yaml => -1533781679.yaml} | 0
.../{-1141587806.yaml => -1545058710.yaml} | 0
.../{-1164134400.yaml => -1569157975.yaml} | 0
.../{-1177867363.yaml => -1581632251.yaml} | 0
.../{-1190597504.yaml => -1592744663.yaml} | 0
.../{-1241254685.yaml => -1637096250.yaml} | 0
.../{-1287858113.yaml => -1682040034.yaml} | 0
.../{-2019620283.yaml => -1897283987.yaml} | 0
.../{-2070819246.yaml => -1946904381.yaml} | 0
.../{-1725746253.yaml => -2080265983.yaml} | 0
.../{-1739448485.yaml => -2092685929.yaml} | 0
.../{-1751589910.yaml => -2103698821.yaml} | 0
.../{-1775227539.yaml => -2124573175.yaml} | 0
.../{-1789367903.yaml => -2137058494.yaml} | 0
.../{1086505991.yaml => 1220553308.yaml} | 0
.../{1097760815.yaml => 1232335243.yaml} | 0
.../{1138917448.yaml => 1268787721.yaml} | 0
.../{1150205447.yaml => 1280026481.yaml} | 0
.../{1420108742.yaml => 1537628577.yaml} | 0
.../{1467312061.yaml => 1585090099.yaml} | 0
.../{1927303313.yaml => 1602616237.yaml} | 0
.../{1938979366.yaml => 1613681022.yaml} | 0
.../{1952346695.yaml => 1626072053.yaml} | 0
.../{1974030854.yaml => 1646900102.yaml} | 0
.../{1985613525.yaml => 1657966214.yaml} | 0
.../{1998093904.yaml => 1670376312.yaml} | 0
.../{1634357786.yaml => 1757260422.yaml} | 0
.../{1681783699.yaml => 1805230755.yaml} | 0
.../{1830642082.yaml => 1937728316.yaml} | 0
.../{1878182050.yaml => 1985086121.yaml} | 0
.../{1951197037.yaml => 2057262257.yaml} | 0
.../{1998619737.yaml => 2104354795.yaml} | 0
.../{-1801493263.yaml => 2146834518.yaml} | 0
.../dummy/{287216712.yaml => 456833036.yaml} | 0
.../dummy/{299277607.yaml => 468732604.yaml} | 0
.../dummy/{759067436.yaml => 469161994.yaml} | 0
.../dummy/{770754525.yaml => 480506437.yaml} | 0
.../dummy/{312857611.yaml => 482207779.yaml} | 0
.../dummy/{784016529.yaml => 492975979.yaml} | 0
.../dummy/{335131185.yaml => 504244477.yaml} | 0
.../dummy/{807557424.yaml => 513794821.yaml} | 0
.../dummy/{347069224.yaml => 516123791.yaml} | 0
.../dummy/{827720657.yaml => 524841244.yaml} | 0
.../dummy/{360590070.yaml => 529500696.yaml} | 0
.../dummy/{840878174.yaml => 537303732.yaml} | 0
.../dummy/{397572609.yaml => 566160662.yaml} | 0
.../dummy/{409649008.yaml => 578239938.yaml} | 0
.../dummy/{886296696.yaml => 581613745.yaml} | 0
.../dummy/{423450698.yaml => 591889373.yaml} | 0
.../dummy/{445682784.yaml => 614256992.yaml} | 0
.../dummy/{931879073.yaml => 625795318.yaml} | 0
.../dummy/{461533561.yaml => 626539933.yaml} | 0
.../dummy/{477108873.yaml => 640361184.yaml} | 0
.../dummy/{518554172.yaml => 678290561.yaml} | 0
.../dummy/{530725211.yaml => 690376123.yaml} | 0
.../dummy/{544173349.yaml => 703888106.yaml} | 0
.../dummy/{571021364.yaml => 728549465.yaml} | 0
.../dummy/{582894029.yaml => 740483676.yaml} | 0
.../dummy/{597217384.yaml => 753965555.yaml} | 0
.../dummy/{1086409496.yaml => 774184385.yaml} | 0
.../dummy/{1132989736.yaml => 818585753.yaml} | 0
src/mtc/src/impl/moveit_mediator.cpp | 75 +++++++++++++++---
71 files changed, 149 insertions(+), 66 deletions(-)
rename src/mtc/results/dummy/{-1113960400.yaml => -1511245846.yaml} (100%)
rename src/mtc/results/dummy/{-1130060311.yaml => -1533781679.yaml} (100%)
rename src/mtc/results/dummy/{-1141587806.yaml => -1545058710.yaml} (100%)
rename src/mtc/results/dummy/{-1164134400.yaml => -1569157975.yaml} (100%)
rename src/mtc/results/dummy/{-1177867363.yaml => -1581632251.yaml} (100%)
rename src/mtc/results/dummy/{-1190597504.yaml => -1592744663.yaml} (100%)
rename src/mtc/results/dummy/{-1241254685.yaml => -1637096250.yaml} (100%)
rename src/mtc/results/dummy/{-1287858113.yaml => -1682040034.yaml} (100%)
rename src/mtc/results/dummy/{-2019620283.yaml => -1897283987.yaml} (100%)
rename src/mtc/results/dummy/{-2070819246.yaml => -1946904381.yaml} (100%)
rename src/mtc/results/dummy/{-1725746253.yaml => -2080265983.yaml} (100%)
rename src/mtc/results/dummy/{-1739448485.yaml => -2092685929.yaml} (100%)
rename src/mtc/results/dummy/{-1751589910.yaml => -2103698821.yaml} (100%)
rename src/mtc/results/dummy/{-1775227539.yaml => -2124573175.yaml} (100%)
rename src/mtc/results/dummy/{-1789367903.yaml => -2137058494.yaml} (100%)
rename src/mtc/results/dummy/{1086505991.yaml => 1220553308.yaml} (100%)
rename src/mtc/results/dummy/{1097760815.yaml => 1232335243.yaml} (100%)
rename src/mtc/results/dummy/{1138917448.yaml => 1268787721.yaml} (100%)
rename src/mtc/results/dummy/{1150205447.yaml => 1280026481.yaml} (100%)
rename src/mtc/results/dummy/{1420108742.yaml => 1537628577.yaml} (100%)
rename src/mtc/results/dummy/{1467312061.yaml => 1585090099.yaml} (100%)
rename src/mtc/results/dummy/{1927303313.yaml => 1602616237.yaml} (100%)
rename src/mtc/results/dummy/{1938979366.yaml => 1613681022.yaml} (100%)
rename src/mtc/results/dummy/{1952346695.yaml => 1626072053.yaml} (100%)
rename src/mtc/results/dummy/{1974030854.yaml => 1646900102.yaml} (100%)
rename src/mtc/results/dummy/{1985613525.yaml => 1657966214.yaml} (100%)
rename src/mtc/results/dummy/{1998093904.yaml => 1670376312.yaml} (100%)
rename src/mtc/results/dummy/{1634357786.yaml => 1757260422.yaml} (100%)
rename src/mtc/results/dummy/{1681783699.yaml => 1805230755.yaml} (100%)
rename src/mtc/results/dummy/{1830642082.yaml => 1937728316.yaml} (100%)
rename src/mtc/results/dummy/{1878182050.yaml => 1985086121.yaml} (100%)
rename src/mtc/results/dummy/{1951197037.yaml => 2057262257.yaml} (100%)
rename src/mtc/results/dummy/{1998619737.yaml => 2104354795.yaml} (100%)
rename src/mtc/results/dummy/{-1801493263.yaml => 2146834518.yaml} (100%)
rename src/mtc/results/dummy/{287216712.yaml => 456833036.yaml} (100%)
rename src/mtc/results/dummy/{299277607.yaml => 468732604.yaml} (100%)
rename src/mtc/results/dummy/{759067436.yaml => 469161994.yaml} (100%)
rename src/mtc/results/dummy/{770754525.yaml => 480506437.yaml} (100%)
rename src/mtc/results/dummy/{312857611.yaml => 482207779.yaml} (100%)
rename src/mtc/results/dummy/{784016529.yaml => 492975979.yaml} (100%)
rename src/mtc/results/dummy/{335131185.yaml => 504244477.yaml} (100%)
rename src/mtc/results/dummy/{807557424.yaml => 513794821.yaml} (100%)
rename src/mtc/results/dummy/{347069224.yaml => 516123791.yaml} (100%)
rename src/mtc/results/dummy/{827720657.yaml => 524841244.yaml} (100%)
rename src/mtc/results/dummy/{360590070.yaml => 529500696.yaml} (100%)
rename src/mtc/results/dummy/{840878174.yaml => 537303732.yaml} (100%)
rename src/mtc/results/dummy/{397572609.yaml => 566160662.yaml} (100%)
rename src/mtc/results/dummy/{409649008.yaml => 578239938.yaml} (100%)
rename src/mtc/results/dummy/{886296696.yaml => 581613745.yaml} (100%)
rename src/mtc/results/dummy/{423450698.yaml => 591889373.yaml} (100%)
rename src/mtc/results/dummy/{445682784.yaml => 614256992.yaml} (100%)
rename src/mtc/results/dummy/{931879073.yaml => 625795318.yaml} (100%)
rename src/mtc/results/dummy/{461533561.yaml => 626539933.yaml} (100%)
rename src/mtc/results/dummy/{477108873.yaml => 640361184.yaml} (100%)
rename src/mtc/results/dummy/{518554172.yaml => 678290561.yaml} (100%)
rename src/mtc/results/dummy/{530725211.yaml => 690376123.yaml} (100%)
rename src/mtc/results/dummy/{544173349.yaml => 703888106.yaml} (100%)
rename src/mtc/results/dummy/{571021364.yaml => 728549465.yaml} (100%)
rename src/mtc/results/dummy/{582894029.yaml => 740483676.yaml} (100%)
rename src/mtc/results/dummy/{597217384.yaml => 753965555.yaml} (100%)
rename src/mtc/results/dummy/{1086409496.yaml => 774184385.yaml} (100%)
rename src/mtc/results/dummy/{1132989736.yaml => 818585753.yaml} (100%)
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 6e11520..bf168fc 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 24055
+/home/matteo/reachability/devel/./cmake.lock 24364
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/.vscode/c_cpp_properties.json b/src/.vscode/c_cpp_properties.json
index 836711d..f0bfc34 100644
--- a/src/.vscode/c_cpp_properties.json
+++ b/src/.vscode/c_cpp_properties.json
@@ -1,51 +1,29 @@
{
- "configurations": [
- {
- "browse": {
- "databaseFilename": "",
- "limitSymbolsToIncludedHeaders": true
- },
- "includePath": [
- "/home/matteo/ws_moveit/devel/include/**",
- "/opt/ros/noetic/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_planners/chomp/chomp_motion_planner/include/**",
- "/home/matteo/ws_moveit/src/eigen_stl_containers/include/**",
- "/home/matteo/ws_moveit/src/geometric_shapes/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_planners/chomp/chomp_interface/include/**",
- "/home/matteo/ws_moveit/src/moveit_resources/prbt_ikfast_manipulator_plugin/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_ros/benchmarks/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_plugins/moveit_ros_control_interface/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_ros/move_group/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_ros/occupancy_map_monitor/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_ros/robot_interaction/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_ros/moveit_servo/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_setup_assistant/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_plugins/moveit_simple_controller_manager/include/**",
- "/home/matteo/ws_moveit/src/moveit_visual_tools/include/**",
- "/home/matteo/ws_moveit/src/octomap_msgs/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_planners/pilz_industrial_motion_planner/include/**",
- "/home/matteo/ws_moveit/src/moveit/moveit_planners/pilz_industrial_motion_planner_testutils/include/**",
- "/home/matteo/ws_moveit/src/pybind11_catkin/include/**",
- "/home/matteo/ws_moveit/src/random_numbers/include/**",
- "/home/matteo/ws_panda/src/reachability/include/**",
- "/home/matteo/ws_moveit/src/rosparam_shortcuts/include/**",
- "/home/matteo/ws_moveit/src/rviz_visual_tools/include/**",
- "/home/matteo/ws_moveit/src/srdfdom/include/**",
- "/home/matteo/ws_moveit/src/warehouse_ros/include/**",
- "/usr/include/**",
- "${workspaceFolder}/mtc/include",
- "${workspaceFolder}/mtc/include/impl",
- "${workspaceFolder}/mtc/include/impl/ho",
- "${workspaceFolder}/mtc/src",
- "${workspaceFolder}/yaml_to_mtc/include",
- "${workspaceFolder}/moveit_task_constructor/core/include",
- "${workspaceFolder}/moveit_task_constructor/",
- "${workspaceFolder}/object_creator/include"
- ],
- "name": "ROS",
- "cStandard": "c17",
- "cppStandard": "c++17"
- }
- ],
- "version": 4
+ "configurations": [
+ {
+ "browse": {
+ "databaseFilename": "${default}",
+ "limitSymbolsToIncludedHeaders": false
+ },
+ "includePath": [
+ "/home/matteo/reachability/devel/include/**",
+ "/opt/ros/noetic/include/**",
+ "/home/matteo/reachability/src/moveit_task_constructor/core/include/**",
+ "/home/matteo/reachability/src/moveit_task_constructor/demo/include/**",
+ "/home/matteo/reachability/src/mtc/include/**",
+ "/home/matteo/reachability/src/object_creator/include/**",
+ "/home/matteo/reachability/src/reachability/include/**",
+ "/home/matteo/reachability/src/moveit_task_constructor/rviz_marker_tools/include/**",
+ "/home/matteo/reachability/src/simulation_util/include/**",
+ "/home/matteo/reachability/src/yaml_to_mtc/include/**",
+ "/usr/include/**"
+ ],
+ "name": "ROS",
+ "intelliSenseMode": "gcc-x64",
+ "compilerPath": "/usr/bin/gcc",
+ "cStandard": "gnu11",
+ "cppStandard": "c++14"
+ }
+ ],
+ "version": 4
}
\ No newline at end of file
diff --git a/src/franka_description/robots/ceti_double.urdf.xacro b/src/franka_description/robots/ceti_double.urdf.xacro
index 9619220..74ae3bd 100644
--- a/src/franka_description/robots/ceti_double.urdf.xacro
+++ b/src/franka_description/robots/ceti_double.urdf.xacro
@@ -13,7 +13,7 @@
<link name="world" />
- <xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-1801493263.yaml" />
+ <xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-1511245846.yaml" />
<xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" />
<xacro:property name="x1" value="${props['objects'][8]['pos']['x']}" />
diff --git a/src/mtc/descriptions/dummy2.yaml b/src/mtc/descriptions/dummy2.yaml
index 5fe8bea..ac7a2a9 100644
--- a/src/mtc/descriptions/dummy2.yaml
+++ b/src/mtc/descriptions/dummy2.yaml
@@ -1 +1,51 @@
-{task: {groups: {robot_x: [-0.300000 -0.700000 0.890000, -0.300000 -0.600000 0.890000, -0.200000 -0.700000 0.890000, -0.200000 -0.600000 0.890000, 0.100000 -0.700000 0.890000, 0.100000 -0.600000 0.890000, 0.100000 -0.300000 0.890000, 0.100000 -0.200000 0.890000, 0.100000 -0.100000 0.890000, 0.200000 -0.300000 0.890000, 0.200000 -0.200000 0.890000, 0.200000 -0.100000 0.890000, 0.300000 -0.300000 0.890000, 0.300000 -0.200000 0.890000, 0.300000 -0.100000 0.890000, 0.100000 0.100000 0.890000, 0.100000 0.200000 0.890000, 0.100000 0.300000 0.890000, 0.200000 0.100000 0.890000, 0.200000 0.200000 0.890000, 0.200000 0.300000 0.890000, 0.300000 0.100000 0.890000, 0.300000 0.200000 0.890000, 0.300000 0.300000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000], robot_y: [0.100000 1.010000 0.890000, 0.100000 1.110000 0.890000, 0.100000 1.210000 0.890000, 0.200000 1.010000 0.890000, 0.200000 1.110000 0.890000, 0.200000 1.210000 0.890000, 0.300000 1.010000 0.890000, 0.300000 1.110000 0.890000, 0.300000 1.210000 0.890000, 0.100000 1.410000 0.890000, 0.100000 1.510000 0.890000, 0.100000 1.610000 0.890000, 0.200000 1.410000 0.890000, 0.200000 1.510000 0.890000, 0.200000 1.610000 0.890000, 0.300000 1.410000 0.890000, 0.300000 1.510000 0.890000, 0.300000 1.610000 0.890000, -0.300000 1.910000 0.890000, -0.300000 2.010000 0.890000, -0.200000 1.910000 0.890000, -0.200000 2.010000 0.890000, 0.100000 1.910000 0.890000, 0.100000 2.010000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000, -0.300000 0.600000 0.890000, -0.300000 0.700000 0.890000, -0.200000 0.600000 0.890000, -0.200000 0.700000 0.890000, 0.100000 0.600000 0.890000, 0.100000 0.700000 0.890000]}}}
\ No newline at end of file
+task:
+ groups:
+ panda_arm1:
+ - -0.300000 -0.700000 0.890000
+ - -0.300000 -0.600000 0.890000
+ - -0.200000 -0.700000 0.890000
+ - -0.200000 -0.600000 0.890000
+ - 0.100000 -0.700000 0.890000
+ - 0.100000 -0.600000 0.890000
+ - 0.100000 -0.300000 0.890000
+ - 0.100000 -0.200000 0.890000
+ - 0.100000 -0.100000 0.890000
+ - 0.200000 -0.300000 0.890000
+ - 0.200000 -0.200000 0.890000
+ - 0.200000 -0.100000 0.890000
+ - 0.300000 -0.300000 0.890000
+ - 0.300000 -0.200000 0.890000
+ - 0.300000 -0.100000 0.890000
+ - 0.100000 0.100000 0.890000
+ - 0.100000 0.200000 0.890000
+ - 0.100000 0.300000 0.890000
+ - 0.200000 0.100000 0.890000
+ - 0.200000 0.200000 0.890000
+ - 0.200000 0.300000 0.890000
+ - 0.300000 0.100000 0.890000
+ - 0.300000 0.200000 0.890000
+ - 0.300000 0.300000 0.890000
+ panda_arm2:
+ - 0.100000 1.110000 0.890000
+ - 0.100000 1.210000 0.890000
+ - 0.200000 1.010000 0.890000
+ - 0.200000 1.110000 0.890000
+ - 0.200000 1.210000 0.890000
+ - 0.300000 1.010000 0.890000
+ - 0.300000 1.110000 0.890000
+ - 0.300000 1.210000 0.890000
+ - 0.100000 1.410000 0.890000
+ - 0.100000 1.510000 0.890000
+ - 0.100000 1.610000 0.890000
+ - 0.200000 1.410000 0.890000
+ - 0.200000 1.510000 0.890000
+ - 0.200000 1.610000 0.890000
+ - 0.300000 1.410000 0.890000
+ - 0.300000 1.510000 0.890000
+ - 0.300000 1.610000 0.890000
+ - -0.300000 1.910000 0.890000
+ - -0.300000 2.010000 0.890000
+ - -0.200000 1.910000 0.890000
+ - -0.200000 2.010000 0.890000
+ - 0.100000 1.910000 0.890000
+ - 0.100000 2.010000 0.890000
\ No newline at end of file
diff --git a/src/mtc/include/impl/abstract_robot.h b/src/mtc/include/impl/abstract_robot.h
index 9a3430e..e3cbc6f 100644
--- a/src/mtc/include/impl/abstract_robot.h
+++ b/src/mtc/include/impl/abstract_robot.h
@@ -51,6 +51,8 @@ class Abstract_robot {
observer_mask_ = std::bitset<3>(0);
};
+
+
inline std::string& name() { return name_;}
inline tf2::Transform& tf() { return tf_;}
diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h
index 8b73a8e..ae5dac0 100644
--- a/src/mtc/include/impl/moveit_mediator.h
+++ b/src/mtc/include/impl/moveit_mediator.h
@@ -71,6 +71,7 @@ class Moveit_mediator : public Abstract_mediator{
void publish_tables();
void load_robot_description();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
+ void parallel_exec(Abstract_robot& penis, moveit_msgs::CollisionObject& source, std::vector<tf2::Transform>& target);
void create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h
index ae1c707..e798395 100644
--- a/src/mtc/include/impl/moveit_robot.h
+++ b/src/mtc/include/impl/moveit_robot.h
@@ -25,6 +25,7 @@ class Moveit_robot : public Robot{
map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));
+
}
inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}
diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch
index f5c4b99..a808d5e 100644
--- a/src/mtc/launch/cell_routine.launch
+++ b/src/mtc/launch/cell_routine.launch
@@ -3,12 +3,12 @@
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
<!-- this is to change-->
- <rosparam command="load" file="$(find mtc)/results/dummy/-1801493263.yaml"/>
+ <rosparam command="load" file="$(find mtc)/results/dummy/-1511245846.yaml"/>
<rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" />
<rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
- <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
+ <rosparam command="load" file="$(find mtc)/descriptions/dummy2.yaml"/>
<include file="$(find ceti_double)/launch/demo.launch">
diff --git a/src/mtc/results/dummy/-1113960400.yaml b/src/mtc/results/dummy/-1511245846.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1113960400.yaml
rename to src/mtc/results/dummy/-1511245846.yaml
diff --git a/src/mtc/results/dummy/-1130060311.yaml b/src/mtc/results/dummy/-1533781679.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1130060311.yaml
rename to src/mtc/results/dummy/-1533781679.yaml
diff --git a/src/mtc/results/dummy/-1141587806.yaml b/src/mtc/results/dummy/-1545058710.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1141587806.yaml
rename to src/mtc/results/dummy/-1545058710.yaml
diff --git a/src/mtc/results/dummy/-1164134400.yaml b/src/mtc/results/dummy/-1569157975.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1164134400.yaml
rename to src/mtc/results/dummy/-1569157975.yaml
diff --git a/src/mtc/results/dummy/-1177867363.yaml b/src/mtc/results/dummy/-1581632251.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1177867363.yaml
rename to src/mtc/results/dummy/-1581632251.yaml
diff --git a/src/mtc/results/dummy/-1190597504.yaml b/src/mtc/results/dummy/-1592744663.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1190597504.yaml
rename to src/mtc/results/dummy/-1592744663.yaml
diff --git a/src/mtc/results/dummy/-1241254685.yaml b/src/mtc/results/dummy/-1637096250.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1241254685.yaml
rename to src/mtc/results/dummy/-1637096250.yaml
diff --git a/src/mtc/results/dummy/-1287858113.yaml b/src/mtc/results/dummy/-1682040034.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1287858113.yaml
rename to src/mtc/results/dummy/-1682040034.yaml
diff --git a/src/mtc/results/dummy/-2019620283.yaml b/src/mtc/results/dummy/-1897283987.yaml
similarity index 100%
rename from src/mtc/results/dummy/-2019620283.yaml
rename to src/mtc/results/dummy/-1897283987.yaml
diff --git a/src/mtc/results/dummy/-2070819246.yaml b/src/mtc/results/dummy/-1946904381.yaml
similarity index 100%
rename from src/mtc/results/dummy/-2070819246.yaml
rename to src/mtc/results/dummy/-1946904381.yaml
diff --git a/src/mtc/results/dummy/-1725746253.yaml b/src/mtc/results/dummy/-2080265983.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1725746253.yaml
rename to src/mtc/results/dummy/-2080265983.yaml
diff --git a/src/mtc/results/dummy/-1739448485.yaml b/src/mtc/results/dummy/-2092685929.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1739448485.yaml
rename to src/mtc/results/dummy/-2092685929.yaml
diff --git a/src/mtc/results/dummy/-1751589910.yaml b/src/mtc/results/dummy/-2103698821.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1751589910.yaml
rename to src/mtc/results/dummy/-2103698821.yaml
diff --git a/src/mtc/results/dummy/-1775227539.yaml b/src/mtc/results/dummy/-2124573175.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1775227539.yaml
rename to src/mtc/results/dummy/-2124573175.yaml
diff --git a/src/mtc/results/dummy/-1789367903.yaml b/src/mtc/results/dummy/-2137058494.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1789367903.yaml
rename to src/mtc/results/dummy/-2137058494.yaml
diff --git a/src/mtc/results/dummy/1086505991.yaml b/src/mtc/results/dummy/1220553308.yaml
similarity index 100%
rename from src/mtc/results/dummy/1086505991.yaml
rename to src/mtc/results/dummy/1220553308.yaml
diff --git a/src/mtc/results/dummy/1097760815.yaml b/src/mtc/results/dummy/1232335243.yaml
similarity index 100%
rename from src/mtc/results/dummy/1097760815.yaml
rename to src/mtc/results/dummy/1232335243.yaml
diff --git a/src/mtc/results/dummy/1138917448.yaml b/src/mtc/results/dummy/1268787721.yaml
similarity index 100%
rename from src/mtc/results/dummy/1138917448.yaml
rename to src/mtc/results/dummy/1268787721.yaml
diff --git a/src/mtc/results/dummy/1150205447.yaml b/src/mtc/results/dummy/1280026481.yaml
similarity index 100%
rename from src/mtc/results/dummy/1150205447.yaml
rename to src/mtc/results/dummy/1280026481.yaml
diff --git a/src/mtc/results/dummy/1420108742.yaml b/src/mtc/results/dummy/1537628577.yaml
similarity index 100%
rename from src/mtc/results/dummy/1420108742.yaml
rename to src/mtc/results/dummy/1537628577.yaml
diff --git a/src/mtc/results/dummy/1467312061.yaml b/src/mtc/results/dummy/1585090099.yaml
similarity index 100%
rename from src/mtc/results/dummy/1467312061.yaml
rename to src/mtc/results/dummy/1585090099.yaml
diff --git a/src/mtc/results/dummy/1927303313.yaml b/src/mtc/results/dummy/1602616237.yaml
similarity index 100%
rename from src/mtc/results/dummy/1927303313.yaml
rename to src/mtc/results/dummy/1602616237.yaml
diff --git a/src/mtc/results/dummy/1938979366.yaml b/src/mtc/results/dummy/1613681022.yaml
similarity index 100%
rename from src/mtc/results/dummy/1938979366.yaml
rename to src/mtc/results/dummy/1613681022.yaml
diff --git a/src/mtc/results/dummy/1952346695.yaml b/src/mtc/results/dummy/1626072053.yaml
similarity index 100%
rename from src/mtc/results/dummy/1952346695.yaml
rename to src/mtc/results/dummy/1626072053.yaml
diff --git a/src/mtc/results/dummy/1974030854.yaml b/src/mtc/results/dummy/1646900102.yaml
similarity index 100%
rename from src/mtc/results/dummy/1974030854.yaml
rename to src/mtc/results/dummy/1646900102.yaml
diff --git a/src/mtc/results/dummy/1985613525.yaml b/src/mtc/results/dummy/1657966214.yaml
similarity index 100%
rename from src/mtc/results/dummy/1985613525.yaml
rename to src/mtc/results/dummy/1657966214.yaml
diff --git a/src/mtc/results/dummy/1998093904.yaml b/src/mtc/results/dummy/1670376312.yaml
similarity index 100%
rename from src/mtc/results/dummy/1998093904.yaml
rename to src/mtc/results/dummy/1670376312.yaml
diff --git a/src/mtc/results/dummy/1634357786.yaml b/src/mtc/results/dummy/1757260422.yaml
similarity index 100%
rename from src/mtc/results/dummy/1634357786.yaml
rename to src/mtc/results/dummy/1757260422.yaml
diff --git a/src/mtc/results/dummy/1681783699.yaml b/src/mtc/results/dummy/1805230755.yaml
similarity index 100%
rename from src/mtc/results/dummy/1681783699.yaml
rename to src/mtc/results/dummy/1805230755.yaml
diff --git a/src/mtc/results/dummy/1830642082.yaml b/src/mtc/results/dummy/1937728316.yaml
similarity index 100%
rename from src/mtc/results/dummy/1830642082.yaml
rename to src/mtc/results/dummy/1937728316.yaml
diff --git a/src/mtc/results/dummy/1878182050.yaml b/src/mtc/results/dummy/1985086121.yaml
similarity index 100%
rename from src/mtc/results/dummy/1878182050.yaml
rename to src/mtc/results/dummy/1985086121.yaml
diff --git a/src/mtc/results/dummy/1951197037.yaml b/src/mtc/results/dummy/2057262257.yaml
similarity index 100%
rename from src/mtc/results/dummy/1951197037.yaml
rename to src/mtc/results/dummy/2057262257.yaml
diff --git a/src/mtc/results/dummy/1998619737.yaml b/src/mtc/results/dummy/2104354795.yaml
similarity index 100%
rename from src/mtc/results/dummy/1998619737.yaml
rename to src/mtc/results/dummy/2104354795.yaml
diff --git a/src/mtc/results/dummy/-1801493263.yaml b/src/mtc/results/dummy/2146834518.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1801493263.yaml
rename to src/mtc/results/dummy/2146834518.yaml
diff --git a/src/mtc/results/dummy/287216712.yaml b/src/mtc/results/dummy/456833036.yaml
similarity index 100%
rename from src/mtc/results/dummy/287216712.yaml
rename to src/mtc/results/dummy/456833036.yaml
diff --git a/src/mtc/results/dummy/299277607.yaml b/src/mtc/results/dummy/468732604.yaml
similarity index 100%
rename from src/mtc/results/dummy/299277607.yaml
rename to src/mtc/results/dummy/468732604.yaml
diff --git a/src/mtc/results/dummy/759067436.yaml b/src/mtc/results/dummy/469161994.yaml
similarity index 100%
rename from src/mtc/results/dummy/759067436.yaml
rename to src/mtc/results/dummy/469161994.yaml
diff --git a/src/mtc/results/dummy/770754525.yaml b/src/mtc/results/dummy/480506437.yaml
similarity index 100%
rename from src/mtc/results/dummy/770754525.yaml
rename to src/mtc/results/dummy/480506437.yaml
diff --git a/src/mtc/results/dummy/312857611.yaml b/src/mtc/results/dummy/482207779.yaml
similarity index 100%
rename from src/mtc/results/dummy/312857611.yaml
rename to src/mtc/results/dummy/482207779.yaml
diff --git a/src/mtc/results/dummy/784016529.yaml b/src/mtc/results/dummy/492975979.yaml
similarity index 100%
rename from src/mtc/results/dummy/784016529.yaml
rename to src/mtc/results/dummy/492975979.yaml
diff --git a/src/mtc/results/dummy/335131185.yaml b/src/mtc/results/dummy/504244477.yaml
similarity index 100%
rename from src/mtc/results/dummy/335131185.yaml
rename to src/mtc/results/dummy/504244477.yaml
diff --git a/src/mtc/results/dummy/807557424.yaml b/src/mtc/results/dummy/513794821.yaml
similarity index 100%
rename from src/mtc/results/dummy/807557424.yaml
rename to src/mtc/results/dummy/513794821.yaml
diff --git a/src/mtc/results/dummy/347069224.yaml b/src/mtc/results/dummy/516123791.yaml
similarity index 100%
rename from src/mtc/results/dummy/347069224.yaml
rename to src/mtc/results/dummy/516123791.yaml
diff --git a/src/mtc/results/dummy/827720657.yaml b/src/mtc/results/dummy/524841244.yaml
similarity index 100%
rename from src/mtc/results/dummy/827720657.yaml
rename to src/mtc/results/dummy/524841244.yaml
diff --git a/src/mtc/results/dummy/360590070.yaml b/src/mtc/results/dummy/529500696.yaml
similarity index 100%
rename from src/mtc/results/dummy/360590070.yaml
rename to src/mtc/results/dummy/529500696.yaml
diff --git a/src/mtc/results/dummy/840878174.yaml b/src/mtc/results/dummy/537303732.yaml
similarity index 100%
rename from src/mtc/results/dummy/840878174.yaml
rename to src/mtc/results/dummy/537303732.yaml
diff --git a/src/mtc/results/dummy/397572609.yaml b/src/mtc/results/dummy/566160662.yaml
similarity index 100%
rename from src/mtc/results/dummy/397572609.yaml
rename to src/mtc/results/dummy/566160662.yaml
diff --git a/src/mtc/results/dummy/409649008.yaml b/src/mtc/results/dummy/578239938.yaml
similarity index 100%
rename from src/mtc/results/dummy/409649008.yaml
rename to src/mtc/results/dummy/578239938.yaml
diff --git a/src/mtc/results/dummy/886296696.yaml b/src/mtc/results/dummy/581613745.yaml
similarity index 100%
rename from src/mtc/results/dummy/886296696.yaml
rename to src/mtc/results/dummy/581613745.yaml
diff --git a/src/mtc/results/dummy/423450698.yaml b/src/mtc/results/dummy/591889373.yaml
similarity index 100%
rename from src/mtc/results/dummy/423450698.yaml
rename to src/mtc/results/dummy/591889373.yaml
diff --git a/src/mtc/results/dummy/445682784.yaml b/src/mtc/results/dummy/614256992.yaml
similarity index 100%
rename from src/mtc/results/dummy/445682784.yaml
rename to src/mtc/results/dummy/614256992.yaml
diff --git a/src/mtc/results/dummy/931879073.yaml b/src/mtc/results/dummy/625795318.yaml
similarity index 100%
rename from src/mtc/results/dummy/931879073.yaml
rename to src/mtc/results/dummy/625795318.yaml
diff --git a/src/mtc/results/dummy/461533561.yaml b/src/mtc/results/dummy/626539933.yaml
similarity index 100%
rename from src/mtc/results/dummy/461533561.yaml
rename to src/mtc/results/dummy/626539933.yaml
diff --git a/src/mtc/results/dummy/477108873.yaml b/src/mtc/results/dummy/640361184.yaml
similarity index 100%
rename from src/mtc/results/dummy/477108873.yaml
rename to src/mtc/results/dummy/640361184.yaml
diff --git a/src/mtc/results/dummy/518554172.yaml b/src/mtc/results/dummy/678290561.yaml
similarity index 100%
rename from src/mtc/results/dummy/518554172.yaml
rename to src/mtc/results/dummy/678290561.yaml
diff --git a/src/mtc/results/dummy/530725211.yaml b/src/mtc/results/dummy/690376123.yaml
similarity index 100%
rename from src/mtc/results/dummy/530725211.yaml
rename to src/mtc/results/dummy/690376123.yaml
diff --git a/src/mtc/results/dummy/544173349.yaml b/src/mtc/results/dummy/703888106.yaml
similarity index 100%
rename from src/mtc/results/dummy/544173349.yaml
rename to src/mtc/results/dummy/703888106.yaml
diff --git a/src/mtc/results/dummy/571021364.yaml b/src/mtc/results/dummy/728549465.yaml
similarity index 100%
rename from src/mtc/results/dummy/571021364.yaml
rename to src/mtc/results/dummy/728549465.yaml
diff --git a/src/mtc/results/dummy/582894029.yaml b/src/mtc/results/dummy/740483676.yaml
similarity index 100%
rename from src/mtc/results/dummy/582894029.yaml
rename to src/mtc/results/dummy/740483676.yaml
diff --git a/src/mtc/results/dummy/597217384.yaml b/src/mtc/results/dummy/753965555.yaml
similarity index 100%
rename from src/mtc/results/dummy/597217384.yaml
rename to src/mtc/results/dummy/753965555.yaml
diff --git a/src/mtc/results/dummy/1086409496.yaml b/src/mtc/results/dummy/774184385.yaml
similarity index 100%
rename from src/mtc/results/dummy/1086409496.yaml
rename to src/mtc/results/dummy/774184385.yaml
diff --git a/src/mtc/results/dummy/1132989736.yaml b/src/mtc/results/dummy/818585753.yaml
similarity index 100%
rename from src/mtc/results/dummy/1132989736.yaml
rename to src/mtc/results/dummy/818585753.yaml
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index fbdd761..5dfc397 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -1,5 +1,5 @@
#include "impl/moveit_mediator.h"
-#include <yaml_to_mtc.h>
+#include <thread>
@@ -127,25 +127,76 @@ void Moveit_mediator::setup_task(){
create_Task(robots_[i], psi_->getObjects().at("bottle"), objects_[i][j]);
}
}
+ } else {
+ moveit_msgs::CollisionObject bottle1;
+ bottle1.id = "bottle1";
+ bottle1.header.frame_id = "world";
+ bottle1.header.stamp = ros::Time::now();
+ bottle1.primitives.resize(1);
+ bottle1.primitives[0].type = bottle1.primitives[0].BOX;
+ bottle1.primitives[0].dimensions.resize(3);
+ bottle1.primitives[0].dimensions[0] = box_size.getX();
+ bottle1.primitives[0].dimensions[1] = box_size.getY();
+ bottle1.primitives[0].dimensions[2] = box_size.getZ();
+
+ bottle1.primitive_poses.resize(1);
+ bottle1.primitive_poses[0].position.x = -0.3f;
+ bottle1.primitive_poses[0].position.y = -0.6f;
+ bottle1.primitive_poses[0].position.z = 0.9355f;
+ bottle1.primitive_poses[0].orientation.x = 0;
+ bottle1.primitive_poses[0].orientation.y = 0;
+ bottle1.primitive_poses[0].orientation.z = 0;
+ bottle1.primitive_poses[0].orientation.w = 1;
+ bottle1.operation = bottle1.ADD;
+
+ moveit_msgs::CollisionObject bottle2;
+ bottle2.id = "bottle2";
+ bottle2.header.frame_id = "world";
+ bottle2.header.stamp = ros::Time::now();
+ bottle2.primitives.resize(1);
+ bottle2.primitives[0].type = bottle2.primitives[0].BOX;
+ bottle2.primitives[0].dimensions.resize(3);
+ bottle2.primitives[0].dimensions[0] = box_size.getX();
+ bottle2.primitives[0].dimensions[1] = box_size.getY();
+ bottle2.primitives[0].dimensions[2] = box_size.getZ();
+
+ bottle2.primitive_poses.resize(1);
+ bottle2.primitive_poses[0].position.x = 0.1f;
+ bottle2.primitive_poses[0].position.y = 1.01f;
+ bottle2.primitive_poses[0].position.z = 0.9355f;
+ bottle2.primitive_poses[0].orientation.x = 0;
+ bottle2.primitive_poses[0].orientation.y = 0;
+ bottle2.primitive_poses[0].orientation.z = 0;
+ bottle2.primitive_poses[0].orientation.w = 1;
+ bottle2.operation = bottle2.ADD;
+
+ psi_->applyCollisionObject(bottle1);
+ psi_->applyCollisionObject(bottle2);
- tf2::Transform tr(tf2::Quaternion(0,0,0,1), tf2::Vector3(0, 0, 0.9305f));
/*
- rewrite_task_template(robots_[0], psi_->getObjects().at("bottle"), tr);
-
- Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
- Yaml_Mtc_Parser parser = Yaml_Mtc_Parser();
- moveit::task_constructor::Task task = parser.init_task(this->nh_);
- int max_planning_solutions = 1; // default one solution
- this->nh_.getParam("max_planning_solutions", max_planning_solutions);
- if (task.plan(max_planning_solutions)){
- moveit_msgs::MoveItErrorCodes execute_result;
- execute_result = task.execute(*task.solutions().front());
+ for(int j =0; j < objects_[0].size();j++){
+ create_Task(robots_[0], psi_->getObjects().at("bottle1"), objects_[0][j]);
+ }
+
+ for(int j =0; j < objects_[1].size();j++){
+ create_Task(robots_[1], psi_->getObjects().at("bottle2"), objects_[1][j]);
}
*/
+ std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(psi_->getObjects().at("bottle1")), std::ref(objects_[0]));
+ std::thread two(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[1]), std::ref(psi_->getObjects().at("bottle2")), std::ref(objects_[1]));
+
+ one.join();
+ two.join();
}
}
+void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit_msgs::CollisionObject& source, std::vector<tf2::Transform>& target){
+ for(int j =0; j < target.size();j++){
+ create_Task(&ar, source, target[j]);
+ }
+}
+
void Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){
tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
std::string support_surface1 = "nichts";
--
GitLab