Skip to content
Snippets Groups Projects
Commit 88948f3f authored by KingMaZito's avatar KingMaZito
Browse files

callBack implementation for pickup/drop/place tested

parent b8075244
No related branches found
No related tags found
No related merge requests found
...@@ -77,8 +77,12 @@ class MGMediator: public AbstractMediator, virtual Controller{ ...@@ -77,8 +77,12 @@ class MGMediator: public AbstractMediator, virtual Controller{
void publishTables(); void publishTables();
moveit::task_constructor::Task pickUp(const std::string& obj, const std::string& robotId); 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 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 processSharedStructure();
void setupMqtt();
}; };
#endif #endif
\ No newline at end of file
...@@ -21,9 +21,9 @@ ...@@ -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': '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': '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': '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': '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': '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 } }, { '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
#include "mediator/mg_mediator.h" #include "mediator/mg_mediator.h"
#include "yaml-cpp/yaml.h"
std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> shared_structure; std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> shared_structure;
std::mutex shared_mutex; std::mutex shared_mutex;
std::condition_variable cv; std::condition_variable cv;
...@@ -28,7 +30,10 @@ void MGMediator::processSharedStructure() { ...@@ -28,7 +30,10 @@ void MGMediator::processSharedStructure() {
--it; // Adjust the iterator after erasing --it; // Adjust the iterator after erasing
} }
} }
} // The lock is automatically released here when `lock` goes out of scope }
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 // Print the sizes of the sub_trajectory vectors
// if (!shared_structure.empty()) { // if (!shared_structure.empty()) {
...@@ -37,7 +42,7 @@ void MGMediator::processSharedStructure() { ...@@ -37,7 +42,7 @@ void MGMediator::processSharedStructure() {
// } // }
// } // }
// ROS_INFO("Do something"); ROS_INFO("Do something");
peak.clear(); peak.clear();
...@@ -55,20 +60,85 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh) ...@@ -55,20 +60,85 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
, mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms")) , 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))){ , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1))){
// MQTT HANDLE
// 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();
// }
// }
// });
// 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);
}
void MGMediator::setupMqtt(){
auto mqtt = std::make_shared<MqttConnection>("localhost", "op"); auto mqtt = std::make_shared<MqttConnection>("localhost", "op");
mqtt->listen("command/place");
mqtt->listen("command/pickup");
addConnection(std::move(mqtt)); for (const auto& rob : robots_){
std::stringstream ss;
ss << rob.first << "/command";
mqtt->listen(ss.str());
addCallback("command/pickup", [&](const std::string& data) { addCallback(ss.str(), [&](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") }; YAML::Node node;
for (const auto& pair: pickup_command) { try {
moveit::task_constructor::Task mgt = pickUp(pair.first, pair.second); 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)) { if(mgt.plan(1)) {
//mgt.introspection().publishSolution(*mgt.solutions().front()); //mgt.introspection().publishSolution(*mgt.solutions().front());
//mgt.execute(*mgt.solutions().front()); mgt.execute(*mgt.solutions().front());
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e; moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection()); mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
...@@ -81,15 +151,12 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh) ...@@ -81,15 +151,12 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
cv.notify_one(); cv.notify_one();
} }
} }
});
addCallback("command/place", [&](const std::string& data) { if (node["mode"].as<std::string>() == "PLACE"){
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") }; moveit::task_constructor::Task mgt = place(node["object"].as<std::string>(), node["robot"].as<std::string>(), node["place"].as<std::string>());
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)) { if(mgt.plan(1)) {
//mgt.introspection().publishSolution(*mgt.solutions().front()); //mgt.introspection().publishSolution(*mgt.solutions().front());
//mgt.execute(*mgt.solutions().front()); mgt.execute(*mgt.solutions().front());
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e; moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection()); mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
...@@ -100,44 +167,38 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh) ...@@ -100,44 +167,38 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
// Notify the consuming thread // Notify the consuming thread
cv.notify_one(); cv.notify_one();
} }
} }
});
// 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"); if (node["mode"].as<std::string>() == "DROP"){
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>("world", "/rviz_visual_tools"); moveit::task_constructor::Task mgt = drop(node["object"].as<std::string>(), node["robot"].as<std::string>(), node["drop"].as<std::string>());
visual_tools_->loadMarkerPub(); 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());
visual_tools_->waitForMarkerPub(); {
std::lock_guard<std::mutex> lock(shared_mutex);
shared_structure.push_back(std::move(e));
}
visual_tools_->loadRemoteControl(); // Notify the consuming thread
visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_); cv.notify_one();
visual_tools_->trigger(); }
}
});
}
addConnection(std::move(mqtt));
sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
// cartesian
cartesian_planner_->setMaxVelocityScaling(1.0);
cartesian_planner_->setMaxAccelerationScaling(1.0);
cartesian_planner_->setStepSize(.01);
} }
void MGMediator::mediate(){ void MGMediator::mediate(){
setupMqtt();
setPanel(); setPanel();
publishTables(); publishTables();
...@@ -181,7 +242,7 @@ moveit::task_constructor::Task MGMediator::pickUp(const std::string& obj, const ...@@ -181,7 +242,7 @@ moveit::task_constructor::Task MGMediator::pickUp(const std::string& obj, const
ROS_INFO("ss1 %s", support_surface1.c_str()); 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_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
task_.loadRobotModel(); task_.loadRobotModel();
task_.setRobotModel(mr->mgi()->getRobotModel()); task_.setRobotModel(mr->mgi()->getRobotModel());
...@@ -359,7 +420,7 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s ...@@ -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_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
task_.loadRobotModel(); task_.loadRobotModel();
task_.setRobotModel(mr->mgi()->getRobotModel()); task_.setRobotModel(mr->mgi()->getRobotModel());
...@@ -514,6 +575,157 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s ...@@ -514,6 +575,157 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s
return task_; 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 //! connect robot and initialize Moveit components
/*! /*!
Set up acm_ and rs_ members to track, merge, and publish changes during execution. Set up acm_ and rs_ members to track, merge, and publish changes during execution.
...@@ -598,6 +810,20 @@ void MGMediator::publishTables(){ ...@@ -598,6 +810,20 @@ 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 << ",{\"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}}"; 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 << "]}";
for (auto& c: cuboid_reader_->cuboidObstacle()){
moveit_msgs::CollisionObject item; moveit_msgs::CollisionObject item;
item.id = c.Name; item.id = c.Name;
...@@ -621,18 +847,32 @@ void MGMediator::publishTables(){ ...@@ -621,18 +847,32 @@ void MGMediator::publishTables(){
item.operation = item.ADD; item.operation = item.ADD;
psi_->applyCollisionObject(item); 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()){ 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)); moveit_msgs::CollisionObject item;
if(pair.second->checkSingleObjectCollision(trans, str, panel_mask)) { item.id = c.Name;
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<< "},"; item.header.frame_id = "world";
ss << "\"color\": {\"r\": 0.2,\"g\": 0.2,\"b\": 0.2}}"; 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);
} }
ss << "]}";
std::stringstream rss; std::stringstream rss;
rss << "World/panda_arm1panda_arm2/" << pair.first; rss << "World/panda_arm1panda_arm2/" << pair.first;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment