diff --git a/include/mediator/mg_mediator.h b/include/mediator/mg_mediator.h
index 4f1845b1495451d3fe85ca708c64de29d0b7896c..7752625c5f68e60be3943512c25b660b8db1cb00 100644
--- a/include/mediator/mg_mediator.h
+++ b/include/mediator/mg_mediator.h
@@ -77,8 +77,12 @@ class MGMediator: public AbstractMediator, virtual Controller{
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);
+ moveit::task_constructor::Task drop(const std::string& obj, const std::string& robotId, const std::string& drop);
+
void processSharedStructure();
+ void setupMqtt();
+
};
#endif
\ No newline at end of file
diff --git a/results/dummy/-2045918175/-2045918175.yaml b/results/dummy/-2045918175/-2045918175.yaml
index 98ff98e52135f0f1beded485c574411040095998..31c2de1cec53737a428dde386584921c452382e4 100644
--- a/results/dummy/-2045918175/-2045918175.yaml
+++ b/results/dummy/-2045918175/-2045918175.yaml
@@ -21,9 +21,9 @@
{ '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': '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': 'ABIN', 'type': 'BIN', 'pos': { 'x': -0.1, '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': 'BBIN', 'type': 'BIN', '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 bfe9c159b5c3abac8acd202f0533c63138db0058..faa005405999e2fe070360c8d091ab47e89ea440 100644
--- a/src/mediator/mg_mediator.cpp
+++ b/src/mediator/mg_mediator.cpp
@@ -1,5 +1,7 @@
#include "mediator/mg_mediator.h"
+#include "yaml-cpp/yaml.h"
+
std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> shared_structure;
std::mutex shared_mutex;
std::condition_variable cv;
@@ -16,19 +18,22 @@ void MGMediator::processSharedStructure() {
// Iterate over the vectors in shared_structure
for (auto it = shared_structure.begin(); it != shared_structure.end(); ++it) {
- if (!it->solution.sub_trajectory.empty()) {
- // Get the first entry of the vector and add it to peak
- peak.push_back(it->solution.sub_trajectory.front());
- it->solution.sub_trajectory.erase(it->solution.sub_trajectory.begin());
- }
+ if (!it->solution.sub_trajectory.empty()) {
+ // Get the first entry of the vector and add it to peak
+ peak.push_back(it->solution.sub_trajectory.front());
+ it->solution.sub_trajectory.erase(it->solution.sub_trajectory.begin());
+ }
- if (it->solution.sub_trajectory.empty()) {
- // Remove the vector if all entries have been processed
- it = shared_structure.erase(it);
- --it; // Adjust the iterator after erasing
- }
- }
- } // The lock is automatically released here when `lock` goes out of scope
+ if (it->solution.sub_trajectory.empty()) {
+ // Remove the vector if all entries have been processed
+ it = shared_structure.erase(it);
+ --it; // Adjust the iterator after erasing
+ }
+ }
+ }
+
+ sendToAll("payload/panda_arm1", "penis");
+ // The lock is automatically released here when `lock` goes out of scope
// Print the sizes of the sub_trajectory vectors
// if (!shared_structure.empty()) {
@@ -37,7 +42,7 @@ void MGMediator::processSharedStructure() {
// }
// }
- // ROS_INFO("Do something");
+ ROS_INFO("Do something");
peak.clear();
@@ -55,55 +60,28 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
, 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
- auto mqtt = std::make_shared<MqttConnection>("localhost", "op");
- mqtt->listen("command/place");
- mqtt->listen("command/pickup");
- addConnection(std::move(mqtt));
+ // 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_msgs::ExecuteTaskSolutionGoal e;
+ // mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
- 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());
- moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
- mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
-
- {
- std::lock_guard<std::mutex> lock(shared_mutex);
- shared_structure.push_back(std::move(e));
- }
+ // {
+ // std::lock_guard<std::mutex> lock(shared_mutex);
+ // shared_structure.push_back(std::move(e));
+ // }
- // Notify the consuming thread
- cv.notify_one();
- }
- }
- });
-
- 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_msgs::ExecuteTaskSolutionGoal e;
- mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
-
- {
- std::lock_guard<std::mutex> lock(shared_mutex);
- shared_structure.push_back(std::move(e));
- }
+ // // Notify the consuming thread
+ // cv.notify_one();
- // Notify the consuming thread
- cv.notify_one();
-
- }
- }
- });
+ // }
+ // }
+ // });
// moveit::task_constructor::Task mgt2 = place("A", "panda_arm1", "dafd");
@@ -137,7 +115,90 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
}
+void MGMediator::setupMqtt(){
+ auto mqtt = std::make_shared<MqttConnection>("localhost", "op");
+
+ for (const auto& rob : robots_){
+ std::stringstream ss;
+ ss << rob.first << "/command";
+ mqtt->listen(ss.str());
+
+ addCallback(ss.str(), [&](const std::string& data) {
+ YAML::Node node;
+ try {
+ node = YAML::Load(data);
+ for(YAML::const_iterator it=node.begin(); it!=node.end();++it) {
+ std::stringstream sss;
+ sss << "Key: " << it->first.as<std::string>() << ", Value: " << it->second.as<std::string>();
+ ROS_INFO("%s", sss.str().c_str());
+ }
+ } catch (const YAML::RepresentationException& e){ ROS_INFO("Bad Command"); return;}
+
+ if (node["mode"].as<std::string>() == "PICKUP"){
+ moveit::task_constructor::Task mgt = pickUp(node["object"].as<std::string>(), node["robot"].as<std::string>());
+ if(mgt.plan(1)) {
+ //mgt.introspection().publishSolution(*mgt.solutions().front());
+ mgt.execute(*mgt.solutions().front());
+ moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
+ mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
+
+ {
+ std::lock_guard<std::mutex> lock(shared_mutex);
+ shared_structure.push_back(std::move(e));
+ }
+
+ // Notify the consuming thread
+ cv.notify_one();
+ }
+ }
+
+ if (node["mode"].as<std::string>() == "PLACE"){
+ moveit::task_constructor::Task mgt = place(node["object"].as<std::string>(), node["robot"].as<std::string>(), node["place"].as<std::string>());
+ if(mgt.plan(1)) {
+ //mgt.introspection().publishSolution(*mgt.solutions().front());
+ mgt.execute(*mgt.solutions().front());
+ moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
+ mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
+
+ {
+ std::lock_guard<std::mutex> lock(shared_mutex);
+ shared_structure.push_back(std::move(e));
+ }
+
+ // Notify the consuming thread
+ cv.notify_one();
+ }
+ }
+
+ if (node["mode"].as<std::string>() == "DROP"){
+ moveit::task_constructor::Task mgt = drop(node["object"].as<std::string>(), node["robot"].as<std::string>(), node["drop"].as<std::string>());
+ if(mgt.plan(1)) {
+ //mgt.introspection().publishSolution(*mgt.solutions().front());
+ mgt.execute(*mgt.solutions().front());
+ moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
+ mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
+
+ {
+ std::lock_guard<std::mutex> lock(shared_mutex);
+ shared_structure.push_back(std::move(e));
+ }
+
+ // Notify the consuming thread
+ cv.notify_one();
+ }
+ }
+ });
+ }
+
+ addConnection(std::move(mqtt));
+
+
+
+}
+
+
void MGMediator::mediate(){
+ setupMqtt();
setPanel();
publishTables();
@@ -181,7 +242,7 @@ moveit::task_constructor::Task MGMediator::pickUp(const std::string& obj, const
ROS_INFO("ss1 %s", support_surface1.c_str());
- std::string name = "Pick&Place";
+ std::string name = "Pick";
task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
task_.loadRobotModel();
task_.setRobotModel(mr->mgi()->getRobotModel());
@@ -359,7 +420,7 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s
- std::string name = "Pick&Place";
+ std::string name = "Place";
task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
task_.loadRobotModel();
task_.setRobotModel(mr->mgi()->getRobotModel());
@@ -514,6 +575,157 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s
return task_;
}
+moveit::task_constructor::Task MGMediator::drop(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();
+ }
+
+ moveit_msgs::CollisionObject target_bin = psi_->getObjects().at(target);
+ const std::string object = obj;
+
+ moveit::task_constructor::Task task_;
+ tf2::Transform t(tf2::Quaternion(target_bin.pose.orientation.x, target_bin.pose.orientation.y, target_bin.pose.orientation.z, target_bin.pose.orientation.w), tf2::Vector3(target_bin.pose.position.x, target_bin.pose.position.y, (3* target_bin.primitives[0].dimensions[2]) + target_bin.pose.position.z));
+ std::string support_surface1 = "nichts";
+
+ // Just some value
+ 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 = "Drop";
+ 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" });
+
+ {
+ // 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>("detach object");
+ stage->detachObject(object, mr->map()["hand_frame"]);
+ 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));
+
+ {
+ 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);
+ 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.
@@ -598,8 +810,22 @@ 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}}";
}
+ // acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
+ }
+
+ for (auto& c: cuboid_reader_->cuboidObstacle()){
+ tf2::Transform trans(tf2::Quaternion(0,0,0,1), tf2::Vector3(c.Pose.position.x, c.Pose.position.y, c.Pose.position.z));
+ if(pair.second->checkSingleObjectCollision(trans, str, panel_mask)) {
+ ss << ",{\"id\": \"" << c.Name <<"\",\"type\": \"BIN\",\"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}}";
+ }
+ }
+ ss << "]}";
+
- moveit_msgs::CollisionObject item;
+ for (auto& c: cuboid_reader_->cuboidObstacle()){
+
+ moveit_msgs::CollisionObject item;
item.id = c.Name;
item.header.frame_id = "world";
item.header.stamp = ros::Time::now();
@@ -621,18 +847,32 @@ void MGMediator::publishTables(){
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_->cuboidBox()){
+ 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;
- for (auto& c: cuboid_reader_->cuboidObstacle()){
- tf2::Transform trans(tf2::Quaternion(0,0,0,1), tf2::Vector3(c.Pose.position.x, c.Pose.position.y, c.Pose.position.z));
- if(pair.second->checkSingleObjectCollision(trans, str, panel_mask)) {
- ss << ",{\"id\": \"" << c.Name <<"\",\"type\": \"BIN\",\"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}}";
- }
- }
- ss << "]}";
+ 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);
+ }
std::stringstream rss;
rss << "World/panda_arm1panda_arm2/" << pair.first;