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

...

parent eec15ca6
Branches
No related tags found
No related merge requests found
Pipeline #15017 failed
/home/matteo/ws_panda/devel/./cmake.lock 42 /home/matteo/ws_panda/devel/./cmake.lock 42
/home/matteo/reachability/devel/./cmake.lock 26396 /home/matteo/reachability/devel/./cmake.lock 26448
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
...@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{
void build_wings(std::bitset<3>& wing,int& robot) override; void build_wings(std::bitset<3>& wing,int& robot) override;
void set_wings(std::vector<std::vector<wing_BP>>& wbp) override; void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
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 publish_tables(); void publish_tables();
void load_robot_description(); void load_robot_description();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
......
...@@ -19,8 +19,11 @@ class Moveit_robot : public Robot{ ...@@ -19,8 +19,11 @@ class Moveit_robot : public Robot{
std::stringstream hand_n, ik_frame_n, name_n; std::stringstream hand_n, ik_frame_n, name_n;
hand_n << "hand_" << name.back(); hand_n << "hand_" << name.back();
ik_frame_n << "panda_" << name.back() << "_link8"; ik_frame_n << "panda_" << name.back() << "_link8";
auto mgi_hand = moveit::planning_interface::MoveGroupInterface(hand_n.str());
map_.insert(std::make_pair<std::string, std::string>("right_finger", mgi_hand.getLinkNames()[0].c_str()));
map_.insert(std::make_pair<std::string, std::string>("left_finger", mgi_hand.getLinkNames()[1].c_str()));
map_.insert(std::make_pair<std::string, std::string>("hand_link", mgi_hand.getLinkNames()[2].c_str()));
map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str())); map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str())); map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str())); map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));
......
...@@ -240,6 +240,7 @@ void Moveit_mediator::setup_task(){ ...@@ -240,6 +240,7 @@ void Moveit_mediator::setup_task(){
moveit_msgs::RobotTrajectory* t2 = nullptr; moveit_msgs::RobotTrajectory* t2 = nullptr;
moveit_msgs::PlanningScene* ps1 = nullptr; moveit_msgs::PlanningScene* ps1 = nullptr;
moveit_msgs::PlanningScene* ps2 = nullptr; moveit_msgs::PlanningScene* ps2 = nullptr;
moveit_msgs::PlanningScene ps_m;
if (i < tasks[0].front().solution.sub_trajectory.size()){ if (i < tasks[0].front().solution.sub_trajectory.size()){
t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory; t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
...@@ -325,6 +326,7 @@ void Moveit_mediator::setup_task(){ ...@@ -325,6 +326,7 @@ void Moveit_mediator::setup_task(){
ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff; ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
ROS_INFO("[Robot 2] acm"); ROS_INFO("[Robot 2] acm");
ROS_INFO("default entry names"); ROS_INFO("default entry names");
for (auto den : ps2->allowed_collision_matrix.default_entry_names){ for (auto den : ps2->allowed_collision_matrix.default_entry_names){
...@@ -406,15 +408,22 @@ void Moveit_mediator::setup_task(){ ...@@ -406,15 +408,22 @@ void Moveit_mediator::setup_task(){
if (t1){ if (t1){
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]); Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1)); th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
merge_ps(ps_m, ps1, mr);
} }
if (t2){ if (t2){
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]); Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]);
th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2)); } th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));
merge_ps(ps_m, ps2, mr);
}
for(int i = 0; i < th.size(); i++){ for(int i = 0; i < th.size(); i++){
th[i].join(); th[i].join();
} }
planning_scene_diff_publisher_.publish(ps_m);
} }
tasks[0].pop(); tasks[0].pop();
tasks[1].pop(); tasks[1].pop();
...@@ -423,7 +432,48 @@ void Moveit_mediator::setup_task(){ ...@@ -423,7 +432,48 @@ void Moveit_mediator::setup_task(){
} }
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);
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"]);
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]);
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_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( auto aco : in->robot_state.attached_collision_objects) { for( auto aco : in->robot_state.attached_collision_objects) {
out.robot_state.attached_collision_objects.push_back(aco); out.robot_state.attached_collision_objects.push_back(aco);
...@@ -445,8 +495,6 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::Plan ...@@ -445,8 +495,6 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::Plan
void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){ void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
mr.mgi()->execute(rt); mr.mgi()->execute(rt);
// planning_scene_diff_publisher_.publish(ps);
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment