diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 6e11520998536a490c5e8ee512c497b4502c2550..bf168fc51fdcad9ba0f69ab1a01217510adfb5e6 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 836711d39b12de45d9d0e96800c31359984789fc..f0bfc349a6ad669a6bf010738f8f789bbdc07673 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 96192201a6b1a2b2ae377c7c7f4ca65a187955c8..74ae3bd1fef1e50fd587412f7e0a1d3454d7f8a0 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 5fe8bea2718de6fd2c9092bb5b0aac0025525f78..ac7a2a901f8d43bd77e94b7a0746637a824dbf6f 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 9a3430e742ee735a4a61b6be90ebd3b6f4447c12..e3cbc6f62972356ff018ad57e104353ecc42f43b 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 8b73a8e11a41cb56a3d514fc058c8442e0d62fba..ae5dac0aba3d76bac6b2d049b2daffc84cededb2 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 ae1c707684a95ac47bc6da6152f5f60dbbe826a4..e798395308e5ee18042669769210f8fa8ac183bd 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 f5c4b997ded04d1527be5e281a4c92c8315e0b2f..a808d5e4adfc4c5fde0828bbd04ceee86cf38359 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 fbdd76144886590e56778662a9d175dce0b9d86d..5dfc397ce55ce5ef8922305982c350366c6af718 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";