Skip to content
Snippets Groups Projects
Commit 2b1e4ab0 authored by KingMaZito's avatar KingMaZito
Browse files

between MGs and Task Constructors

parent fe0fb0a2
No related branches found
No related tags found
No related merge requests found
......@@ -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
......@@ -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
......@@ -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;
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", "");
}
......
......@@ -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);
}
}
......
......@@ -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];
......
......@@ -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());
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment