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 @@ ...@@ -8,18 +8,47 @@
#include <ccf/connection/MqttConnection.h> #include <ccf/connection/MqttConnection.h>
#include <ccf/controller/Controller.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 //should be a mix between MG, bahvior tree, moveit task constructor
class MGMediator: public AbstractMediator, virtual Controller{ class MGMediator: public AbstractMediator, virtual Controller{
protected: protected:
std::shared_ptr<moveit::core::RobotModel> robot_model_;
std::vector<std::unique_ptr<Connection>> connections_; std::vector<std::unique_ptr<Connection>> connections_;
std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; //!< MoveItVisualTools 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<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::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>> acm_; //!< shared allowed collision matrix between robots
std::map<std::string, std::vector<uint8_t>> rs_; //!< shared robot state between all 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 std::string referenceRobot_; //!< Reference Robot
...@@ -46,6 +75,8 @@ class MGMediator: public AbstractMediator, virtual Controller{ ...@@ -46,6 +75,8 @@ class MGMediator: public AbstractMediator, virtual Controller{
void setPanel() override; void setPanel() override;
void publishTables(); 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 #endif
\ No newline at end of file
...@@ -20,8 +20,10 @@ ...@@ -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_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': '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': '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': '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': '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': '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': '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
This diff is collapsed.
...@@ -26,6 +26,8 @@ void CuboidReader::read(){ ...@@ -26,6 +26,8 @@ void CuboidReader::read(){
(resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0); (resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0);
Cuboid o; Cuboid o;
o.Name = id.c_str(); o.Name = id.c_str();
o.Pose.position.x = pos.getX(); o.Pose.position.x = pos.getX();
o.Pose.position.y = pos.getY(); o.Pose.position.y = pos.getY();
...@@ -38,12 +40,19 @@ void CuboidReader::read(){ ...@@ -38,12 +40,19 @@ void CuboidReader::read(){
o.y_width = size.getY(); o.y_width = size.getY();
o.z_heigth = size.getZ(); 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); cuboid_box_.push_back(o);
ROS_INFO("--- Object: %s --- Type: BOX ---", o.Name.c_str()); 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: 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: 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: 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]+)"))){ if(resources[i]["type"] == "BIN" || std::regex_match(id, match, std::regex("obstacle([0-9]+)"))){
...@@ -76,11 +85,17 @@ void CuboidReader::read(){ ...@@ -76,11 +85,17 @@ void CuboidReader::read(){
o.y_width = size.getY(); o.y_width = size.getY();
o.z_heigth = size.getZ(); 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); cuboid_obstacle_.push_back(o);
ROS_INFO("--- Object: %s --- Type: BIN ---", o.Name.c_str()); 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: 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: 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: 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 ...@@ -23,7 +23,7 @@ bool CetiRobot::checkSingleObjectCollision(tf2::Transform& obj, std::string& str
std::stringstream ss; std::stringstream ss;
std::regex rx("panda_arm([0-9]+)"); std::regex rx("panda_arm([0-9]+)");
std::smatch match; std::smatch match;
std::regex_match(str, match, rx); std::regex_match(name(), match, rx);
ss << "base_" << match[1]; ss << "base_" << match[1];
A = tf_ * bounds_[0]; A = tf_ * bounds_[0];
......
...@@ -20,6 +20,7 @@ void PandaDecorator::spezifieRootBounds(){ ...@@ -20,6 +20,7 @@ void PandaDecorator::spezifieRootBounds(){
void PandaDecorator::spezifieRobotGroups(){ void PandaDecorator::spezifieRobotGroups(){
mgi_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(next_->name()); mgi_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(next_->name());
std::stringstream hand_n, ik_frame_n, name_n, base_n; 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"); 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(){ ...@@ -30,6 +31,7 @@ void PandaDecorator::spezifieRobotGroups(){
ik_frame_n << "panda_" << match[1] << "_link8"; ik_frame_n << "panda_" << match[1] << "_link8";
base_n << "base_" << match[1]; base_n << "base_" << match[1];
mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str()); 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