From 9bb472c0fa251d128b75ff6d5884d2c226d5727c Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Wed, 26 Jul 2023 04:25:18 +0200
Subject: [PATCH] debug
---
src/mediator/mg_mediator.cpp | 71 ++++++++++++++++++++++++++----------
1 file changed, 52 insertions(+), 19 deletions(-)
diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp
index e4614f4..45f2db2 100644
--- a/src/mediator/mg_mediator.cpp
+++ b/src/mediator/mg_mediator.cpp
@@ -28,22 +28,19 @@ void MGMediator::mergeACM(){
}
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");
std::unique_lock<std::mutex> lock(shared_mutex);
- manipulateACM(st.scene_diff, robotId);
- mergePS(st.scene_diff, robotId);
+ //manipulateACM(st.scene_diff, robotId);
+ //mergePS(st.scene_diff, robotId);
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);
+ planning_scene_diff_publisher_->publish(st.scene_diff);
ROS_INFO("parallelExec step 4");
}
@@ -51,18 +48,55 @@ void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string
// get full mr link list
ROS_INFO("mergePS step 1");
+ AbstractRobotDecorator* mr= robots_.at(robotId).get();
if (!ps_){
- ps_ = std::make_shared<moveit_msgs::PlanningScene>(in);
- ROS_INFO("mergePS step 2");
+ ps_ = std::make_shared<moveit_msgs::PlanningScene>();
+
+ 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");
+
+ for(int i = 0; i < in.link_padding.size(); i++){
+ if(link == in.link_padding[i].link_name){
+ ps_->link_padding.push_back(in.link_padding[i]);
+ }
+ }
+
+ 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;
}
- AbstractRobotDecorator* mr = nullptr;
- {
- std::unique_lock<std::mutex> lock(shared_mutex);
- mr= robots_.at(robotId).get();
- }
ROS_INFO("mergePS step 3");
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() {
ROS_INFO("processSharedStructure step 6");
for(auto& t : th){
t.join();
-
}
ROS_INFO("processSharedStructure step 7");
- mergeACM();
+ //mergeACM();
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
--
GitLab