Skip to content
Snippets Groups Projects
Commit 9bb472c0 authored by KingMaZito's avatar KingMaZito
Browse files

debug

parent 45366ecd
Branches
Tags
No related merge requests found
...@@ -28,22 +28,19 @@ void MGMediator::mergeACM(){ ...@@ -28,22 +28,19 @@ void MGMediator::mergeACM(){
} }
void MGMediator::parallelExec(moveit_task_constructor_msgs::SubTrajectory st, const std::string& robotId){ void MGMediator::parallelExec(moveit_task_constructor_msgs::SubTrajectory st, const std::string& robotId){
AbstractRobotDecorator* mr = nullptr;
{ {
mr= robots_.at(robotId).get();
ROS_INFO("parallelExec step 1"); ROS_INFO("parallelExec step 1");
std::unique_lock<std::mutex> lock(shared_mutex); std::unique_lock<std::mutex> lock(shared_mutex);
manipulateACM(st.scene_diff, robotId); //manipulateACM(st.scene_diff, robotId);
mergePS(st.scene_diff, robotId); //mergePS(st.scene_diff, robotId);
ROS_INFO("parallelExec step 2"); ROS_INFO("parallelExec step 2");
} }
AbstractRobotDecorator* mr = nullptr;
{
std::unique_lock<std::mutex> lock(shared_mutex);
mr= robots_.at(robotId).get();
ROS_INFO("parallelExec step 3");
}
mr->mgi()->execute(st.trajectory); mr->mgi()->execute(st.trajectory);
planning_scene_diff_publisher_->publish(st.scene_diff);
ROS_INFO("parallelExec step 4"); ROS_INFO("parallelExec step 4");
} }
...@@ -51,18 +48,55 @@ void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string ...@@ -51,18 +48,55 @@ void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string
// get full mr link list // get full mr link list
ROS_INFO("mergePS step 1"); ROS_INFO("mergePS step 1");
AbstractRobotDecorator* mr= robots_.at(robotId).get();
if (!ps_){ if (!ps_){
ps_ = std::make_shared<moveit_msgs::PlanningScene>(in); ps_ = std::make_shared<moveit_msgs::PlanningScene>();
ROS_INFO("mergePS step 2");
return; for (auto ao : in.robot_state.attached_collision_objects) ps_->robot_state.attached_collision_objects.push_back(ao);
ps_->robot_state.is_diff |= in.robot_state.is_diff;
ps_->is_diff |= in.is_diff;
ps_->robot_state.joint_state.header = in.robot_state.joint_state.header;
ps_->robot_model_name = "panda";
ROS_INFO("mergePS step 4");
std::vector<std::string> links;
for (auto link : mr->mgi()->getLinkNames()) links.push_back(link);
links.push_back(mr->map()["left_finger"]);
links.push_back(mr->map()["right_finger"]);
links.push_back(mr->map()["hand_link"]);
links.push_back(mr->map()["base"]);
ROS_INFO("mergePS step 5");
for (auto link : links) {
for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){
if(link == in.robot_state.joint_state.name[i]){
ps_->robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
ps_->robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
ps_->robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
} }
}
ROS_INFO("mergePS step 6");
AbstractRobotDecorator* mr = nullptr; for(int i = 0; i < in.link_padding.size(); i++){
{ if(link == in.link_padding[i].link_name){
std::unique_lock<std::mutex> lock(shared_mutex); ps_->link_padding.push_back(in.link_padding[i]);
mr= robots_.at(robotId).get(); }
}
ROS_INFO("mergePS step 7");
for(int i = 0; i < in.link_scale.size(); i++){
if(link == in.link_scale[i].link_name){
ps_->link_scale.push_back(in.link_scale[i]);
}
}
} }
ROS_INFO("mergePS step 8");
return;
}
ROS_INFO("mergePS step 3"); ROS_INFO("mergePS step 3");
for (auto ao : in.robot_state.attached_collision_objects) ps_->robot_state.attached_collision_objects.push_back(ao); for (auto ao : in.robot_state.attached_collision_objects) ps_->robot_state.attached_collision_objects.push_back(ao);
...@@ -179,16 +213,15 @@ void MGMediator::processSharedStructure() { ...@@ -179,16 +213,15 @@ void MGMediator::processSharedStructure() {
ROS_INFO("processSharedStructure step 6"); ROS_INFO("processSharedStructure step 6");
for(auto& t : th){ for(auto& t : th){
t.join(); t.join();
} }
ROS_INFO("processSharedStructure step 7"); ROS_INFO("processSharedStructure step 7");
mergeACM(); //mergeACM();
ROS_INFO("processSharedStructure step 8"); ROS_INFO("processSharedStructure step 8");
planning_scene_diff_publisher_->publish(ps_.get()); //planning_scene_diff_publisher_->publish(*ps_);
ps_ = nullptr;
// The lock is automatically released here when `lock` goes out of scope // The lock is automatically released here when `lock` goes out of scope
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment