diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9c208bff20e0dc367bc9a20dc42d45681450c847..472609c81198b0a9cf9fbc07cf19c8858ca8bafa 100644
--- a/CMakeLists.txt
+++ b/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}
diff --git a/include/mediator/mg_mediator.h b/include/mediator/mg_mediator.h
new file mode 100644
index 0000000000000000000000000000000000000000..98eadbf7eb516a8dc98a4b2a01ba3db546de0a6c
--- /dev/null
+++ b/include/mediator/mg_mediator.h
@@ -0,0 +1,51 @@
+#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
diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd87e8cb167ae59acd8511e361d87b75f48ea1f9
--- /dev/null
+++ b/src/mediator/mg_mediator.cpp
@@ -0,0 +1,103 @@
+#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();
+ }
+}
diff --git a/src/mg_routine.cpp b/src/mg_routine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..811cdf5d3b486fa97796c9c99db4a280a3f8c57f
--- /dev/null
+++ b/src/mg_routine.cpp
@@ -0,0 +1,39 @@
+#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