From 053500d1f78009a2875609b6bd78fb2523f6ec8f Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Thu, 29 Dec 2022 18:03:14 +0100
Subject: [PATCH] trajetory for n robots
---
.vscode/settings.json | 75 +++++++-
CMakeLists.txt | 1 +
include/impl/moveit_mediator.h | 10 +
include/reader/job_reader.h | 8 +-
jobs/dummy.yaml | 96 +++++-----
launch/cell_routine.launch | 2 +-
launch/mtc_auerswald.launch | 1 +
results/dummy/-562289182.yaml | 12 +-
src/impl/moveit_mediator.cpp | 330 +++++++++++++++++++++++++++++----
src/reader/job_reader.cpp | 25 ++-
10 files changed, 458 insertions(+), 102 deletions(-)
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 6ffb513..401dd61 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -2,5 +2,78 @@
"python.autoComplete.extraPaths": [
"/home/matteo/ccf_sim_workspace/devel/lib/python3/dist-packages",
"/opt/ros/noetic/lib/python3/dist-packages"
- ]
+ ],
+ "files.associations": {
+ "cctype": "cpp",
+ "clocale": "cpp",
+ "cmath": "cpp",
+ "csignal": "cpp",
+ "cstdarg": "cpp",
+ "cstddef": "cpp",
+ "cstdio": "cpp",
+ "cstdlib": "cpp",
+ "cstring": "cpp",
+ "ctime": "cpp",
+ "cwchar": "cpp",
+ "cwctype": "cpp",
+ "any": "cpp",
+ "array": "cpp",
+ "atomic": "cpp",
+ "hash_map": "cpp",
+ "hash_set": "cpp",
+ "strstream": "cpp",
+ "bit": "cpp",
+ "*.tcc": "cpp",
+ "bitset": "cpp",
+ "chrono": "cpp",
+ "codecvt": "cpp",
+ "complex": "cpp",
+ "condition_variable": "cpp",
+ "cstdint": "cpp",
+ "deque": "cpp",
+ "forward_list": "cpp",
+ "list": "cpp",
+ "map": "cpp",
+ "set": "cpp",
+ "unordered_map": "cpp",
+ "unordered_set": "cpp",
+ "vector": "cpp",
+ "exception": "cpp",
+ "algorithm": "cpp",
+ "functional": "cpp",
+ "iterator": "cpp",
+ "memory": "cpp",
+ "memory_resource": "cpp",
+ "numeric": "cpp",
+ "optional": "cpp",
+ "random": "cpp",
+ "ratio": "cpp",
+ "regex": "cpp",
+ "string": "cpp",
+ "string_view": "cpp",
+ "system_error": "cpp",
+ "tuple": "cpp",
+ "type_traits": "cpp",
+ "utility": "cpp",
+ "fstream": "cpp",
+ "initializer_list": "cpp",
+ "iomanip": "cpp",
+ "iosfwd": "cpp",
+ "iostream": "cpp",
+ "istream": "cpp",
+ "limits": "cpp",
+ "mutex": "cpp",
+ "new": "cpp",
+ "ostream": "cpp",
+ "shared_mutex": "cpp",
+ "sstream": "cpp",
+ "stdexcept": "cpp",
+ "streambuf": "cpp",
+ "thread": "cpp",
+ "cfenv": "cpp",
+ "cinttypes": "cpp",
+ "typeindex": "cpp",
+ "typeinfo": "cpp",
+ "variant": "cpp"
+ }
}
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c5ab33e..f2afac9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -81,6 +81,7 @@ src/reader/abstract_param_reader.cpp
src/reader/robot_reader.cpp
src/reader/wing_reader.cpp
src/reader/cuboid_reader.cpp
+src/reader/job_reader.cpp
diff --git a/include/impl/moveit_mediator.h b/include/impl/moveit_mediator.h
index df4adfb..d9866f6 100644
--- a/include/impl/moveit_mediator.h
+++ b/include/impl/moveit_mediator.h
@@ -45,6 +45,9 @@
#include <moveit/planning_scene_monitor/current_state_monitor.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
+#include "reader/job_reader.h"
+#include "reader/cuboid_reader.h"
+
#include <stdint.h>
@@ -67,7 +70,11 @@ class Moveit_mediator : public Abstract_mediator{
std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
+ std::unique_ptr<Job_reader> job_reader_;
+ std::unique_ptr<Cuboid_reader> cuboid_reader_;
+
std::map<std::string, std::vector<uint8_t>> acm_;
+ std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_;
@@ -90,11 +97,14 @@ class Moveit_mediator : public Abstract_mediator{
void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
void merge_acm(moveit_msgs::PlanningScene& in);
+ void task_planner();
void publish_tables();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);
moveit::task_constructor::Task create_Task(Moveit_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_;};
+ inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;};
+
};
diff --git a/include/reader/job_reader.h b/include/reader/job_reader.h
index fedfd87..f02530b 100644
--- a/include/reader/job_reader.h
+++ b/include/reader/job_reader.h
@@ -4,21 +4,23 @@
#include "ros/ros.h"
#include <ros/package.h>
#include <xmlrpcpp/XmlRpc.h>
+#include "boost/circular_buffer.hpp"
+
#include "reader/abstract_param_reader.h"
class Job_reader : public Abstract_param_reader{
protected:
- std::map<std::string, std::vector<job_data>> job_data_;
+ boost::circular_buffer<std::pair<std::string, job_data>> job_data_;
public:
Job_reader(std::shared_ptr<ros::NodeHandle> const& d)
: Abstract_param_reader(d)
{read();}
- inline void set_job_data(std::map<std::string, std::vector<job_data>>& robot_data) {job_data_ = robot_data;}
- inline std::map<std::string, std::vector<job_data>>& robot_data() {return job_data_;}
+ inline void set_job_data(boost::circular_buffer<std::pair<std::string, job_data>>& robot_data) {job_data_ = robot_data;}
+ inline boost::circular_buffer<std::pair<std::string, job_data>>& robot_data() {return job_data_;}
void read() override;
};
diff --git a/jobs/dummy.yaml b/jobs/dummy.yaml
index 7a5ef0f..29d85b9 100644
--- a/jobs/dummy.yaml
+++ b/jobs/dummy.yaml
@@ -1,55 +1,55 @@
-{ 'task':
+{ 'tasks':
{'groups' : [
{ 'name': 'panda_arm1', 'jobs':[[
- { 'pos': { 'x': -0.300000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': -0.300000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': -0.200000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': -0.200000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }
+ { '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.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.200000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.300000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': -0.300000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': -0.300000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': -0.200000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': -0.200000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
- { 'pos': { 'x': 0.100000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }
+ { '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 } }
]]}
]
}
diff --git a/launch/cell_routine.launch b/launch/cell_routine.launch
index 7a516c1..373d6c3 100644
--- a/launch/cell_routine.launch
+++ b/launch/cell_routine.launch
@@ -9,7 +9,7 @@
<rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
<rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
- <!-- <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/> -->
+ <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
diff --git a/launch/mtc_auerswald.launch b/launch/mtc_auerswald.launch
index 7573c9c..cfc8642 100644
--- a/launch/mtc_auerswald.launch
+++ b/launch/mtc_auerswald.launch
@@ -66,6 +66,7 @@
<rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
<rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
+
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/grasp_map_config.yaml"/>
diff --git a/results/dummy/-562289182.yaml b/results/dummy/-562289182.yaml
index 3e97a90..0c4d30e 100644
--- a/results/dummy/-562289182.yaml
+++ b/results/dummy/-562289182.yaml
@@ -22,11 +22,7 @@
{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
- { 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
- { 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': .2, 'y': .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
- { 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B
- { 'id': 'green1', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
- { 'id': 'green2', 'type': 'BOX', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
- { 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
- { 'id': 'red2', 'type': 'BOX', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } }
-]}
\ No newline at end of file
+{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.11, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } } # target B
+]}
+
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index 9133caf..247672e 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -66,7 +66,8 @@ bool Moveit_mediator::check_collision(const int& robot){
void Moveit_mediator::mediate(){
publish_tables();
- setup_task();
+ task_planner();
+ setup_task();
ros::waitForShutdown();
@@ -96,6 +97,8 @@ void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>,
void Moveit_mediator::setup_task(){
ROS_INFO("=> write Task Constructor Objects");
+ for(auto& a: acm_) a.second.resize(acm_.size(), 0);
+ /*
bool coop = false;
@@ -225,26 +228,9 @@ void Moveit_mediator::setup_task(){
tasks.push_back(tasks_per_robot);
//ths.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), psi_->getObjects().at(ss.str()), objects_[i]));
}
- /*
- //for (int i = 0; i < ths.size(); i++) ths[i].join();
- std::vector<std::vector<std::vector<moveit_msgs::RobotTrajectory>>> rt_r_t;
- for (int i =0; i < tasks.size(); i++){
- std::vector<std::vector<moveit_msgs::RobotTrajectory>> rt_r;
- while (!tasks[i].empty()){
- std::vector<moveit_msgs::RobotTrajectory> rt;
- for(auto sub_t : tasks[i].front().solution.sub_trajectory){
- rt.push_back(sub_t.trajectory);
- }
- tasks[i].pop();
- rt_r.push_back(rt);
- }
- rt_r_t.push_back(rt_r);
- }
- */
for(auto& a: acm_) a.second.resize(acm_.size(), 0);
-
-
+
while (!tasks[0].empty() && !tasks[1].empty()){
for (int i =0; i < std::max(tasks[0].front().solution.sub_trajectory.size(), tasks[1].front().solution.sub_trajectory.size()); i++){
moveit_msgs::RobotTrajectory* t1 = nullptr;
@@ -287,10 +273,10 @@ void Moveit_mediator::setup_task(){
// Iterate task collsion matrix
for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){
if( mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){
- /* In case an entry matches an robot spezific link*/
+ In case an entry matches an robot spezific link
ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- /* For that specific entry, loop over values*/
+ For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
@@ -316,10 +302,10 @@ void Moveit_mediator::setup_task(){
}
if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
- /* In case an entry matches an robot spezific link*/
+ In case an entry matches an robot spezific link
ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- /* For that specific entry, loop over values*/
+ For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
@@ -345,10 +331,10 @@ void Moveit_mediator::setup_task(){
}
if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box)){
- /* In case an entry matches an robot spezific link*/
+ In case an entry matches an robot spezific link
ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- /* For that specific entry, loop over values*/
+ For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
@@ -374,10 +360,10 @@ void Moveit_mediator::setup_task(){
}
if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table)){
- /* In case an entry matches an robot spezific link*/
+ In case an entry matches an robot spezific link
ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j], j);
for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- /* For that specific entry, loop over values*/
+ For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
@@ -423,10 +409,10 @@ void Moveit_mediator::setup_task(){
// Iterate task collsion matrix
for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){
if( mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
- /* In case an entry matches an robot spezific link*/
+ In case an entry matches an robot spezific link
ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- /* For that specific entry, loop over values*/
+ For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
@@ -452,10 +438,10 @@ void Moveit_mediator::setup_task(){
}
if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
- /* In case an entry matches an robot spezific link*/
+ In case an entry matches an robot spezific link
ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- /* For that specific entry, loop over values*/
+ For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
@@ -481,10 +467,10 @@ void Moveit_mediator::setup_task(){
}
if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box)){
- /* In case an entry matches an robot spezific link*/
+ In case an entry matches an robot spezific link
ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- /* For that specific entry, loop over values*/
+ For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
@@ -510,10 +496,10 @@ void Moveit_mediator::setup_task(){
}
if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table)){
- /* In case an entry matches an robot spezific link*/
+ In case an entry matches an robot spezific link
ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j], j);
for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
- /* For that specific entry, loop over values*/
+ For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
@@ -553,9 +539,276 @@ void Moveit_mediator::setup_task(){
tasks[1].pop();
}
}
+ */
+
+
+ std::regex item("box_([0-9]+)");
+ std::smatch match;
+ ROS_INFO("tasks %i", tasks_.size());
+
+ while(!tasks_.empty()){
+ ROS_INFO("in tasks");
+ std::vector<std::pair<std::string, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> q_ets;
+ for (auto*r:robots_){
+ ROS_INFO("robot iteration");
+ auto itlow = tasks_.lower_bound (r->name()); // itlow points to b
+ auto itup = tasks_.upper_bound (r->name());
+ for (auto it=itlow; it!=itup; ++it){
+ tf2::Vector3 b_start_position = (*it).second.first;
+ for(auto& s_co : psi_->getObjects()){
+ if(!std::regex_match(s_co.first, match, item)) continue;
+ if(tf2::tf2Distance2(b_start_position, tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y,s_co.second.pose.position.z)) == 0) {
+ q_ets.push_back(std::pair<std::string, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>(r->name(), (*it).second.second));
+ tasks_.erase(it);
+ break;
+ }
+ }
+ }
+ }
+
+ ROS_INFO("tasks %i", tasks_.size());
+ ROS_INFO("jobs %i", q_ets.size());
+
+ // now execution should be possible, but with bocking
+ std::vector<std::thread> th;
+
+ while(!q_ets.empty()){
+ moveit_msgs::PlanningScene ps_m;
+ ps_m.is_diff = 0;
+ for (auto*r:robots_){
+ for (auto& s_qets : q_ets){
+ if(s_qets.first == r->name()){
+ if(!s_qets.second.front().solution.sub_trajectory.empty()){
+ moveit_msgs::RobotTrajectory* rt = (!s_qets.second.front().solution.sub_trajectory.empty()) ? &s_qets.second.front().solution.sub_trajectory.front().trajectory : nullptr;
+ moveit_msgs::PlanningScene* ps = (!s_qets.second.front().solution.sub_trajectory.empty()) ? &s_qets.second.front().solution.sub_trajectory.front().scene_diff : nullptr;
+
+ if (rt){
+ Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r);
+ // mr->mgi()->execute(*rt);
+ th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *rt, *ps));
+
+ // First find ID from panda to start with
+ std::regex rx_panda("panda_arm([0-9]+)");
+ std::smatch match;
+ std::regex_match(mr->name(), match, rx_panda);
+
+ // build panda link regex
+ std::stringstream ss;
+ ss << "panda_" << match[1] << "_.*";
+ std::regex rx_panda_links(ss.str());
+ std::regex rx_box("box.*");
+ std::regex rx_table("table.*");
+
+ if(ps){
+ for(int j = 0; j < ps->allowed_collision_matrix.entry_names.size(); j++ ){
+ if( mr->map().at("base") == ps->allowed_collision_matrix.entry_names[j]){
+ for(int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k]));
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (mr->map().at("base")== ps->allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+
+ if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
+ for(int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k]));
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (mr->map().at("base")== ps->allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+
+ if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_box)){
+ for(int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k]));
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (mr->map().at("base")== ps->allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+
+ if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_table)){
+ for(int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k]));
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ if (mr->map().at("base")== ps->allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+ }
+ merge_ps(ps_m, ps, mr);
+ }
+ s_qets.second.front().solution.sub_trajectory.erase(s_qets.second.front().solution.sub_trajectory.begin());
+ }
+ } else {s_qets.second.pop();};
+ }
+ }
+ }
+ for(int i = 0; i < th.size(); i++){
+ if(th[i].joinable()) th[i].join();
+ }
+ merge_acm(ps_m);
+ if(ps_m.is_diff) planning_scene_diff_publisher_->publish(ps_m);
+ }
+
+ }
}
+
+void Moveit_mediator::task_planner(){
+ /* There are 2 ways to interprete a Task
+ 1. A box position in acm is also the first entry in task,
+ 2. A box position in acm is not the first entry in task, in that case we can might try for each position
+ */
+ auto jq = job_reader_->robot_data();
+ auto cd = cuboid_reader_->cuboid_bin();
+
+ //std::vector<std::string> objs = {"bottle1", "bottle2"};
+
+ for (int i = 0; i < cd.size(); i ++){
+ std::stringstream ss;
+ ss << "box_" << i;
+
+ moveit_msgs::CollisionObject item;
+ item.id = ss.str();
+ item.header.frame_id = "world";
+ item.header.stamp = ros::Time::now();
+ item.primitives.resize(1);
+ item.primitives[0].type = item.primitives[0].BOX;
+ item.primitives[0].dimensions.resize(3);
+ item.primitives[0].dimensions[0] = cd[i].x_depth;
+ item.primitives[0].dimensions[1] = cd[i].y_width;
+ item.primitives[0].dimensions[2] = cd[i].z_heigth;
+
+ item.primitive_poses.resize(1);
+ item.primitive_poses[0].position.x = cd[i].Pose.position.x;
+ item.primitive_poses[0].position.y = cd[i].Pose.position.y;
+ item.primitive_poses[0].position.z = cd[i].Pose.position.z;
+ item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
+ item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
+ item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
+ item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
+ item.operation = item.ADD;
+
+ psi_->applyCollisionObject(item);
+ acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
+
+ // Could also safe id's somewhere
+ }
+
+ std::regex item("box_([0-9]+)");
+ std::smatch match;
+
+ // circular buffer is job-q
+ while(!jq.empty()){
+ for(auto& s_co : psi_->getObjects()){
+ if(!std::regex_match(s_co.first, match, item)) continue;
+ std::pair<std::string, job_data> temp = jq.front();
+ ROS_INFO("1. job entry %f %f %f", temp.second.jobs_.front().getOrigin().getX(), temp.second.jobs_.front().getOrigin().getY(), temp.second.jobs_.front().getOrigin().getZ());
+ ROS_INFO("object position %f %f %f", s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z);
+ if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z )) == 0) {
+ std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot;
+ // get rob by name
+ Moveit_robot* mr;
+ for(auto* r: robots_) if (r->name() == temp.first) mr = dynamic_cast<Moveit_robot*>(r);
+
+ // loop jobs
+ for (int k = 1; k < temp.second.jobs_.size(); k++){
+ moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(s_co.first), temp.second.jobs_[k]);
+ if(mgt.plan(1)) {
+ moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
+ mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
+ tasks_per_robot.push(e);
+
+ moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first);
+ temp_co.operation = temp_co.MOVE;
+ temp_co.pose.position.x = temp.second.jobs_[k].getOrigin().getX();
+ temp_co.pose.position.y = temp.second.jobs_[k].getOrigin().getY();
+ temp_co.pose.position.z = temp.second.jobs_[k].getOrigin().getZ();
+ temp_co.pose.orientation.x = temp.second.jobs_[k].getRotation().getX();
+ temp_co.pose.orientation.y = temp.second.jobs_[k].getRotation().getY();
+ temp_co.pose.orientation.z = temp.second.jobs_[k].getRotation().getZ();
+ temp_co.pose.orientation.w = temp.second.jobs_[k].getRotation().getW();
+ psi_->applyCollisionObject(temp_co);
+ }
+ }
+ std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> pair(temp.second.jobs_.front().getOrigin(), tasks_per_robot);
+ tasks_.insert(std::pair<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>(mr->name(), pair));
+ jq.pop_front();
+ } else {jq.push_back(temp);}
+ }
+ }
+
+ // clean up
+ for (int i = 0; i < cd.size(); i ++){
+ std::stringstream ss;
+ ss << "box_" << i;
+
+ moveit_msgs::CollisionObject item;
+ item.id = ss.str();
+ item.header.frame_id = "world";
+ item.header.stamp = ros::Time::now();
+ item.primitives.resize(1);
+ item.primitives[0].type = item.primitives[0].BOX;
+ item.primitives[0].dimensions.resize(3);
+ item.primitives[0].dimensions[0] = cd[i].x_depth;
+ item.primitives[0].dimensions[1] = cd[i].y_width;
+ item.primitives[0].dimensions[2] = cd[i].z_heigth;
+
+ item.pose.position.x = cd[i].Pose.position.x;
+ item.pose.position.y = cd[i].Pose.position.y;
+ item.pose.position.z = cd[i].Pose.position.z;
+ item.pose.orientation.x = cd[i].Pose.orientation.x;
+ item.pose.orientation.y = cd[i].Pose.orientation.y;
+ item.pose.orientation.z = cd[i].Pose.orientation.z;
+ item.pose.orientation.w = cd[i].Pose.orientation.w;
+ item.operation = item.MOVE;
+
+ psi_->applyCollisionObject(item);
+ // Could also safe id's somewhere
+ }
+
+}
+
+
void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
acmt.entry_values.resize(acm_.size());
@@ -831,7 +1084,7 @@ moveit::task_constructor::Task Moveit_mediator::create_Task(Moveit_robot* mr, mo
p.header.frame_id = "world";
p.pose.position.x = target.getOrigin().getX();
p.pose.position.y = target.getOrigin().getY();
- p.pose.position.z = 0.9355f;
+ p.pose.position.z = target.getOrigin().getZ();
p.pose.orientation.x = target.getRotation().getX();
p.pose.orientation.y = target.getRotation().getY();
p.pose.orientation.z = target.getRotation().getZ();
@@ -1013,7 +1266,10 @@ Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> object
, psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
, mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
, planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
- , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description")){
+ , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
+ , job_reader_(std::make_unique<Job_reader>(nh_))
+ , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
+
if (planning_scene_monitor_->getPlanningScene()){
planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
diff --git a/src/reader/job_reader.cpp b/src/reader/job_reader.cpp
index fd7955d..07d081f 100644
--- a/src/reader/job_reader.cpp
+++ b/src/reader/job_reader.cpp
@@ -1,14 +1,22 @@
#include "reader/job_reader.h"
+#include <queue>
+#include "boost/circular_buffer.hpp"
void Job_reader::read(){
ROS_INFO("--- JOB_READER ---");
XmlRpc::XmlRpcValue resources;
- nh_->getParam("/task/groups", resources);
+ nh_->getParam("/tasks/groups", resources);
+
+ int count =0;
+ for (int i = 0; i < resources.size(); i++){
+ count+= resources[i]["jobs"].size();
+ }
+
+ boost::circular_buffer<std::pair<std::string, job_data>> cb_jd(count);
for (int i = 0; i < resources.size(); i++){
std::string robot = resources[i]["name"];
//nh_->getParam("/tasks/groups/" + robot, jobs);
- std::vector<job_data> v_jd;
for (int j = 0; j < resources[i]["jobs"].size(); j++){
ROS_INFO("--- %s --- JOB_%i ---", robot.c_str(), j);
job_data jd;
@@ -31,8 +39,17 @@ void Job_reader::read(){
ROS_INFO("=> Grab: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
jd.jobs_.push_back(tf2::Transform(rot, pos));
}
- v_jd.push_back(jd);
+ cb_jd.push_back(std::pair<std::string, job_data>(robot, jd));
}
- job_data_.insert(std::pair<std::string, std::vector<job_data>>(robot, v_jd));
+ }
+
+ set_job_data(cb_jd);
+
+
+ // validate
+ for(auto& s_jd : job_data_){
+ for(auto& pose : s_jd.second.jobs_) {
+ ROS_INFO("%s %f %f %f %f %f %f %f", s_jd.first.c_str(), pose.getOrigin().getX(), pose.getOrigin().getY(), pose.getOrigin().getZ(), pose.getRotation().getX(), pose.getRotation().getY(), pose.getRotation().getZ(), pose.getRotation().getW());
+ }
}
}
\ No newline at end of file
--
GitLab