Skip to content
Snippets Groups Projects
Commit 0ec453e7 authored by KingMaZito's avatar KingMaZito
Browse files

Groot now in parallel

parent 67123e00
Branches
No related tags found
No related merge requests found
...@@ -23,13 +23,13 @@ class Execution : public BT::StatefulActionNode{ ...@@ -23,13 +23,13 @@ class Execution : public BT::StatefulActionNode{
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_; moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_;
Moveit_robot* mr_reference_; Moveit_robot* mr_reference_;
ros::Publisher* publisher_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_; std::vector<moveit_task_constructor_msgs::SubTrajectory>::iterator it_;
public: public:
Execution(const std::string& name, const BT::NodeConfiguration& config); 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 {}; } inline static BT::PortsList providedPorts() { return {}; }
......
...@@ -77,7 +77,7 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -77,7 +77,7 @@ class Moveit_mediator : public Abstract_mediator{
std::map<std::string, std::vector<uint8_t>> acm_; 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::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{ ...@@ -95,18 +95,19 @@ class Moveit_mediator : public Abstract_mediator{
void status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status); 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 merge_acm(moveit_msgs::PlanningScene& in);
void task_planner(); void task_planner();
void publish_tables(); 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 parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt);
void manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScene& ps);
//void parallel_exec(); //void parallel_exec();
moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); 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 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;}; inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;};
......
...@@ -22,9 +22,8 @@ BT::NodeStatus Execution::tick() { ...@@ -22,9 +22,8 @@ BT::NodeStatus Execution::tick() {
BT::NodeStatus Execution::onStart(){ BT::NodeStatus Execution::onStart(){
if (it_ != ets_.solution.sub_trajectory.end()){ if (it_ != ets_.solution.sub_trajectory.end()){
executions_->insert_or_assign(mr_reference_->name(), it_->trajectory); std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
ROS_INFO("%i", it_->trajectory.joint_trajectory.points.size()); executions_->insert_or_assign(mr_reference_->name(), pair);
ROS_INFO("%i", it_->trajectory.multi_dof_joint_trajectory.points.size());
it_++; it_++;
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} else { } else {
...@@ -36,9 +35,8 @@ BT::NodeStatus Execution::onStart(){ ...@@ -36,9 +35,8 @@ BT::NodeStatus Execution::onStart(){
// this method until it return something different from RUNNING // this method until it return something different from RUNNING
BT::NodeStatus Execution::onRunning(){ BT::NodeStatus Execution::onRunning(){
if (it_ != ets_.solution.sub_trajectory.end()){ if (it_ != ets_.solution.sub_trajectory.end()){
executions_->insert_or_assign(mr_reference_->name(), it_->trajectory); std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
ROS_INFO("%i", it_->trajectory.joint_trajectory.points.size()); executions_->insert_or_assign(mr_reference_->name(), pair);
ROS_INFO("%i", it_->trajectory.multi_dof_joint_trajectory.points.size());
it_++; it_++;
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} else { } else {
...@@ -49,7 +47,7 @@ BT::NodeStatus Execution::onRunning(){ ...@@ -49,7 +47,7 @@ BT::NodeStatus Execution::onRunning(){
// callback to execute if the action was aborted by another node // callback to execute if the action was aborted by another node
void Execution::onHalted(){} 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; mr_reference_= mr_reference;
publisher_reference_ = publisher_reference; publisher_reference_ = publisher_reference;
ets_ = ets; ets_ = ets;
......
...@@ -555,55 +555,9 @@ void Moveit_mediator::setup_task(){ ...@@ -555,55 +555,9 @@ void Moveit_mediator::setup_task(){
} }
*/ */
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()); void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScene& ps){
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));
// First find ID from panda to start with // First find ID from panda to start with
std::regex rx_panda("panda_arm([0-9]+)"); std::regex rx_panda("panda_arm([0-9]+)");
std::smatch match; std::smatch match;
...@@ -616,97 +570,125 @@ void Moveit_mediator::setup_task(){ ...@@ -616,97 +570,125 @@ void Moveit_mediator::setup_task(){
std::regex rx_box("box.*"); std::regex rx_box("box.*");
std::regex rx_table("table.*"); std::regex rx_table("table.*");
if(ps){ // Iterate task collsion matrix
for(int j = 0; j < ps->allowed_collision_matrix.entry_names.size(); j++ ){ 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]){ 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++){ //In case an entry matches an robot spezific link
int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k])); ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
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]; for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
} // For that specific entry, loop over values
if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){ int distance = std::distance(acm_.begin(),acm_.find(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[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)){ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
for(int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
int distance = std::distance(acm_.begin(),acm_.find(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[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_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)){
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]){
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];
} }
} }
} }
if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_box)){ 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++){ //In case an entry matches an robot spezific link
int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k])); ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[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)){
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)){
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]){
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];
} }
} }
} }
if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_table)){ 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++){ //In case an entry matches an robot spezific link
int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k])); ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[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_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_panda_links)){
} ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
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];
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[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_ps(ps_m, ps, mr);
} }
s_qets.second.front().solution.sub_trajectory.erase(s_qets.second.front().solution.sub_trajectory.begin());
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];
} }
} 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();
} }
merge_acm(ps_m);
if(ps_m.is_diff) planning_scene_diff_publisher_->publish(ps_m);
} }
} }
} }
...@@ -751,6 +733,9 @@ void Moveit_mediator::task_planner(){ ...@@ -751,6 +733,9 @@ void Moveit_mediator::task_planner(){
// Could also safe id's somewhere // 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::regex item("box_([0-9]+)");
std::smatch match; std::smatch match;
...@@ -775,7 +760,6 @@ void Moveit_mediator::task_planner(){ ...@@ -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("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); 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) { 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; std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> bt_list;
Moveit_robot* mr; Moveit_robot* mr;
...@@ -787,7 +771,6 @@ void Moveit_mediator::task_planner(){ ...@@ -787,7 +771,6 @@ void Moveit_mediator::task_planner(){
if(mgt.plan(1)) { if(mgt.plan(1)) {
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e; moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection()); mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
tasks_per_robot.push(e);
bt_list.push_back(e); bt_list.push_back(e);
moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first); moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first);
...@@ -845,8 +828,6 @@ void Moveit_mediator::task_planner(){ ...@@ -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); 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})); 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(); jq.pop_front();
} else {jq.push_back(temp);} } else {jq.push_back(temp);}
} }
...@@ -909,21 +890,26 @@ void Moveit_mediator::task_planner(){ ...@@ -909,21 +890,26 @@ void Moveit_mediator::task_planner(){
status = tree.tickRoot(); status = tree.tickRoot();
std::vector<std::thread> th; std::vector<std::thread> th;
moveit_msgs::PlanningScene ps_m;
for(auto& exec : executions_){ for(auto& exec : executions_){
for (int i = 0; i < robots_.size(); i++){ for (int i = 0; i < robots_.size(); i++){
if (exec.first == robots_[i]->name()){ if (exec.first == robots_[i]->name()){
auto mr = dynamic_cast<Moveit_robot*>(robots_[i]); 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){ for(auto& t : th){
if(t.joinable()) t.join(); 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){ ...@@ -971,8 +957,6 @@ void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
} }
ps_m.allowed_collision_matrix = acmt; ps_m.allowed_collision_matrix = acmt;
ROS_INFO("broken after merge");
} }
void Moveit_mediator::status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status){ 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 ...@@ -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 // get full mr link list
std::vector<std::string> links; std::vector<std::string> links;
for (auto link : mr->mgi()->getLinkNames())links.push_back(link); 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 ...@@ -991,33 +975,33 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla
links.push_back(mr->map()["base"]); links.push_back(mr->map()["base"]);
for (auto ao : in->robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao); 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.robot_state.is_diff) out.robot_state.is_diff = in.robot_state.is_diff;
if (in->is_diff) out.is_diff = in->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_state.joint_state.header = in.robot_state.joint_state.header;
out.robot_model_name = "panda"; out.robot_model_name = "panda";
for (auto link : links) { for (auto link : links) {
for(int i = 0; i < in->robot_state.joint_state.name.size(); 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()){ 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.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.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]); out.robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
ROS_INFO("check"); ROS_INFO("check");
} }
} }
for(int i = 0; i < in->link_padding.size(); i++){ for(int i = 0; i < in.link_padding.size(); i++){
if(link.c_str() == in->link_padding[i].link_name.c_str()){ if(link.c_str() == in.link_padding[i].link_name.c_str()){
out.link_padding.push_back(in->link_padding[i]); out.link_padding.push_back(in.link_padding[i]);
} }
} }
for(int i = 0; i < in->link_scale.size(); i++){ for(int i = 0; i < in.link_scale.size(); i++){
if(link.c_str() == in->link_scale[i].link_name.c_str()){ if(link.c_str() == in.link_scale[i].link_name.c_str()){
out.link_scale.push_back(in->link_scale[i]); 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 ...@@ -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) 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) : Abstract_mediator(objects, pub), nh_(nh)
, sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>()) , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment