...

include/impl/moveit_mediator.h

@@ -76,6 +76,8 @@ class Moveit_mediator : public Abstract_mediator{
     std::unique_ptr<Cuboid_reader> cuboid_reader_;
 
     std::map<std::string, std::vector<uint8_t>> acm_;
+    std::map<std::string, std::vector<uint8_t>> rs_;
+
     std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_;
     std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>> executions_;
 

src/impl/moveit_mediator.cpp

@@ -29,10 +29,17 @@ void Moveit_mediator::connect_robots(Abstract_robot* robot) {
 	robots_.push_back(robot);
 	ROS_INFO("%s connected...", robot->name().c_str());
 
+
 	Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
 	acm_.insert(std::pair<std::string, std::vector<uint8_t>>(mr->map().at("base").c_str(), std::vector<uint8_t>()));
 	for(auto name: mr->mgi()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
 	for(auto name: mr->mgi_hand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
+
+	for (auto link : mr->mgi()->getLinkNames()) rs_.insert_or_assign(link, std::vector<std::uint8_t>());
+	rs_.insert_or_assign(mr->map()["left_finger"], std::vector<std::uint8_t>());
+	rs_.insert_or_assign(mr->map()["right_finger"], std::vector<std::uint8_t>());
+	rs_.insert_or_assign(mr->map()["hand_link"], std::vector<std::uint8_t>());
+	rs_.insert_or_assign(mr->map()["base"], std::vector<std::uint8_t>());
 }
 
 void Moveit_mediator::publish_tables(){
@@ -896,8 +903,8 @@ void Moveit_mediator::task_planner(){
 			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.first));
-					//mr->mgi()->execute(exec.second.first);
+					//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);
 				}
@@ -977,6 +984,13 @@ 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){
 	// get full mr link list 
+
+	for (auto ao : in.robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
+	out.robot_state.is_diff |= in.robot_state.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";
+
 	std::vector<std::string> links;
 	for (auto link : mr->mgi()->getLinkNames()) links.push_back(link);
 	links.push_back(mr->map()["left_finger"]);
@@ -984,33 +998,23 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla
 	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()){
+			if(link == in.robot_state.joint_state.name[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.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()){
+			if(link == in.link_padding[i].link_name){
 				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()){
+			if(link == in.link_scale[i].link_name){
 				out.link_scale.push_back(in.link_scale[i]);
 			}
 		}