From 2b1e4ab06c8945546970ba86554c85a64300c7a4 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Mon, 17 Jul 2023 22:29:19 +0200
Subject: [PATCH] between MGs and Task Constructors

---
 include/mediator/mg_mediator.h             |  31 ++
 results/dummy/-2045918175/-2045918175.yaml |   6 +-
 src/mediator/mg_mediator.cpp               | 521 ++++++++++++++++++++-
 src/reader/cuboid_reader.cpp               |  15 +
 src/robot/ceti_robot.cpp                   |   2 +-
 src/robot/decorators/panda_decorator.cpp   |   2 +
 6 files changed, 557 insertions(+), 20 deletions(-)

diff --git a/include/mediator/mg_mediator.h b/include/mediator/mg_mediator.h
index 98eadbf..3c661f1 100644
--- a/include/mediator/mg_mediator.h
+++ b/include/mediator/mg_mediator.h
@@ -8,18 +8,47 @@
 #include <ccf/connection/MqttConnection.h>
 #include <ccf/controller/Controller.h>
 
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/stages/compute_ik.h>
+#include <moveit/task_constructor/stages/connect.h>
+#include <moveit/task_constructor/stages/current_state.h>
+#include <moveit/task_constructor/stages/generate_grasp_pose.h>
+#include <moveit/task_constructor/stages/generate_place_pose.h>
+#include <moveit/task_constructor/stages/generate_pose.h>
+#include <moveit/task_constructor/stages/modify_planning_scene.h>
+#include <moveit/task_constructor/stages/move_relative.h>
+#include <moveit/task_constructor/stages/move_to.h>
+#include <moveit/task_constructor/stages/predicate_filter.h>
+#include <moveit/task_constructor/solvers/cartesian_path.h>
+#include <moveit/task_constructor/solvers/pipeline_planner.h>
+#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
+#include <moveit/task_constructor/properties.h>
+#include <moveit/task_constructor/solvers/joint_interpolation.h>
+#include <moveit/task_constructor/solvers/planner_interface.h>
+#include <moveit/task_constructor/stage.h>
+#include <moveit/task_constructor/stages/fixed_state.h>
+
 
 //should be a mix between MG, bahvior tree, moveit task constructor
 class MGMediator: public AbstractMediator, virtual Controller{
     protected:
+    std::shared_ptr<moveit::core::RobotModel> robot_model_;
     std::vector<std::unique_ptr<Connection>> connections_;
 
     std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; //!< MoveItVisualTools
     std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor
 
+    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_; //!< Move Group Interface of the whole multi-cell
+    std::shared_ptr<ros::Publisher> planning_scene_diff_publisher_; //!< Publisher to manage PlanningScene diffs
+    std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_; //!< Moveit task Constructior simple planner
+    std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_; //!< Moveit task Constructior cartesian planner
+
     std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_; //!< PlanningSceneInteface to manage Scene Objects
     std::map<std::string, std::vector<uint8_t>> acm_; //!< shared allowed collision matrix between robots
     std::map<std::string, std::vector<uint8_t>> rs_; //!< shared robot state between all robots
+    std::shared_ptr<planning_scene::PlanningScene> ps_; //!< Shared Planning Scene
+
 
 
     std::string referenceRobot_; //!< Reference Robot
@@ -46,6 +75,8 @@ class MGMediator: public AbstractMediator, virtual Controller{
     void setPanel() override;
 
     void publishTables();
+    moveit::task_constructor::Task pickUp(const std::string& obj, const std::string& robotId);
+    moveit::task_constructor::Task place(const std::string& obj, const std::string& robotId, const std::string& target);
 
 };
 #endif
\ No newline at end of file
diff --git a/results/dummy/-2045918175/-2045918175.yaml b/results/dummy/-2045918175/-2045918175.yaml
index c82b215..98ff98e 100644
--- a/results/dummy/-2045918175/-2045918175.yaml
+++ b/results/dummy/-2045918175/-2045918175.yaml
@@ -20,8 +20,10 @@
 { 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
 { 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
 { 'id': 'table2_right_panel' , 'pos': { 'x': -0.000007 , 'y': 1.957498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, 
-{ 'id': 'blue1',  'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
-{ 'id': 'blue2',  'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.11, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
+{ 'id': 'A',  'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
+{ 'id': 'ABIN',  'type': 'OBSTACLE', 'pos': { 'x': -0.25, 'y': -0.65, 'z': 0.9355 },'size': { 'length': 0.0330, 'width': 0.066, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, 
+{ 'id': 'B',  'type': 'BOX', 'pos': { 'x': -0.3, 'y': 0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'g': 1 } }, 
+{ 'id': 'BBIN',  'type': 'OBSTACLE', 'pos': { 'x': -0.25, 'y': 1.7, 'z': 0.9355 },'size': { 'length': 0.033, 'width': 0.066, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'g': 1 } }, 
 { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
 { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, 
 ]}
\ No newline at end of file
diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp
index 7f70d13..7bd9d80 100644
--- a/src/mediator/mg_mediator.cpp
+++ b/src/mediator/mg_mediator.cpp
@@ -3,7 +3,65 @@
 MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh) 
 : AbstractMediator(nh)
 , Controller(*nh.get())
-{   
+, sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
+, cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
+, psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
+, mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
+, planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1))){
+
+    // MQTT HANDLE
+    addCallback("command/pickup", [&](const std::string& data) {
+        std::vector<std::pair<std::string, std::string>> pickup_command{std::make_pair("A", "panda_arm1"), std::make_pair("B", "panda_arm2") };
+        for (const auto& pair: pickup_command) {
+            moveit::task_constructor::Task mgt = pickUp(pair.first, pair.second);
+            if(mgt.plan(1)) {
+                //mgt.introspection().publishSolution(*mgt.solutions().front());
+                mgt.execute(*mgt.solutions().front());
+            }
+        }
+    });
+
+	addCallback("command/place", [&](const std::string& data) {
+        std::vector<std::tuple<std::string, std::string, std::string>> place_command{std::make_tuple("A", "panda_arm1", "target"), std::make_tuple("B", "panda_arm2", "target") };
+        for (const auto& tuple: place_command) {
+            moveit::task_constructor::Task mgt = place(std::get<0>(tuple), std::get<1>(tuple), std::get<2>(tuple));
+            if(mgt.plan(1)) {
+                //mgt.introspection().publishSolution(*mgt.solutions().front());
+                mgt.execute(*mgt.solutions().front());
+            }
+        }
+    });
+
+
+	moveit::task_constructor::Task mgt2 = place("A", "panda_arm1", "dafd");
+	if(mgt2.plan(1)) {
+		//mgt.introspection().publishSolution(*mgt.solutions().front());
+		mgt2.execute(*mgt2.solutions().front());
+	}
+
+
+    robot_model_loader::RobotModelLoaderPtr robot_model_loader;
+    robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
+    robot_model_ = robot_model_loader->getModel();
+
+    planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
+    visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>("world", "/rviz_visual_tools");
+    visual_tools_->loadMarkerPub();
+
+    visual_tools_->waitForMarkerPub();
+
+    visual_tools_->loadRemoteControl();
+    visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);
+    visual_tools_->trigger();
+
+
+    sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
+
+    // cartesian
+    cartesian_planner_->setMaxVelocityScaling(1.0);
+    cartesian_planner_->setMaxAccelerationScaling(1.0);
+    cartesian_planner_->setStepSize(.01);
+
     addConnection(std::make_shared<MqttConnection>("localhost:1883", "asdfadf"));
 }
 
@@ -11,9 +69,375 @@ void MGMediator::mediate(){
     setPanel();
     publishTables();
 
+	moveit::task_constructor::Task mgt1 = pickUp("A", "panda_arm1");
+	if(mgt1.plan(1)) {
+		//mgt.introspection().publishSolution(*mgt.solutions().front());
+		mgt1.execute(*mgt1.solutions().front());
+	}
+	 
+	
 
 };
 
+moveit::task_constructor::Task MGMediator::pickUp(const std::string& obj, const std::string& robotId){
+
+    AbstractRobotDecorator* mr = nullptr;
+    try{
+        mr= robots_.at(robotId).get();
+    } catch (std::out_of_range& oor){
+        ROS_INFO("Robot not found");
+        ros::shutdown();
+    }
+
+    moveit_msgs::CollisionObject source = psi_->getObjects().at(obj);
+    const std::string object = source.id;
+    moveit::task_constructor::Task task_;
+    tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
+	std::string support_surface1 = "nichts";
+
+	for (const auto& s_r : robots_){
+		std::string str;
+		std::bitset<3> panel_mask(7);
+        if(s_r.second->checkSingleObjectCollision(t, str, panel_mask)) support_surface1 = str;
+	}
+
+	ROS_INFO("ss1 %s", support_surface1.c_str());
+
+	std::string name = "Pick&Place";
+	task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
+	task_.loadRobotModel();
+	task_.setRobotModel(mr->mgi()->getRobotModel());
+
+	task_.setProperty("group", mr->name());
+	task_.setProperty("eef", mr->map()["eef_name"]);
+	task_.setProperty("hand", mr->map()["hand_group_name"]);
+	task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]);
+	task_.setProperty("ik_frame", mr->map()["hand_frame"]);
+
+	moveit::task_constructor::Stage* current_state_ptr = nullptr;  
+	{
+        auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
+        auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
+        applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
+            if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
+                comment = "object with id '" + object + "' is already attached and cannot be picked";
+                return false;
+            }
+            return true;
+        });
+
+        current_state_ptr = applicability_filter.get();
+            task_.add(std::move(applicability_filter));
+    }
+
+
+	{  // Open Hand
+		auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
+		stage->setGroup(mr->map()["eef_name"]);
+		stage->setGoal("open");
+		task_.add(std::move(stage));
+	}
+
+
+
+	{  // Move-to pre-grasp
+		auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
+		    "move to pick",  moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(),  sampling_planner_} });
+		stage->setTimeout(5.0);
+		stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
+		task_.add(std::move(stage));
+	}
+
+	moveit::task_constructor::Stage* attach_object_stage = nullptr;  // Forward attach_object_stage to place pose generator
+	{
+		auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
+		task_.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
+		grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
+
+
+		{	// Approach Obj
+			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner_);
+			stage->properties().set("marker_ns", "approach_object");
+			stage->properties().set("link", mr->map()["hand_frame"]);
+			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
+			stage->setMinMaxDistance(0.07, 0.2);
+
+			// Set hand forward direction
+			geometry_msgs::Vector3Stamped vec;
+			vec.header.frame_id = mr->map()["hand_frame"];
+			vec.vector.z = 1.0;
+			stage->setDirection(vec);
+			grasp->insert(std::move(stage));
+		}
+
+		{
+			// Sample grasp pose
+			auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
+			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
+			stage->properties().set("marker_ns", "grasp_pose");
+			stage->setPreGraspPose("open");
+			stage->setObject(object);
+			stage->setAngleDelta(M_PI / 2);
+			stage->setMonitoredStage(current_state_ptr);  // Hook into current state
+
+			// Compute IK
+			Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
+                                   Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
+                                   Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
+			Eigen::Translation3d trans(0.1f, 0, 0);
+			Eigen::Isometry3d ik = eigen * trans;
+
+			auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
+			wrapper->setMaxIKSolutions(8);
+			wrapper->setMinSolutionDistance(1.0);
+			wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
+			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
+			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
+			grasp->insert(std::move(wrapper));
+		}
+
+
+		{
+			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
+			stage->allowCollisions(
+			    object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
+			    true);
+			grasp->insert(std::move(stage));
+		}
+
+		{
+			auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
+			stage->setGroup(mr->map()["eef_name"]);
+			stage->setGoal("close");
+			grasp->insert(std::move(stage));
+		}
+
+		{	// Attach obj
+			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
+			stage->attachObject(object, mr->map()["hand_frame"]);
+			attach_object_stage = stage.get();
+			grasp->insert(std::move(stage));
+		}
+
+		{	// Allow Collision obj table
+			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
+			stage->allowCollisions({ object }, support_surface1, true);
+			grasp->insert(std::move(stage));
+		}
+
+		{
+			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner_);
+			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
+			stage->setMinMaxDistance(0.1, 0.2);
+			stage->setIKFrame(mr->map()["hand_frame"]);
+			stage->properties().set("marker_ns", "lift_object");
+
+			// Set upward direction
+			geometry_msgs::Vector3Stamped vec;
+			vec.header.frame_id = "world";
+			vec.vector.z = 1.0;
+			stage->setDirection(vec);
+			grasp->insert(std::move(stage));
+		}
+
+
+		{	// forbid collision
+			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
+			stage->allowCollisions({ object }, support_surface1, false);
+			grasp->insert(std::move(stage));
+		}
+
+		// Add grasp container to task
+		task_.add(std::move(grasp));
+	}
+
+    return task_;
+
+	
+}
+
+moveit::task_constructor::Task MGMediator::place(const std::string& obj, const std::string& robotId, const std::string& target){
+
+    AbstractRobotDecorator* mr = nullptr;
+    try{
+        mr= robots_.at(robotId).get();
+    } catch (std::out_of_range& oor){
+        ROS_INFO("Robot not found");
+        ros::shutdown();
+    }
+
+    const std::string object = obj;
+    moveit::task_constructor::Task task_;
+
+	// Just some value
+    tf2::Transform t(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(-0.3f, -0.6f, 0.9355f));
+	std::string support_surface2;
+
+	for (const auto& s_r : robots_){
+		std::string str;
+		std::bitset<3> panel_mask(7);
+        if(s_r.second->checkSingleObjectCollision(t, str, panel_mask)) support_surface2 = str;
+	}
+
+	ROS_INFO("ss1 %s", support_surface2.c_str());
+
+
+
+	std::string name = "Pick&Place";
+	task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
+	task_.loadRobotModel();
+	task_.setRobotModel(mr->mgi()->getRobotModel());
+
+	task_.setProperty("group", mr->name());
+	task_.setProperty("eef", mr->map()["eef_name"]);
+	task_.setProperty("hand", mr->map()["hand_group_name"]);
+	task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]);
+	task_.setProperty("ik_frame", mr->map()["hand_frame"]);
+
+	moveit::task_constructor::Stage* current_state_ptr = nullptr;  
+	{
+        auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
+        auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
+        applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
+            if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
+                comment = "object with id '" + object + "' is already attached and cannot be picked";
+				// psst, I changed the return values and it works... dont tell this mama
+                return true;
+            }
+            return false;
+        });
+
+        current_state_ptr = applicability_filter.get();
+            task_.add(std::move(applicability_filter));    
+	}
+
+	{
+		auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
+		    "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_ } });
+		stage->setTimeout(5.0);
+		stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
+		task_.add(std::move(stage));
+	}
+
+	{
+		auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object");
+		task_.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
+		place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" });
+
+		{
+		auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow cokbkmomsurface)");
+		stage->allowCollisions( {object} , support_surface2, true);
+		place->insert(std::move(stage));
+		}
+
+		{
+			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner_);
+			stage->properties().set("marker_ns", "lower_object");
+			stage->properties().set("link", mr->map()["hand_frame"]);
+			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
+			stage->setMinMaxDistance(.03, .13);
+
+			// Set downward direction
+			geometry_msgs::Vector3Stamped vec;
+			vec.header.frame_id = "world";
+			vec.vector.z = -1.0;
+			stage->setDirection(vec);
+			place->insert(std::move(stage));
+		}
+
+		{
+			// Generate Place Pose
+			auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose");
+			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" });
+			stage->properties().set("marker_ns", "place_pose");
+			stage->setObject(object);
+
+			// Set target pose
+			geometry_msgs::PoseStamped p;
+			p.header.frame_id = "world";
+			p.pose.position.x = t.getOrigin().getX();
+			p.pose.position.y = t.getOrigin().getY();
+			p.pose.position.z = t.getOrigin().getZ();
+			p.pose.orientation.x = t.getRotation().getX();
+			p.pose.orientation.y = t.getRotation().getY();
+			p.pose.orientation.z = t.getRotation().getZ();
+			p.pose.orientation.w = t.getRotation().getW();
+
+
+
+			stage->setPose(p);
+			stage->setMonitoredStage(current_state_ptr);  // Hook into attach_object_stage
+
+			// Compute IK
+			Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
+                                   Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
+                                   Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
+			Eigen::Translation3d trans(0.1f, 0, 0);
+			Eigen::Isometry3d ik = eigen * trans;
+			auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage));
+			wrapper->setMaxIKSolutions(2);
+			wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
+			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
+			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
+			place->insert(std::move(wrapper));
+		}
+
+		{
+			auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
+			stage->setGroup(mr->map()["eef_name"]);
+			stage->setGoal("open");
+			place->insert(std::move(stage));
+		}
+
+		{
+			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
+			stage->allowCollisions(
+			    object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
+			    false);
+			place->insert(std::move(stage));
+		}
+
+		{
+			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object");
+			stage->detachObject(object, mr->map()["hand_frame"]);
+			place->insert(std::move(stage));
+		}
+
+		{
+			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner_);
+			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
+			stage->setMinMaxDistance(.1, .2);
+			stage->setIKFrame(mr->map()["hand_frame"]);
+			stage->properties().set("marker_ns", "retreat");
+			geometry_msgs::Vector3Stamped vec;
+			vec.header.frame_id = mr->map()["hand_frame"];
+			vec.vector.z = -1.0;
+			stage->setDirection(vec);
+			place->insert(std::move(stage));
+		}
+
+		{
+			auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
+			stage->setGroup(mr->map()["eef_name"]);
+			stage->setGoal("close");
+			place->insert(std::move(stage));
+		}
+
+		// Add place container to task
+		task_.add(std::move(place));
+	}
+
+	{
+		auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner_);
+		stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
+		stage->setGoal("ready");
+		stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
+		task_.add(std::move(stage));
+	}
+
+	return task_;
+}
+
 //! connect robot and initialize Moveit components
 /*!
 Set up acm_ and rs_ members to track, merge, and publish changes during execution.
@@ -37,27 +461,60 @@ void MGMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot){
 }; 
 
 void MGMediator::publishTables(){
-    ros::WallDuration sleep_time(1.0);
-	// for (const auto& s_r : robots_){
-	// 	auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
-
-	// 	for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){
-	// 		if(ceti_bot->observerMask()[k]){
-	// 			auto wmd = dynamic_cast<MoveitPanel*>(ceti_bot->observers()[k]->next());
-	// 			psi_->applyCollisionObject(wmd->marker());
-	// 			acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->marker().id.c_str(), std::vector<uint8_t>()));
-	// 			sleep_time.sleep();
-	// 		}
-    //     }
-    // }
+    ros::WallDuration sleep_time(5.0);
+
+    std::string workSpace = "{\"mode\" : \"CONSTRUCT\", \"data\": {\"panda_arm1panda_arm2\" : [\"panda_arm1\", \"panda_arm2\"]}}";
+    sendToAll("init", workSpace);
+
+    for (const auto& s_r : robots_){
+    	auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
+
+    	for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){
+    		if(ceti_bot->observerMask()[k]){
+    			auto wmd = dynamic_cast<MoveitPanel*>(ceti_bot->observers()[k]->next());
+
+                geometry_msgs::Vector3 size;
+                size.x = wmd->size().getX();
+                size.y = wmd->size().getY();
+                size.z = wmd->size().getZ();
+
+                geometry_msgs::Pose pose;
+                pose.position.x = wmd->marker().primitive_poses[0].position.x;
+                pose.position.y = wmd->marker().primitive_poses[0].position.y;
+                pose.position.z = wmd->marker().primitive_poses[0].position.z;
+                pose.orientation.x = wmd->marker().primitive_poses[0].orientation.x;
+                pose.orientation.y = wmd->marker().primitive_poses[0].orientation.y;
+                pose.orientation.z = wmd->marker().primitive_poses[0].orientation.z;
+                pose.orientation.w = wmd->marker().primitive_poses[0].orientation.w;
+
+
+                ROS_INFO("%f %f %f", size.x, size.y, size.z);
+                ROS_INFO("%f %f %f", pose.position.x, pose.position.y, pose.position.z);
+                ROS_INFO("%f %f %f %f", pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
+
+
+                // visual_tools_->publishCollisionCuboid(pose, size.x, size.y, size.z, wmd->name(),rviz_visual_tools::colors::DARK_GREY);
+                // visual_tools_->setAlpha(1.0f);
+                // visual_tools_->trigger();
+
+                psi_->applyCollisionObject(wmd->marker());
+    			acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->marker().id.c_str(), std::vector<uint8_t>()));
+    			sleep_time.sleep();
+    		}
+        }
+    }
+    sendToAll("initcon", "");
 
     std::string str;
     std::map<const std::string, std::vector<Cuboid>> robot_to_cuboid;
     std::bitset<3> panel_mask(7);
 
+
     for (const auto& pair : robots_) {        
         robot_to_cuboid.insert_or_assign(pair.first,  std::vector<Cuboid>());
-        std::stringstream ss;
+        std::stringstream ss;   
+	
+
         ss << "{\"objects\": [{\"id\": \""<< pair.first << "\", \"type\": \"ARM\"}";
         for (auto& c: cuboid_reader_->cuboidBox()){
             tf2::Transform trans(tf2::Quaternion(0,0,0,1), tf2::Vector3(c.Pose.position.x, c.Pose.position.y, c.Pose.position.z));
@@ -65,6 +522,31 @@ void MGMediator::publishTables(){
                 ss << ",{\"id\": \"" << c.Name <<"\",\"type\": \"BOX\",\"pos\": {\"x\": "<< c.Pose.position.x << ",\"y\": "<< c.Pose.position.y <<",\"z\": " << c.Pose.position.x << "},\"size\": {\"length\": "<< c.x_depth <<",\"width\": "<< c.y_width <<",\"height\": "<< c.z_heigth <<"},\"orientation\": {\"x\": "<< c.Pose.orientation.x <<", \"y\": "<< c.Pose.orientation.y <<" , \"z\": "<< c.Pose.orientation.z <<", \"w\": "<< c.Pose.orientation.w<< "},";
                 ss << "\"color\": {\"r\": 0.2,\"g\": 0.2,\"b\": 0.2}}";
             }
+
+            moveit_msgs::CollisionObject item;
+            item.id = c.Name;
+            item.header.frame_id = "world";
+            item.header.stamp = ros::Time::now();
+            item.primitives.resize(1);
+            item.primitives[0].type = item.primitives[0].BOX;
+            item.primitives[0].dimensions.resize(3);
+            item.primitives[0].dimensions[0] = c.x_depth;
+            item.primitives[0].dimensions[1] = c.y_width;
+            item.primitives[0].dimensions[2] = c.z_heigth;
+
+            item.primitive_poses.resize(1);
+            item.primitive_poses[0].position.x = c.Pose.position.x;
+            item.primitive_poses[0].position.y = c.Pose.position.y;
+            item.primitive_poses[0].position.z = c.Pose.position.z;
+            item.primitive_poses[0].orientation.x = c.Pose.orientation.x;
+            item.primitive_poses[0].orientation.y = c.Pose.orientation.y;
+            item.primitive_poses[0].orientation.z = c.Pose.orientation.z;
+            item.primitive_poses[0].orientation.w = c.Pose.orientation.w;
+            item.operation = item.ADD;
+
+            psi_->applyCollisionObject(item);
+            // acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
+
         }
 
         for (auto& c: cuboid_reader_->cuboidObstacle()){
@@ -76,10 +558,15 @@ void MGMediator::publishTables(){
         }
         ss << "]}";
 
+        std::stringstream rss;
+        rss << "World/panda_arm1panda_arm2/" << pair.first;
+
         // publish 
-        sendToAll("World/robotarm1robotarm2", ss.str());
-    } 
+        sleep_time.sleep();
+        sendToAll(rss.str(), ss.str());
+    }   
 
+    sendToAll("command/pickup", "");
 
 }
 
diff --git a/src/reader/cuboid_reader.cpp b/src/reader/cuboid_reader.cpp
index 001da07..31609ca 100644
--- a/src/reader/cuboid_reader.cpp
+++ b/src/reader/cuboid_reader.cpp
@@ -26,6 +26,8 @@ void CuboidReader::read(){
             (resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0);
 
             Cuboid o;
+
+            
             o.Name = id.c_str();
             o.Pose.position.x = pos.getX();
             o.Pose.position.y = pos.getY();
@@ -38,12 +40,19 @@ void CuboidReader::read(){
             o.y_width = size.getY();
             o.z_heigth = size.getZ();
 
+            o.r = (resources[i]["color"].hasMember("r")) ? floatOf(resources[i]["color"]["r"]) : 0;
+            o.g = (resources[i]["color"].hasMember("g")) ? floatOf(resources[i]["color"]["g"]) : 0;
+            o.b = (resources[i]["color"].hasMember("b")) ? floatOf(resources[i]["color"]["b"]) : 0;
+
+
             
             cuboid_box_.push_back(o);
             ROS_INFO("--- Object: %s  --- Type: BOX  ---", o.Name.c_str());
             ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
             ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
             ROS_INFO("=> Object: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+            ROS_INFO("=> Object: color('%f', '%f', '%f')", o.r, o.g, o.b);
+
         }
 
         if(resources[i]["type"] == "BIN" || std::regex_match(id, match, std::regex("obstacle([0-9]+)"))){
@@ -76,11 +85,17 @@ void CuboidReader::read(){
             o.y_width = size.getY();
             o.z_heigth = size.getZ();
 
+            o.r = (resources[i]["color"].hasMember("r")) ? floatOf(resources[i]["color"]["r"]) : 0;
+            o.g = (resources[i]["color"].hasMember("g")) ? floatOf(resources[i]["color"]["g"]) : 0;
+            o.b = (resources[i]["color"].hasMember("b")) ? floatOf(resources[i]["color"]["b"]) : 0;
+
             cuboid_obstacle_.push_back(o);
             ROS_INFO("--- Object: %s  --- Type: BIN  ---", o.Name.c_str());
             ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
             ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
             ROS_INFO("=> Object: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+            ROS_INFO("=> Object: color('%f', '%f', '%f')", o.r, o.g, o.b);
+
 
         }
     }
diff --git a/src/robot/ceti_robot.cpp b/src/robot/ceti_robot.cpp
index 84bea3c..2da400a 100644
--- a/src/robot/ceti_robot.cpp
+++ b/src/robot/ceti_robot.cpp
@@ -23,7 +23,7 @@ bool CetiRobot::checkSingleObjectCollision(tf2::Transform& obj, std::string& str
     std::stringstream ss;
     std::regex rx("panda_arm([0-9]+)");
     std::smatch match;
-    std::regex_match(str, match, rx);
+    std::regex_match(name(), match, rx);
 
     ss << "base_" << match[1]; 
     A = tf_ * bounds_[0];
diff --git a/src/robot/decorators/panda_decorator.cpp b/src/robot/decorators/panda_decorator.cpp
index 60a16bb..5767234 100644
--- a/src/robot/decorators/panda_decorator.cpp
+++ b/src/robot/decorators/panda_decorator.cpp
@@ -20,6 +20,7 @@ void PandaDecorator::spezifieRootBounds(){
 void PandaDecorator::spezifieRobotGroups(){
     mgi_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(next_->name());
 
+
     std::stringstream hand_n, ik_frame_n, name_n, base_n;
 
     std::regex panda_id("panda_arm([0-9]+)"), left_finger("left_finger"), right_finger("right_finger"), hand_link("hand_link");
@@ -30,6 +31,7 @@ void PandaDecorator::spezifieRobotGroups(){
     ik_frame_n << "panda_" << match[1] << "_link8";
     base_n << "base_" << match[1];
 
+
     mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
 
 
-- 
GitLab