diff --git a/include/mediator/mg_mediator.h b/include/mediator/mg_mediator.h
index 7752625c5f68e60be3943512c25b660b8db1cb00..7d7d9e7f96938d2c2ad18feff3c0e37a4619e1a2 100644
--- a/include/mediator/mg_mediator.h
+++ b/include/mediator/mg_mediator.h
@@ -47,7 +47,7 @@ class MGMediator: public AbstractMediator, virtual Controller{
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::shared_ptr<moveit_msgs::PlanningScene> ps_; //!< Shared Planning Scene
@@ -79,6 +79,11 @@ class MGMediator: public AbstractMediator, virtual Controller{
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 manipulateACM(moveit_msgs::PlanningScene& ps, const std::string& robotId);
+ void parallelExec(const moveit_task_constructor_msgs::SubTrajectory st, const std::string& robotId);
+ void mergePS(const moveit_msgs::PlanningScene& in, const std::string& robotId);
+ void mergeACM();
+
void processSharedStructure();
void setupMqtt();
diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp
index a298446ddda552cfe08d00328facb913c8c54112..2a493ab656d6b12a076be04ab95f80fb232fc62d 100644
--- a/src/mediator/mg_mediator.cpp
+++ b/src/mediator/mg_mediator.cpp
@@ -6,6 +6,86 @@ std::vector<std::pair<std::string,moveit_task_constructor_msgs::ExecuteTaskSolut
std::mutex shared_mutex;
std::condition_variable cv;
+
+void MGMediator::mergeACM(){
+ moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
+ acmt.entry_values.resize(acm_.size());
+
+ int i = 0;
+ for (auto& a : acm_){
+ acmt.entry_names.push_back(a.first);
+ acmt.entry_values[i].enabled = a.second;
+ i++;
+ }
+
+ ps_->allowed_collision_matrix = acmt;
+}
+
+void MGMediator::parallelExec(moveit_task_constructor_msgs::SubTrajectory st, const std::string& robotId){
+ {
+ std::unique_lock<std::mutex> lock(shared_mutex);
+ manipulateACM(st.scene_diff, robotId);
+ mergePS(st.scene_diff, robotId);
+ }
+
+ AbstractRobotDecorator* mr = nullptr;
+ {
+ std::unique_lock<std::mutex> lock(shared_mutex);
+ mr= robots_.at(robotId).get();
+ }
+ mr->mgi()->execute(st.trajectory);
+}
+
+void MGMediator::mergePS(const moveit_msgs::PlanningScene& in, const std::string& robotId){
+ // get full mr link list
+ if (!ps_){
+ ps_ = std::make_shared<moveit_msgs::PlanningScene>(in);
+ return;
+ }
+
+ AbstractRobotDecorator* mr = nullptr;
+ {
+ std::unique_lock<std::mutex> lock(shared_mutex);
+ mr= robots_.at(robotId).get();
+ }
+
+ for (auto ao : in.robot_state.attached_collision_objects) ps_->robot_state.attached_collision_objects.push_back(ao);
+ ps_->robot_state.is_diff |= in.robot_state.is_diff;
+ ps_->is_diff |= in.is_diff;
+ ps_->robot_state.joint_state.header = in.robot_state.joint_state.header;
+ ps_->robot_model_name = "panda";
+
+ std::vector<std::string> links;
+ for (auto link : mr->mgi()->getLinkNames()) links.push_back(link);
+ links.push_back(mr->map()["left_finger"]);
+ links.push_back(mr->map()["right_finger"]);
+ links.push_back(mr->map()["hand_link"]);
+ links.push_back(mr->map()["base"]);
+
+ for (auto link : links) {
+ for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){
+ if(link == in.robot_state.joint_state.name[i]){
+ ps_->robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
+ ps_->robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
+ ps_->robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
+ }
+ }
+
+ for(int i = 0; i < in.link_padding.size(); i++){
+ if(link == in.link_padding[i].link_name){
+ ps_->link_padding.push_back(in.link_padding[i]);
+ }
+ }
+
+ for(int i = 0; i < in.link_scale.size(); i++){
+ if(link == in.link_scale[i].link_name){
+ ps_->link_scale.push_back(in.link_scale[i]);
+ }
+ }
+ }
+}
+
+
void MGMediator::processSharedStructure() {
while (ros::ok()) {
std::vector<std::pair<std::string, moveit_task_constructor_msgs::SubTrajectory>> peak;
@@ -24,14 +104,7 @@ void MGMediator::processSharedStructure() {
it->second.solution.sub_trajectory.erase(it->second.solution.sub_trajectory.begin());
}
- if (it->second.solution.sub_trajectory.empty() && it->first == "panda_arm1") {
- /*
- All sub_trajectorys of an Robot are done, so the grammar should be informed about the state
- */
- std::stringstream ss;
- ss << it->first << "/idle";
- sendToAll(ss.str(), "");
-
+ if (it->second.solution.sub_trajectory.empty()) {
/*
Lets publish also scene modifications here
But I want to publish only vanished objects
@@ -39,22 +112,28 @@ void MGMediator::processSharedStructure() {
*/
std::string temp;
- if ((cuboid_reader_->cuboidBox().size() + cuboid_reader_->cuboidBox().size()) > psi_->getObjects().size()){
+ try{
+ for (const auto& c : cuboid_reader_->cuboidBox()){
+ temp = c.Name;
+ psi_->getObjects().at(c.Name);
+ }
+ } catch (const std::out_of_range& e) {
ROS_INFO("object is vanished!");
- try{
- for (const auto& c : cuboid_reader_->cuboidBox()){
- temp = c.Name;
- psi_->getObjects().at(c.Name);
- }
- } catch (const std::out_of_range& e) {
- std::stringstream payload;
- payload << "{\"type\": \"MODIFY\", \"mode\": \"DELETE\", \"robot\": \""<< it->first <<"\", \"object\": \""<< temp <<"\" }";
-
- std::stringstream payload_topic;
- payload_topic << it->first << "/payload";
- sendToAll(payload_topic.str(), payload.str());
- }
- }
+ std::stringstream payload;
+ payload << "{\"type\": \"MODIFY\", \"mode\": \"DELETE\", \"robot\": \""<< it->first <<"\", \"object\": \""<< temp <<"\" }";
+
+ std::stringstream payload_topic;
+ payload_topic << it->first << "/payload";
+ sendToAll(payload_topic.str(), payload.str());
+ }
+
+
+ /*
+ All sub_trajectorys of an Robot are done, so the grammar should be informed about the state
+ */
+ std::stringstream ss;
+ ss << it->first << "/idle";
+ sendToAll(ss.str(), "");
it = shared_structure.erase(it);
@@ -62,22 +141,22 @@ void MGMediator::processSharedStructure() {
}
}
- }
-
+ }
+
+ std::vector<std::thread> th;
for (const auto& exec : peak){
- if(exec.first == "panda_arm1"){
- AbstractRobotDecorator* mr = nullptr;
- try{
- mr= robots_.at(exec.first).get();
- mr->mgi()->execute(exec.second.trajectory);
- planning_scene_diff_publisher_->publish(exec.second.scene_diff);
- } catch (std::out_of_range& oor){
- ROS_INFO("Robot not found");
- ros::shutdown();
- }
- }
+ th.push_back(std::thread(&MGMediator::parallelExec, this, exec.second, std::ref(exec.first)));
+ }
+
+ for(auto& t : th){
+ t.join();
+
}
+ mergeACM();
+ planning_scene_diff_publisher_->publish(*ps_);
+
+
// The lock is automatically released here when `lock` goes out of scope
// Print the sizes of the sub_trajectory vectors
@@ -103,6 +182,7 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
, 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"))
+, ps_(nullptr)
, planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1))){
@@ -868,7 +948,7 @@ 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>()));
+ acm_.insert(std::pair<std::string, std::vector<uint8_t>>(c.Name, std::vector<uint8_t>()));
}
for (auto& c: cuboid_reader_->cuboidObstacle()){
@@ -877,6 +957,7 @@ void MGMediator::publishTables(){
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}}";
}
+ acm_.insert(std::pair<std::string, std::vector<uint8_t>>(c.Name, std::vector<uint8_t>()));
}
ss << "]}";
@@ -978,3 +1059,230 @@ void MGMediator::setPanel(){
s_r.second->notify();
}
}
+
+void MGMediator::manipulateACM(moveit_msgs::PlanningScene& ps, 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();
+ }
+ // First find ID from panda to start with
+ std::regex rx_panda(mr->pattern());
+ std::smatch match;
+ std::regex_match(mr->name(), match, rx_panda);
+
+ // build panda link regex
+ std::stringstream ss;
+ ss << "panda_" << match[1] << "_.*";
+ std::regex rx_panda_links(ss.str());
+ std::regex rx_table("table.*");
+
+
+ // Iterate task collsion matrix
+ for(int j = 0; j < ps.allowed_collision_matrix.entry_names.size(); j++ ){
+ if( mr->map().at("base") == ps.allowed_collision_matrix.entry_names[j]){
+ //In case an entry matches an robot spezific link
+
+ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ // For that specific entry, loop over values
+ int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+ /*
+ BOX -> Based on the context
+ */
+ for (const auto& c : cuboid_reader_->cuboidBox()) {
+ if (c.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ /*
+ BIN -> Based on the context
+ */
+ for (const auto& c : cuboid_reader_->cuboidObstacle()) {
+ if (c.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
+ acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+ if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+
+ if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
+ //In case an entry matches an robot spezific link
+ ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
+ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ //For that specific entry, loop over values
+ int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+ /*
+ BOX -> Based on the context
+ */
+ for (const auto& c : cuboid_reader_->cuboidBox()) {
+ if (c.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ /*
+ BIN -> Based on the context
+ */
+ for (const auto& c : cuboid_reader_->cuboidObstacle()) {
+ if (c.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+ if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+
+ /*
+ BOX -> Based on the context
+ */
+ for (const auto& c : cuboid_reader_->cuboidBox()) {
+ if (c.Name == ps.allowed_collision_matrix.entry_names[j]){
+ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ //For that specific entry, loop over values
+ int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+
+ /*
+ BOX -> Based on the context
+ */
+ for (const auto& c1 : cuboid_reader_->cuboidBox()) {
+ if (c1.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ /*
+ BIN -> Based on the context
+ */
+ for (const auto& c1 : cuboid_reader_->cuboidObstacle()) {
+ if (c1.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+ if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+ }
+
+ /*
+ BIN -> Based on the context
+ */
+ for (const auto& c : cuboid_reader_->cuboidObstacle()) {
+ if (c.Name == ps.allowed_collision_matrix.entry_names[j]){
+ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ //For that specific entry, loop over values
+ int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+ /*
+ BOX -> Based on the context
+ */
+ for (const auto& c1 : cuboid_reader_->cuboidBox()) {
+ if (c1.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ /*
+ BIN -> Based on the context
+ */
+ for (const auto& c1 : cuboid_reader_->cuboidObstacle()) {
+ if (c1.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+ if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+ }
+
+ if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_table)){
+ //In case an entry matches an robot spezific link
+ for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ //For that specific entry, loop over values
+ int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+ /*
+ BOX -> Based on the context
+ */
+ for (const auto& c : cuboid_reader_->cuboidBox()) {
+ if (c.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ /*
+ BIN -> Based on the context
+ */
+ for (const auto& c : cuboid_reader_->cuboidObstacle()) {
+ if (c.Name == ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+
+ if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+
+ if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
+ acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
+ }
+ }
+ }
+ }
+}
+
diff --git a/src/mg_routine.cpp b/src/mg_routine.cpp
index 333250c348256f1bd2e18c1667f8f9785565c60c..1f27a6d65ba730ab87d70a385e7da4e329e901d8 100644
--- a/src/mg_routine.cpp
+++ b/src/mg_routine.cpp
@@ -18,7 +18,7 @@ int main(int argc, char **argv){
ros::init(argc, argv, "mg_routine");
std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
- ros::AsyncSpinner spinner(4);
+ ros::AsyncSpinner spinner(6);
spinner.start();
std::shared_ptr<MGMediator> mediator = std::make_shared<MGMediator>(n);