diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 5fe0a469a7d78e0289e1efb6e99272ac49ac2943..f7c33dbe04d21804f6960432b598dd962adc43cb 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
/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_filter.so 79
diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h
index 1a6533f32316b0ede3b470ef15233dbaf42424a6..98eb16068243ed90721cb9c21b92b5645ef4a0d5 100644
--- a/src/mtc/include/impl/moveit_mediator.h
+++ b/src/mtc/include/impl/moveit_mediator.h
@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{
void build_wings(std::bitset<3>& wing,int& robot) 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 load_robot_description();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h
index 178b3f67b100ed1cfbeb5309deb75230a17ca7aa..f05adb689106877b51b553587b63dab427f4bf69 100644
--- a/src/mtc/include/impl/moveit_robot.h
+++ b/src/mtc/include/impl/moveit_robot.h
@@ -19,8 +19,11 @@ class Moveit_robot : public Robot{
std::stringstream hand_n, ik_frame_n, name_n;
hand_n << "hand_" << name.back();
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>("hand_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index c726da371f8c71f7422a0efa20e9b6b179edd619..ffe848a747a866a022359200d1402665ff27887e 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -240,6 +240,7 @@ void Moveit_mediator::setup_task(){
moveit_msgs::RobotTrajectory* t2 = nullptr;
moveit_msgs::PlanningScene* ps1 = nullptr;
moveit_msgs::PlanningScene* ps2 = nullptr;
+ moveit_msgs::PlanningScene ps_m;
if (i < tasks[0].front().solution.sub_trajectory.size()){
t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
@@ -324,6 +325,7 @@ void Moveit_mediator::setup_task(){
t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory;
ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
+
ROS_INFO("[Robot 2] acm");
ROS_INFO("default entry names");
@@ -406,15 +408,22 @@ void Moveit_mediator::setup_task(){
if (t1){
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
+ merge_ps(ps_m, ps1, mr);
+
+
}
if (t2){
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++){
th[i].join();
}
+ planning_scene_diff_publisher_.publish(ps_m);
+
}
tasks[0].pop();
tasks[1].pop();
@@ -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) {
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
void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
mr.mgi()->execute(rt);
- // planning_scene_diff_publisher_.publish(ps);
-
}