diff --git a/include/bt/execution.h b/include/bt/execution.h
index 913d60c8b2b5a44e9aa693b6a546c3e75d258fba..e46654f2ed6bd1b4a8eb53475adfb3f0543ab716 100644
--- a/include/bt/execution.h
+++ b/include/bt/execution.h
@@ -23,13 +23,13 @@ class Execution : public BT::StatefulActionNode{
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_;
Moveit_robot* mr_reference_;
ros::Publisher* publisher_reference_;
- std::map<std::string, moveit_msgs::RobotTrajectory>* executions_;
+ std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions_;
std::vector<moveit_task_constructor_msgs::SubTrajectory>::iterator it_;
public:
Execution(const std::string& name, const BT::NodeConfiguration& config);
- void init(std::map<std::string, moveit_msgs::RobotTrajectory>* executions_, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets);
+ void init(std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions_, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets);
inline static BT::PortsList providedPorts() { return {}; }
diff --git a/include/impl/moveit_mediator.h b/include/impl/moveit_mediator.h
index fa37d5970ec5b3c8b4c8a5167c97ea292ab61dcd..680e700764a1c77ae69dbf8be2b440eb545a395c 100644
--- a/include/impl/moveit_mediator.h
+++ b/include/impl/moveit_mediator.h
@@ -77,7 +77,7 @@ class Moveit_mediator : public Abstract_mediator{
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_;
- std::map<std::string, moveit_msgs::RobotTrajectory> executions_;
+ std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>> executions_;
@@ -95,18 +95,19 @@ class Moveit_mediator : public Abstract_mediator{
void status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status);
- void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
+ 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);
+ void manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScene& ps);
+
//void parallel_exec();
moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
- inline std::map<std::string, moveit_msgs::RobotTrajectory>& executions(){return executions_;};
+ inline std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>& executions(){return executions_;};
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/src/bt/execution.cpp b/src/bt/execution.cpp
index e502f162eab9be1a4e4974399f4f7e4cf88723ab..ad9eccec6594b295149cf52a1236ce82fff5b79e 100644
--- a/src/bt/execution.cpp
+++ b/src/bt/execution.cpp
@@ -22,9 +22,8 @@ BT::NodeStatus Execution::tick() {
BT::NodeStatus Execution::onStart(){
if (it_ != ets_.solution.sub_trajectory.end()){
- executions_->insert_or_assign(mr_reference_->name(), it_->trajectory);
- ROS_INFO("%i", it_->trajectory.joint_trajectory.points.size());
- ROS_INFO("%i", it_->trajectory.multi_dof_joint_trajectory.points.size());
+ std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
+ executions_->insert_or_assign(mr_reference_->name(), pair);
it_++;
return BT::NodeStatus::RUNNING;
} else {
@@ -36,9 +35,8 @@ BT::NodeStatus Execution::onStart(){
// this method until it return something different from RUNNING
BT::NodeStatus Execution::onRunning(){
if (it_ != ets_.solution.sub_trajectory.end()){
- executions_->insert_or_assign(mr_reference_->name(), it_->trajectory);
- ROS_INFO("%i", it_->trajectory.joint_trajectory.points.size());
- ROS_INFO("%i", it_->trajectory.multi_dof_joint_trajectory.points.size());
+ std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
+ executions_->insert_or_assign(mr_reference_->name(), pair);
it_++;
return BT::NodeStatus::RUNNING;
} else {
@@ -49,7 +47,7 @@ BT::NodeStatus Execution::onRunning(){
// callback to execute if the action was aborted by another node
void Execution::onHalted(){}
-void Execution::init(std::map<std::string, moveit_msgs::RobotTrajectory>* executions ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) {
+void Execution::init(std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) {
mr_reference_= mr_reference;
publisher_reference_ = publisher_reference;
ets_ = ets;
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index ea29805702cefe929a54ca6128fb26edc50c229b..613251e5ef655fb62b644c7e5b35a368bb5afec3 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -555,158 +555,140 @@ void Moveit_mediator::setup_task(){
}
*/
-
+}
+void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScene& 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.*");
+
+ // Iterate task collsion matrix
+ 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]){
+ //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 < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ // For that specific entry, loop over values
+ 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)){
+ ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+ 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)){
+ ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+ acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
- 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;
- }
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
+ ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+ 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]){
+ ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+ acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
}
- ROS_INFO("tasks %i", tasks_.size());
- ROS_INFO("jobs %i", q_ets.size());
+ if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
+ //In case an entry matches an robot spezific link
+ ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
+ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ //For that specific entry, loop over values
+ int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
- // now execution should be possible, but with bocking
- std::vector<std::thread> th;
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+ acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
- 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));
-
- // 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();};
- }
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
+ ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+ 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)){
+ ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+ 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]){
+ ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+ acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
- for(int i = 0; i < th.size(); i++){
- if(th[i].joinable()) th[i].join();
+ }
+
+ if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_box)){
+ //In case an entry matches an robot spezific link
+ ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
+ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ //For that specific entry, loop over values
+ 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)){
+ ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+ 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)){
+ ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+ 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)){
+ ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+ 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]){
+ ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+ acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
}
- merge_acm(ps_m);
- if(ps_m.is_diff) planning_scene_diff_publisher_->publish(ps_m);
}
- }
+ if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_table)){
+ //In case an entry matches an robot spezific link
+ ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j], j);
+ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ //For that specific entry, loop over values
+ 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)){
+ ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
+ 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)){
+ ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
+ 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)){
+ ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
+ 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]){
+ ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
+ acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+ }
}
@@ -750,6 +732,9 @@ void Moveit_mediator::task_planner(){
// Could also safe id's somewhere
}
+
+ // Setup shared ACM
+ for(auto& a: acm_) a.second.resize(acm_.size(), 0);
std::regex item("box_([0-9]+)");
std::smatch match;
@@ -775,7 +760,6 @@ void Moveit_mediator::task_planner(){
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;
std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> bt_list;
Moveit_robot* mr;
@@ -787,7 +771,6 @@ void Moveit_mediator::task_planner(){
if(mgt.plan(1)) {
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
- tasks_per_robot.push(e);
bt_list.push_back(e);
moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first);
@@ -845,8 +828,6 @@ void Moveit_mediator::task_planner(){
std::tuple<const std::string&, tf2::Vector3&, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>&> tuple(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list);
task_but_different.insert(std::pair<std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>>(mr->name(),{tuple}));
}
-
- tasks_.insert(std::pair<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>(mr->name(), {temp.second.jobs_.front().getOrigin(), tasks_per_robot}));
jq.pop_front();
} else {jq.push_back(temp);}
}
@@ -909,21 +890,26 @@ void Moveit_mediator::task_planner(){
status = tree.tickRoot();
std::vector<std::thread> th;
+ moveit_msgs::PlanningScene ps_m;
for(auto& exec : executions_){
for (int i = 0; i < robots_.size(); i++){
if (exec.first == robots_[i]->name()){
auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
- th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), exec.second));
+ th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), exec.second.first));
+ //mr->mgi()->execute(exec.second.first);
+ manipulate_acm(mr, exec.second.second);
+ merge_ps(ps_m, exec.second.second, mr);
}
}
}
+
for(auto& t : th){
if(t.joinable()) t.join();
+ merge_acm(ps_m);
+ planning_scene_diff_publisher_->publish(ps_m);
}
-
-
}
@@ -971,8 +957,6 @@ void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
}
ps_m.allowed_collision_matrix = acmt;
- ROS_INFO("broken after merge");
-
}
void Moveit_mediator::status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status){
@@ -981,7 +965,7 @@ void Moveit_mediator::status_callback(const actionlib_msgs::GoalStatusArray::Con
}
}
-void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){
+void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, Moveit_robot* mr){
// get full mr link list
std::vector<std::string> links;
for (auto link : mr->mgi()->getLinkNames())links.push_back(link);
@@ -991,33 +975,33 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla
links.push_back(mr->map()["base"]);
- for (auto ao : in->robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
- if (in->robot_state.is_diff) out.robot_state.is_diff = in->robot_state.is_diff;
- if (in->is_diff) out.is_diff = in->is_diff;
- out.robot_state.joint_state.header = in->robot_state.joint_state.header;
+ for (auto ao : in.robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
+ if (in.robot_state.is_diff) out.robot_state.is_diff = in.robot_state.is_diff;
+ if (in.is_diff) out.is_diff = in.is_diff;
+ out.robot_state.joint_state.header = in.robot_state.joint_state.header;
out.robot_model_name = "panda";
for (auto link : links) {
- for(int i = 0; i < in->robot_state.joint_state.name.size(); i++){
- if(link.c_str() == in->robot_state.joint_state.name[i].c_str()){
- out.robot_state.joint_state.effort.push_back(in->robot_state.joint_state.effort[i]);
- out.robot_state.joint_state.position.push_back(in->robot_state.joint_state.position[i]);
- out.robot_state.joint_state.velocity.push_back(in->robot_state.joint_state.velocity[i]);
+ for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){
+ if(link.c_str() == in.robot_state.joint_state.name[i].c_str()){
+ out.robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
+ out.robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
+ out.robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
ROS_INFO("check");
}
}
- for(int i = 0; i < in->link_padding.size(); i++){
- if(link.c_str() == in->link_padding[i].link_name.c_str()){
- out.link_padding.push_back(in->link_padding[i]);
+ for(int i = 0; i < in.link_padding.size(); i++){
+ if(link.c_str() == in.link_padding[i].link_name.c_str()){
+ out.link_padding.push_back(in.link_padding[i]);
}
}
- for(int i = 0; i < in->link_scale.size(); i++){
- if(link.c_str() == in->link_scale[i].link_name.c_str()){
- out.link_scale.push_back(in->link_scale[i]);
+ for(int i = 0; i < in.link_scale.size(); i++){
+ if(link.c_str() == in.link_scale[i].link_name.c_str()){
+ out.link_scale.push_back(in.link_scale[i]);
}
}
}
@@ -1325,106 +1309,6 @@ moveit::task_constructor::Task Moveit_mediator::create_Task(Moveit_robot* mr, mo
}
-void Moveit_mediator::rewrite_task_template(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));
- // unique ids cartesian, samplimg, interpolation, ready, hand_open, pick_up
-
- std::string support_surface1 = "nichts";
- std::string support_surface2 = "nichts";
-
- XmlRpc::XmlRpcValue task;
- nh_->getParam("task/stages", task);
-
-
-
- for (Abstract_robot* ar : robots_){
- std::string str;
- if(ar->check_single_object_collision(t, str)) support_surface1 = str;
- if(ar->check_single_object_collision(target, str)) support_surface2= str;
- }
-
- Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r);
-
- ROS_INFO("%f %f %f source", t.getOrigin().getX(), t.getOrigin().getY(), t.getOrigin().getZ());
-
- ROS_INFO("surface1 %s", support_surface1.c_str());
- ROS_INFO("surface2 %s", support_surface2.c_str());
-
-
- nh_->setParam("/task/properties/group", r->name());
- nh_->setParam("/task/properties/eef", mr->map()["eef"]);
- nh_->setParam("/task/properties/hand_grasping_frame", mr->map()["hand_grasping_frame"]);
- nh_->setParam("/task/properties/ik_frame", mr->map()["ik_frame"]);
- nh_->setParam("/task/properties/hand", mr->map()["hand"]);
-
- XmlRpc::XmlRpcValue a, b, c, d, e, h;
- a["group"] = mr->map()["hand"];
- e["joint_model_group_name"] = mr->map()["hand"];
-
- b = task[4]["stages"];
- b[1]["stage"]["properties"]["object"] = source.id;
- b[3]["properties"] = a;
- b[2]["set"]["allow_collisions"]["first"] = source.id;
- b[2]["set"]["allow_collisions"]["second"] = e;
-
- c = task[6]["stages"];
-
- c[0]["set"]["allow_collisions"]["first"] = source.id;
- c[0]["set"]["allow_collisions"]["second"] = support_surface2;
- c[3]["properties"] = a;
- c[6]["properties"] = a;
- c[4]["set"]["allow_collisions"]["first"] = source.id;
- c[4]["set"]["allow_collisions"]["second"] = e;
-
- c[2]["stage"]["set"]["pose"]["point"]["x"]= static_cast<double>(target.getOrigin().getX());
- c[2]["stage"]["set"]["pose"]["point"]["y"]= static_cast<double>(target.getOrigin().getY());
- c[2]["stage"]["set"]["pose"]["point"]["z"]= static_cast<double>(target.getOrigin().getZ());
-
-
- task[2]["properties"] = a;
-
- XmlRpc::XmlRpcValue connect, f, g;
- f[r->name()] = "sampling";
- g["source"] = "PARENT";
- connect["type"] = "Connect";
- connect["group_planner_vector"] = f;
- connect["propertiesConfigureInitFrom"] = g;
- task[5] = connect;
- task[3] = connect;
-
- a.clear();
- e.clear();
- a["link"] = mr->map()["ik_frame"];
- a["min_distance"] = 0.07;
- a["max_distance"] = 0.2;
- c[5]["properties"] = a;
- b[0]["properties"] = a;
- a["min_distance"] = 0.1;
- a["max_distance"] = 0.2;
- c[1]["properties"] = a;
-
- e["object"] = "bottle";
- e["link"] = mr->map()["ik_frame"];
- b[4]["set"]["attach_object"] = e;
- c[4]["set"]["detach_object"] = e;
- c[2]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"];
- c[5]["set"]["direction"]["vector"]["header"]["frame_id"] = mr->map()["ik_frame"];
- b[1]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"];
- b[6]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"];
- b[0]["set"]["direction"]["vector"]["header"]["frame_id"] = mr->map()["ik_frame"];
-
- b[5]["set"]["allow_collisions"]["first"] = source.id;
- b[5]["set"]["allow_collisions"]["second"] = support_surface1;
- b[7]["set"]["allow_collisions"]["first"] = source.id;
- b[7]["set"]["allow_collisions"]["second"] = support_surface1;
-
- task[6]["stages"] = c;
- task[4]["stages"] = b;
-
- nh_->setParam("task/stages", task);
-
-}
-
Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
: Abstract_mediator(objects, pub), nh_(nh)
, sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())