...

CMakeLists.txt

@@ -25,9 +25,11 @@ find_package(catkin REQUIRED COMPONENTS
   trajectory_msgs
   pcl_ros
 
+  ccf
   gb_grasp
   moveit_grasps
   behaviortree_cpp_v3
+  paho_mqtt
 )
 
 find_package(rostest REQUIRED)
@@ -153,6 +155,43 @@ src/mediator/moveit_mediator.cpp
 src/mediator/grasp_mediator.cpp
 
 
+src/reader/abstract_param_reader.cpp
+src/reader/cuboid_reader.cpp
+src/reader/job_reader.cpp
+src/reader/map_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/task_space_reader.cpp
+src/reader/ts_reader.cpp
+src/reader/wing_reader.cpp
+
+src/robot/decorators/panda_decorator.cpp
+src/robot/decorators/ur5_decorator.cpp
+src/robot/decorators/ur10_decorator.cpp
+src/robot/abstract_robot.cpp
+src/robot/ceti_robot.cpp
+
+src/robot_element/decorators/abstract_robot_element_decorator.cpp
+src/robot_element/decorators/log_decorator.cpp
+src/robot_element/observers/moveit_panel.cpp
+src/robot_element/observers/panel.cpp
+src/robot_element/observers/rviz_panel.cpp
+)
+
+add_executable(mg_routine src/mg_routine.cpp
+src/base_bridge/simple_base_implementation.cpp
+src/base_bridge/simple_base.cpp
+
+src/bt/execution.cpp
+src/bt/position_condition.cpp
+src/bt/parallel_robot.cpp
+
+src/mediator/abstract_mediator.cpp
+src/mediator/base_calculation_mediator.cpp
+src/mediator/moveit_mediator.cpp
+src/mediator/grasp_mediator.cpp
+src/mediator/mg_mediator.cpp
+
+
 src/reader/abstract_param_reader.cpp
 src/reader/cuboid_reader.cpp
 src/reader/job_reader.cpp
@@ -193,6 +232,7 @@ add_dependencies(config_routine ${config_routine_EXPORTED_TARGETS} ${catkin_EXPO
 add_dependencies(cell_routine ${cell_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(mtc2taskspace ${mtc2taskspace_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(robot_base_calculation_approach ${robot_base_calculation_approach_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(mg_routine ${mg_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 
 target_link_libraries(robot_base_calculation_approach 
@@ -203,6 +243,14 @@ target_link_libraries(robot_base_calculation_approach
   yaml-cpp
 )
 
+target_link_libraries(mg_routine 
+  ${catkin_LIBRARIES}
+  ${OCTOMAP_LIBRARIES}
+  ${PCL_LIBRARY_DIRS}
+  ${BEHAVIOR_TREE_LIBRARY}
+  yaml-cpp
+)
+
 target_link_libraries(config_routine 
   ${catkin_LIBRARIES}
   ${OCTOMAP_LIBRARIES}

include/mediator/mg_mediator.h

+#ifndef MG_MEDIATOR_
+#define MG_MEDIATOR_
+
+#include <ros/ros.h>
+#include "mediator/abstract_mediator.h"
+#include "robot_element/observers/moveit_panel.h"
+#include "robot_element/decorators/log_decorator.h"
+#include <ccf/connection/MqttConnection.h>
+#include <ccf/controller/Controller.h>
+
+
+//should be a mix between MG, bahvior tree, moveit task constructor
+class MGMediator: public AbstractMediator, virtual Controller{
+    protected:
+    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::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::string referenceRobot_; //!< Reference Robot
+    std::string resultPath_;//!< path to the result
+    std::map<const std::string, Cuboid> relevant_boxes_;
+
+    public:
+    MGMediator(std::shared_ptr<ros::NodeHandle> const& nh);
+
+    //! mediator function
+    /*!
+    Construction of robots and planning scenes. Calculate plans in preor to chain them so that they can be executed in parallel.
+    */
+    void mediate() override;
+
+    //! connect robot and initialize Moveit components
+    /*!
+    Set up acm_ and rs_ members to track, merge, and publish changes during execution.
+    \param robot RobotDecorator of Robot
+    */
+    void connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) override; 
+
+    //!  Sets panels for robots
+    void setPanel() override;
+
+    void publishTables();
+
+};
+#endif
\ No newline at end of file

src/mediator/mg_mediator.cpp

+#include "mediator/mg_mediator.h"
+
+MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh) 
+: AbstractMediator(nh)
+, Controller(*nh.get())
+{   
+    addConnection(std::make_shared<MqttConnection>("", "super"));
+}
+
+void MGMediator::mediate(){
+    setPanel();
+    publishTables();
+
+
+};
+
+//! connect robot and initialize Moveit components
+/*!
+Set up acm_ and rs_ members to track, merge, and publish changes during execution.
+\param robot RobotDecorator of Robot
+*/
+void MGMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot){
+    ROS_INFO("connecting %s...", robot->name().c_str());
+	robot->spezifieRobotGroups();
+	acm_.insert(std::pair<std::string, std::vector<uint8_t>>(robot->map().at("base").c_str(), std::vector<uint8_t>()));
+
+	for(auto name: robot->mgi()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
+	for(auto name: robot->mgiHand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
+
+	for (auto link : robot->mgi()->getLinkNames()) rs_.insert_or_assign(link, std::vector<std::uint8_t>());
+	rs_.insert_or_assign(robot->map()["left_finger"], std::vector<std::uint8_t>());
+	rs_.insert_or_assign(robot->map()["right_finger"], std::vector<std::uint8_t>());
+	rs_.insert_or_assign(robot->map()["hand_link"], std::vector<std::uint8_t>());
+	rs_.insert_or_assign(robot->map()["base"], std::vector<std::uint8_t>());
+
+	robots_.insert_or_assign(robot->name(), std::move(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();
+			}
+        }
+    }
+
+    std::string str;
+    std::map<const std::string, std::vector<Cuboid>> robot_to_cuboid;
+    std::bitset<3> panel_mask;
+
+    for (const auto& pair : robots_) {        
+        robot_to_cuboid.insert_or_assign(pair.first,  std::vector<Cuboid>());
+        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));
+            if(pair.second->checkSingleObjectCollision(trans, str, panel_mask)) robot_to_cuboid[pair.first].push_back(c);
+        }
+    }
+}
+
+void MGMediator::setPanel(){
+	auto wd = wing_reader_->wingData();
+
+	for (const auto& s_r : robots_){
+        try{
+            auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
+            ceti_bot->setObserverMask(static_cast<wing_config>(wd.at(s_r.first).second));
+        } catch(const std::out_of_range& oor) {
+            ROS_INFO("No mask data for %s", s_r.first.c_str());
+        }
+    }
+
+	for (const auto& s_r : robots_){
+        auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
+        std::vector<std::unique_ptr<AbstractRobotElementDecorator>> panels(3);
+            
+        for (std::size_t j = 0; j < ceti_bot->observerMask().size(); j++){
+            if (ceti_bot->observerMask()[j]){
+                try{
+                    tf2::Transform relative_tf = ceti_bot->tf().inverse() * wd.at(ceti_bot->name()).first[j].pose_;
+
+                    std::unique_ptr<AbstractRobotElement> panel = std::make_unique<MoveitPanel>(wd.at(s_r.first).first[j].name_, relative_tf, wd.at(s_r.first).first[j].size_);
+                    std::unique_ptr<AbstractRobotElementDecorator> log = std::make_unique<LogDecorator>(std::move(panel));
+                    panels[j] = std::move(log);
+                } catch (std::out_of_range &oor){
+                    ROS_INFO("OOR in set_panel");
+                }
+            }
+        }
+        ceti_bot->setObservers(panels);
+    }
+
+	for (const auto& s_r : robots_){
+		s_r.second->notify();
+	}
+}

src/mg_routine.cpp

+#include <xmlrpcpp/XmlRpc.h>
+
+#include "mediator/mg_mediator.h"
+#include "mediator/abstract_mediator.h"
+#include "robot/decorators/panda_decorator.h"
+
+#include "reader/abstract_param_reader.h"
+#include "reader/robot_reader.h"
+#include "reader/wing_reader.h"
+#include "reader/cuboid_reader.h"
+
+#include "robot/abstract_robot.h"
+#include "robot/decorators/panda_decorator.h"
+#include "robot_element/observers/moveit_panel.h"
+
+
+int main(int argc, char **argv){
+    ros::init(argc, argv, "mg_routine");
+    std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
+
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    std::shared_ptr<MGMediator> mediator = std::make_shared<MGMediator>(n);
+
+    auto rd = mediator->robotReader()->robotData();
+    for (int i = 0; i < rd.size() ;i++){
+        std::unique_ptr<AbstractRobot> ceti = std::make_unique<CetiRobot>(rd[i].name_, rd[i].pose_, rd[i].size_);
+        std::unique_ptr<PandaDecorator> ceti_panda = std::make_unique<PandaDecorator>(std::move(ceti));
+        mediator->connectRobots(std::move(ceti_panda));
+    }
+
+    //mediator->set_dirname(filename);    
+    mediator->mediate();
+
+    while (ros::ok()){
+        ros::spinOnce();
+    }
+}
\ No newline at end of file