double base detection + wings is ready

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 9409
+/home/matteo/reachability/devel/./cmake.lock 10354
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79

src/.vscode/c_cpp_properties.json

@@ -32,7 +32,8 @@
                 "/home/matteo/ws_moveit/src/rviz_visual_tools/include/**",
                 "/home/matteo/ws_moveit/src/srdfdom/include/**",
                 "/home/matteo/ws_moveit/src/warehouse_ros/include/**",
-        "/usr/include/**"
+                "/usr/include/**",
+                "${workspaceFolder}/mtc/include"
             ],
             "name": "ROS"
         }

src/.vscode/settings.json

@@ -8,5 +8,76 @@
         "/home/matteo/ws_panda/devel/lib/python3/dist-packages",
         "/home/matteo/ws_moveit/devel/lib/python3/dist-packages",
         "/opt/ros/noetic/lib/python3/dist-packages"
-    ]
+    ],
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "complex": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "regex": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "fstream": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "variant": "cpp"
+    }
 }
\ No newline at end of file

src/mtc/include/impl/abstract_map_loader.h

@@ -26,18 +26,18 @@ class Abstract_map_loader{
     Abstract_map_loader() = default;
     ~Abstract_map_loader() = default;
 
-    virtual void base_calculation()=0;
+    virtual std::vector<std::vector<pcl::PointXYZ>> base_calculation()=0;
 
-    inline std::vector<tf2::Transform> inv_map() {return inv_map_;}
-    inline std::vector<tf2::Transform> map() {return map_;}
+    inline std::vector<tf2::Transform>& inv_map() {return inv_map_;}
+    inline std::vector<tf2::Transform>& map() {return map_;}
     inline void set_inv_map(std::vector<tf2::Transform>& list) {inv_map_ = list;}
     inline void set_map(std::vector<tf2::Transform>& list) {map_ = list;}
     inline void set_task_grasps(std::vector<std::vector<tf2::Transform>>& lists_in_list) {task_grasps_ = lists_in_list;}
-    inline std::vector<std::vector<tf2::Transform>> task_grasps() {return task_grasps_;}
-    inline std::vector<std::vector<std::vector<tf2::Quaternion>>> target_rot() {return target_rot_;}
+    inline std::vector<std::vector<tf2::Transform>>& task_grasps() {return task_grasps_;}
+    inline std::vector<std::vector<std::vector<tf2::Quaternion>>>& target_rot() {return target_rot_;}
     inline void set_target_rot (std::vector<std::vector<std::vector<tf2::Quaternion>>>& rots) {target_rot_ = rots;}
     inline void set_target_cloud(std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& cloud) {target_cloud_ = cloud;}
-    inline std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_cloud () { return target_cloud_;}
+    inline std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& target_cloud () { return target_cloud_;}
 
     inline void set_strategy(Abstract_strategy* some_stratergy) {strategy_ = some_stratergy;}
     inline Abstract_strategy* strategy() {return strategy_;}

src/mtc/include/impl/abstract_mediator.h

@@ -6,6 +6,10 @@
 #include "impl/abstract_robot_element.h"
 #include "impl/robot.h"
 
+#include "octomap/octomap.h"
+#include <pcl/point_cloud.h>
+#include <pcl/octree/octree.h>
+
 #define box_size tf2::Vector3(0.0318f, 0.0636f, 0.091f)
 
 class Abstract_mediator{
@@ -14,16 +18,38 @@ class Abstract_mediator{
     std::vector<std::vector<tf2::Transform>> objects_;
     std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
     ros::Publisher* pub_;
+    std::vector<std::vector<pcl::PointXYZ>> result_vector_;
+    std::vector<Abstract_robot_element*> wings_;
+
 
 
 
     public:
-    Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){}
-    inline void connect_robots(Abstract_robot* robot) {
-        robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());
+    Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
+        wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right));
+        wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left));
+        wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid));
     }
-    
-    virtual void check_collision()=0;
+    inline void connect_robots(Abstract_robot* robot) {robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());}
+    inline void set_result_vector(std::vector<std::vector<pcl::PointXYZ>>& res) {result_vector_ = res;}
+    inline std::vector<std::vector<pcl::PointXYZ>>& result_vector() {return result_vector_;}
+    pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
+    std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
+    void add_wing(Robot* robot) {
+        if (robot->observers().size() == 0){
+            Abstract_robot_element* rviz_right = new Wing_rviz_decorator(wings_[0], robot->rviz_markers()[0]);
+            robot->register_observers(rviz_right);
+        } else if (robot->observers().size() == 1){
+            Abstract_robot_element* rviz_left = new Wing_rviz_decorator(wings_[1], robot->rviz_markers()[0]);
+            robot->register_observers(rviz_left);
+        } else if (robot->observers().size() == 2){
+            Abstract_robot_element* rviz_mid = new Wing_rviz_decorator(wings_[2], robot->rviz_markers()[0]);
+            robot->register_observers(rviz_mid);
+        } else {
+            ROS_INFO("[Warning] %s got to many wings", robot->name().c_str());
+        }
+    }
+    virtual bool check_collision( const int& robot)=0;
     virtual void mediate()=0;
 
 };

src/mtc/include/impl/abstract_robot.h

@@ -15,20 +15,34 @@ class Abstract_robot {
     protected:
     std::string name_;
     tf2::Transform tf_;
+    tf2::Transform root_tf_;
     std::vector<tf2::Transform> bounds_;
+    std::vector<tf2::Transform> robot_root_bounds_;
+
 
 
     public:
     Abstract_robot(std::string name, tf2::Transform tf) : name_(name), tf_(tf) {
+        root_tf_= tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.22f, 0, tf.getOrigin().getZ()));
+
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, 0.4f, tf.getOrigin().getZ()))); // ++
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, 0.4f, tf.getOrigin().getZ()))); // +-
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, -0.4f, tf.getOrigin().getZ()))); //-+
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, -0.4f, tf.getOrigin().getZ()))); // --
+
+        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, 0.095f, 0))); // ++
+        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, 0.095f, 0))); // +-
+        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0)));
+        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0)));
+
     };
     inline std::string& name() { return name_;}
     inline tf2::Transform& tf() { return tf_;}
-    inline void rotate(float deg) {tf2::Quaternion rot; rot.setRPY(0,0,deg); tf2::Transform t(rot, tf2::Vector3(0,0,0)); tf_= tf_* t;};
-    inline void translate(tf2::Vector3 t) {tf2::Transform tf(tf2::Quaternion(0,0,0,1), t); tf_= tf_* tf;};
+    inline tf2::Transform& root_tf() { return root_tf_;}
+    inline void set_tf(tf2::Transform& t) { tf_ = t;}
+    inline std::vector<tf2::Transform>& robot_root_bounds() { return robot_root_bounds_;}
+    inline void rotate(float deg) {tf2::Quaternion rot; rot.setRPY(0,0,deg); tf2::Transform t(rot, tf2::Vector3(0,0,0)); tf_= tf_* t;}
+    inline void translate(tf2::Vector3 t) {tf2::Transform tf(tf2::Quaternion(0,0,0,1), t); tf_*=tf;}
 
     virtual void notify()= 0;
     virtual bool check_single_object_collision(tf2::Transform& obj)= 0;

src/mtc/include/impl/map_loader.h

@@ -9,7 +9,7 @@
 class Map_loader : public Abstract_map_loader {
     public:
     Map_loader(XmlRpc::XmlRpcValue& map_data, XmlRpc::XmlRpcValue& target_data);
-    void base_calculation() override;
+    std::vector<std::vector<pcl::PointXYZ>> base_calculation() override;
 
 };
 

src/mtc/include/impl/mediator.h

@@ -8,67 +8,17 @@
 
 class Mediator : public Abstract_mediator{
     protected:
-    visualization_msgs::MarkerArray* rviz_markers_ = new visualization_msgs::MarkerArray();
+    visualization_msgs::MarkerArray* rviz_markers_ = new visualization_msgs::MarkerArray;
 
     public:
     Mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
-    void check_collision() override {
-        //for (int i = 0; i < objects_.size(); i++) {
-            for(int j = 0; j < objects_[0].size(); j++){
-                visualization_msgs::Marker marker;
-                marker.header.frame_id = "map";
-                marker.header.stamp = ros::Time();
-                marker.ns = "objects";
-                marker.id = j;
-                marker.type = visualization_msgs::Marker::CUBE;
-                marker.action = visualization_msgs::Marker::ADD;
-                marker.pose.position.x = objects_[0][j].getOrigin().getX();
-                marker.pose.position.y = objects_[0][j].getOrigin().getY();
-                marker.pose.position.z = objects_[0][j].getOrigin().getZ();
-                marker.pose.orientation.x = objects_[0][j].getRotation().getX();
-                marker.pose.orientation.y = objects_[0][j].getRotation().getY();
-                marker.pose.orientation.z = objects_[0][j].getRotation().getZ();
-                marker.pose.orientation.w = objects_[0][j].getRotation().getW();
-                marker.scale.x = box_size.getX();
-                marker.scale.y = box_size.getY();
-                marker.scale.z = box_size.getZ();
-                if(robots_[0]->check_single_object_collision(objects_[0][j])){
-                    ROS_INFO("true");
-                    marker.color.r = 0;
-                    marker.color.g = 1.0;
-                    marker.color.b = 0;
-                    marker.color.a = 1;
-                } else {
-                    ROS_INFO("false");
-                    marker.color.r = 1;
-                    marker.color.g = 0;
-                    marker.color.b = 0;
-                    marker.color.a = 1.0;
-                }
-                rviz_markers_->markers.push_back(marker);
-            }
-        //}
-        ROS_INFO("alle punkte durch");
-    }
+    bool check_collision(const int& robot) override;
+    void mediate() override;
 
-    void mediate() override {
-        ros::Rate loop_rate(1);
-        for (Abstract_robot* robot : robots_){
-            Robot* ceti = dynamic_cast<Robot*>(robot);
-            while (ros::ok()){
-                for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear();
-                rviz_markers_->markers.clear();
+    pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
+    std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
+    void approximation(Robot* ceti1);
 
-                ceti->rotate(-M_PI/4);
-                ceti->notify();
-                check_collision();
-                for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub_->publish(*markers);
-                pub_->publish(*rviz_markers_);
-                loop_rate.sleep();
-            }
-        }
-    }
 };
 
-
 #endif
\ No newline at end of file

src/mtc/include/impl/robot.h

@@ -20,6 +20,10 @@ class Robot : public Abstract_robot{
 
     public:
     Robot(std::string name, tf2::Transform tf) : Abstract_robot(name, tf){ 
+        generate_access_fields();
+    }
+
+    void generate_access_fields(){
         for (int i = 0; i <= 2; i++){
             for (int j = 0; j <= 2; j++){
                 if(i == 0 && j == 0) {continue;}
@@ -54,14 +58,25 @@ class Robot : public Abstract_robot{
 
     void notify() override {for(Abstract_robot_element* wing : observers_) wing->update(tf_); for(Abstract_robot_element* field : access_fields_) field->update(tf_);}
     bool check_single_object_collision(tf2::Transform& obj) override {
+
+        tf2::Transform A = tf_ * root_tf_ * robot_root_bounds_[0];
+        tf2::Transform B = tf_ * root_tf_ * robot_root_bounds_[1];
+        tf2::Transform C = tf_ * root_tf_ * robot_root_bounds_[2];
+        tf2::Transform D = tf_ * root_tf_ * robot_root_bounds_[3];
+
+        float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+        float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
+        if ((std::floor(sum*100)/100.f) <= full_area) {return false; } 
+        
+
         for (int i = 0; i < bounds_.size(); i++){
             tf2::Transform A = tf_ * bounds_[0];
             tf2::Transform B = tf_ * bounds_[1];
             tf2::Transform C = tf_ * bounds_[2];
             tf2::Transform D = tf_ * bounds_[3];
 
-            float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
-            float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
+            full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+            sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
 
 
             if ((std::floor(sum*100)/100.f) <= full_area) return true;
@@ -77,10 +92,8 @@ class Robot : public Abstract_robot{
                 tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
                 tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
                 
-                float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
-                float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
-
-                ROS_INFO("sum  %f", sum);
+                full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+                sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
 
                 if ((std::floor(sum*100)/100.f) <= full_area) {
                     return true;
@@ -91,6 +104,13 @@ class Robot : public Abstract_robot{
         }
         return false;
     }
+    void reset(){
+        observers_.clear();
+        access_fields_.clear();
+        generate_access_fields();
+        tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
+    }
+
 
     inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
     inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
@@ -133,4 +153,6 @@ class Robot : public Abstract_robot{
 };
 
 
+
+
 #endif
\ No newline at end of file

src/mtc/src/base_routine.cpp

@@ -14,13 +14,12 @@
 #include "impl/base_by_rotation.h"
 #include <xmlrpcpp/XmlRpc.h>
 
-#include "impl/map_loader.cpp"
+#include "impl/abstract_mediator.cpp"
 #include "impl/base_by_rotation.cpp"
-#include "impl/wing_rviz_decorator.cpp"
 #include "impl/field_rviz_decorator.cpp"
-
-
-
+#include "impl/map_loader.cpp"
+#include "impl/mediator.cpp"
+#include "impl/wing_rviz_decorator.cpp"
 
 
 int main(int argc, char **argv){
@@ -37,81 +36,50 @@ int main(int argc, char **argv){
     Abstract_map_loader* map_loader = new Map_loader(map, task);
     Abstract_strategy* strategy = new Base_by_rotation();
 
+    map_loader->set_strategy(strategy);
+    std::vector<std::vector<pcl::PointXYZ>> results = map_loader->base_calculation();
+
+    Abstract_mediator* mediator = new Mediator(map_loader->task_grasps(), new ros::Publisher(n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)));
+    mediator->set_result_vector(results);
 
-    visualization_msgs::MarkerArray* wings = new visualization_msgs::MarkerArray();
-    visualization_msgs::MarkerArray* fields = new visualization_msgs::MarkerArray();
+    visualization_msgs::MarkerArray* wings1 = new visualization_msgs::MarkerArray();
+    visualization_msgs::MarkerArray* wings2 = new visualization_msgs::MarkerArray();
+    visualization_msgs::MarkerArray* fields1 = new visualization_msgs::MarkerArray();
+    visualization_msgs::MarkerArray* fields2 = new visualization_msgs::MarkerArray();
 
 
     Abstract_robot* robo = new Robot(std::string("Robot_arm1"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)));
-    Abstract_robot_element* right_wing = new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,robo->tf().getOrigin().getZ()-0.005f)), right);
-    Abstract_robot_element* left_wing = new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,robo->tf().getOrigin().getZ()-0.005f)), left);
-    Abstract_robot_element* mid_wing = new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,robo->tf().getOrigin().getZ()-0.005f)), mid);
+    Robot* ceti = dynamic_cast<Robot*>(robo);
+    ceti->add_rviz_markers(wings1);
+    ceti->add_rviz_markers(fields1);
 
-    Abstract_robot_element* rviz_right = new Wing_rviz_decorator(right_wing, wings);
-    Abstract_robot_element* rviz_left = new Wing_rviz_decorator(left_wing, wings);
-    Abstract_robot_element* rviz_mid = new Wing_rviz_decorator(mid_wing, wings);
+    mediator->connect_robots(robo);
 
-    Robot* ceti = dynamic_cast<Robot*>(robo);
-    ceti->add_rviz_markers(wings);
-    ceti->add_rviz_markers(fields);
-    ceti->register_observers(rviz_right);
-    ceti->register_observers(rviz_left);
-    ceti->register_observers(rviz_mid);
+    Abstract_robot* robo2 = new Robot(std::string("Robot_arm2"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)));
+    Robot* ceti2 = dynamic_cast<Robot*>(robo2);
+    ceti2->add_rviz_markers(wings2);
+    ceti2->add_rviz_markers(fields2);
 
-    /*
-    for (int i = 0; i < ceti->access_fields().size(); i++){
-        ceti->access_fields()[i] = new Field_rviz_decorator(ceti->access_fields()[i], fields);
-    }
+    mediator->connect_robots(robo2);
+    mediator->mediate();
 
-    */
     /*
-    for (int i = 0; i < map_loader->task_grasps()[0].size(); i++) {
-        Abstract_object* obj = new Abstract_object(tf2::Transform(tf2::Quaternion(0,0,0,1), map_loader->task_grasps()[0][i]), box_size);
-
-        visualization_msgs::Marker marker;
-        marker.header.frame_id = "map";
-        marker.header.stamp = ros::Time();
-        marker.ns = *((char*)(&objects1));
-        marker.id = *((int*)(&obj));
-        marker.type = visualization_msgs::Marker::CUBE;
-        marker.action = visualization_msgs::Marker::ADD;
-        marker.pose.position.x = map_loader->task_grasps()[0][i].getX();
-        marker.pose.position.y = map_loader->task_grasps()[0][i].getY();
-        marker.pose.position.z = map_loader->task_grasps()[0][i].getZ() +0.025f;
-        marker.pose.orientation.x = 0;
-        marker.pose.orientation.y = 0;
-        marker.pose.orientation.z = 0;
-        marker.pose.orientation.w = 1;
-        marker.scale.x = box_size.getX();
-        marker.scale.y = box_size.getY();
-        marker.scale.z = box_size.getZ();
-        marker.color.r = 1.0;
-        marker.color.g = 0.0;
-        marker.color.b = 0.0;
-        marker.color.a = 1.0; 
-        objects1->markers.push_back(marker);
-        objects.push_back(obj);
+    for (int i = 0; i < ceti->access_fields1().size(); i++){
+        ceti->access_fields1()[i] = new Field_rviz_decorator(ceti->access_fields1()[i], fields1);
     }
     */
-    //map_loader->set_strategy(strategy);
-    //map_loader->base_calculation();
     
+    //free(rviz_right);
+    //free(rviz_mid);
+    //free(rviz_left);
 
-    Abstract_mediator* mediator = new Mediator(map_loader->task_grasps(), new ros::Publisher(n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)));
-    mediator->connect_robots(robo);
-    mediator->mediate();
 
-    free(rviz_right);
-    free(rviz_mid);
-    free(rviz_left);
-
-    // free(map_loader);
-    // free(strategy);
+    free(map_loader);
+    free(strategy);
 
     free(robo);
-    free(right_wing);
-    free(left_wing);
-    free(mid_wing);
+
+    free(robo2);
 
     return 0;
 }
\ No newline at end of file

src/mtc/src/impl/abstract_mediator.cpp

+#include "impl/abstract_mediator.h"
+
+
+std::vector<pcl::PointXYZ> Abstract_mediator::generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution){
+  std::vector<pcl::PointXYZ> ground_plane; 
+  
+  for (float x = origin.getX() - diameter * 1.5; x <= origin.getX() + diameter * 1.5; x += resolution){
+    for (float y = origin.getY() - diameter * 1.5; y <= origin.getY() + diameter * 1.5; y += resolution){
+      pcl::PointXYZ point(x,y, 0.4425f);
+      ground_plane.push_back(point);     
+    }
+  }
+  return ground_plane;
+}
+
+pcl::PointCloud< pcl::PointXYZ >::Ptr Abstract_mediator::vector_to_cloud(std::vector<pcl::PointXYZ>& vector){
+  pcl::PointCloud< pcl::PointXYZ >::Ptr task_voxel(new pcl::PointCloud< pcl::PointXYZ >);
+  for(pcl::PointXYZ& point : vector)
+    task_voxel->push_back(point);
+  
+  return task_voxel;
+}
\ No newline at end of file

src/mtc/src/impl/base_by_rotation.cpp

@@ -3,25 +3,22 @@
 
 void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) {
     ROS_INFO("starting base calculation strategy...");
-    ROS_INFO("condition based inv_map calculation...");
+    ROS_INFO("condition based [inv_map] calculation...");
     std::vector<tf2::Transform> trans;
     for(int i = 0; i < var->map().size(); i++) {
         for (int x = 0; x < var->target_rot().size(); x++) {
             for (int y = 0; y < var->target_rot()[x].size(); y++) {
                 for(int p = 0; p < var->target_rot()[x][y].size(); p++){
-                    if (var->target_rot()[x][y][p].angle(var->map()[i].getRotation()) < 0.436332f) {
-                        ROS_INFO("%f", var->target_rot()[x][y][p].angle(var->map()[i].getRotation()));
+                    if (var->target_rot()[x][y][p].angle(var->map()[i].getRotation()) < 0.349066f) {
                         trans.push_back(tf2::Transform(var->target_rot()[x][y][p], var->map()[i].getOrigin()).inverse());
                         break;
                     }                        
-                        
                 }
             }
         }
-        
     }
     var->set_inv_map(trans);
-    ROS_INFO("inv map caculated...");
+    ROS_INFO("caculated [inv_map] contains %i entrys...", var->inv_map().size());
 };
 
 void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
@@ -37,19 +34,19 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
     }
 
     // Maybe OpenMP
-    ROS_INFO("start cloud calculation...");
+    ROS_INFO("start [cloud] calculation...");
+    tf2::Transform root(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
     for (int i = 0; i < target_clouds.size(); i++){
         for (int j = 0; j < target_clouds[i].size(); j++) {
             for (int y = 0; y < var->inv_map().size(); y++) {
-                //ROS_INFO("%li %li", y, j );
                 for (int x = 0; x < var->target_rot()[i][j].size(); x++) {
                     tf2::Transform target = var->task_grasps()[i][j]; target.setRotation(var->target_rot()[i][j][x]);
-                    tf2::Transform base = target * var->inv_map()[y];
+                    tf2::Transform base = target * (var->inv_map()[y] * root);
                     target_clouds[i][j]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ()));
                 }
             }
         }
     }
-    ROS_INFO("calculation done...");
     var->set_target_cloud(target_clouds);
+    ROS_INFO("[cloud]calculation done...");
 }
\ No newline at end of file

src/mtc/src/impl/map_loader.cpp

@@ -8,8 +8,7 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){
     std::regex reg("\\s+");
     std::vector<tf2::Transform> _map;
 
-    ROS_INFO("template construction...");
-    ROS_INFO("load map data...");
+    ROS_INFO("load [map] data...");
     for (int i = 0; i < map.size();i++){
         std::string str = map[i];
         std::sregex_token_iterator to_it(str.begin(), str.end(),reg,-1);
@@ -20,11 +19,9 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){
         _map.push_back(tf2::Transform(so.normalize(),t));
     }
     this->set_map(_map);
+    ROS_INFO("[map] saved with %i entries...", _map.size());
 
-
-    ROS_INFO("map saved...");
-
-    ROS_INFO("saving tasks...");
+    ROS_INFO("saving [target] positions...");
     std::vector<std::vector<tf2::Transform>> task_;
     for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = task.begin(); it != task.end(); ++it){
         std::vector<tf2::Transform> task_per_robot;
@@ -36,11 +33,12 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){
         } 
         task_.push_back(task_per_robot);
     }
-    task_grasps_ = task_;
-    ROS_INFO("targets saved...");
+    this->set_task_grasps(task_);
+    
+    for (int i = 0; i < task_.size(); i++) ROS_INFO("[target] for Robot %i saved with %i entries...", i, task_[i].size());
 }
 
-void Map_loader::base_calculation(){
+std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){
 
     ROS_INFO("calculating target orientation basic set...");
     std::vector<tf2::Quaternion> basic_rot;
@@ -79,7 +77,7 @@ void Map_loader::base_calculation(){
     ROS_INFO("start cloud quantization...");
     for(int i = 0; i < target_cloud_.size();i++){
         for(int j = 0; j < target_cloud_[i].size();j++){
-            pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.4f);
+            pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.2f);
             octree.setInputCloud(target_cloud_[i][j]);
             octree.addPointsFromInputCloud();
             double min_x, min_y, min_z, max_x, max_y, max_z;
@@ -95,20 +93,37 @@ void Map_loader::base_calculation(){
             }
         }
     }
-    for(int i = 0; i < base_target_map.size(); i++) for(int j = 0; j < base_target_map[i].size(); j++)
-    for(int k = 0; k < base_target_map[i][j].size(); k++) ROS_INFO("%i ", base_target_map[i][j][k]);
+
+
+
+
+    std::vector<std::vector<pcl::PointXYZ>> resulting;
+    for(int i = 0; i < base_target_map.size(); i++) {
+        std::vector<pcl::PointXYZ> points_per_robot;
+        for(int j = 0; j < base_target_map[i].size(); j++){
+            if (base_target_map[i][j].size() == task_grasps_[i].size()) {
+                points_per_robot.push_back(voxelization[j]);
+            }
+        }
+        if (!points_per_robot.empty()) resulting.push_back(points_per_robot);
+    }
+
+    for (int i = 0; i < resulting.size(); i++) {
+        ROS_INFO("Robot %i got %i base positions to ckeck", i, resulting[i].size());
+    }
+    return resulting;
 }
 
 
 std::vector<pcl::PointXYZ> Abstract_map_loader::create_pcl_box(){
     tf2::Vector3 origin(0,0,0);
-    float resolution = 0.125f;
+    float resolution = 0.4f;
     float diameter = 3.0f;
     unsigned char depth = 16;
     std::vector<pcl::PointXYZ> box; 
     octomap::OcTree* tree = new octomap::OcTree(resolution/2);
-    for (float x = origin.getX() - diameter * 1.5 ; x <= origin.getX() + diameter * 1.5 ; x += resolution){
-        for (float y = origin.getY() - diameter * 1.5 ; y <= origin.getY() + diameter * 1.5 ; y += resolution){
+    for (float x = origin.getX() - diameter * 5 ; x <= origin.getX() + diameter * 5 ; x += resolution){
+        for (float y = origin.getY() - diameter * 5 ; y <= origin.getY() + diameter * 5 ; y += resolution){
             for (float z = origin.getZ() - diameter * 1.5 ; z <= origin.getZ() + diameter * 1.5 ; z += resolution){
                 octomap::point3d point(x,y,z);
                 tree->updateNode(point, true);
@@ -123,3 +138,4 @@ std::vector<pcl::PointXYZ> Abstract_map_loader::create_pcl_box(){
 
     return box;
 }
+

src/mtc/src/impl/mediator.cpp

+#include "impl/mediator.h"
+#include "impl/abstract_mediator.h"
+
+
+
+bool Mediator::check_collision( const int& robot){
+    bool succ = true;
+    for(int j = 0; j < objects_[robot].size(); j++){
+        visualization_msgs::Marker marker;
+        marker.header.frame_id = "map";
+        marker.header.stamp = ros::Time();
+        marker.ns = "objects ";
+        marker.id = j;
+        marker.type = visualization_msgs::Marker::CUBE;
+        marker.action = visualization_msgs::Marker::ADD;
+        marker.pose.position.x = objects_[robot][j].getOrigin().getX();
+        marker.pose.position.y = objects_[robot][j].getOrigin().getY();
+        marker.pose.position.z = objects_[robot][j].getOrigin().getZ();
+        marker.pose.orientation.x = objects_[robot][j].getRotation().getX();
+        marker.pose.orientation.y = objects_[robot][j].getRotation().getY();
+        marker.pose.orientation.z = objects_[robot][j].getRotation().getZ();
+        marker.pose.orientation.w = objects_[robot][j].getRotation().getW();
+        marker.scale.x = box_size.getX();
+        marker.scale.y = box_size.getY();
+        marker.scale.z = box_size.getZ();
+        if(robots_[robot]->check_single_object_collision(objects_[robot][j])){
+            marker.color.r = 0;
+            marker.color.g = 1.0;
+            marker.color.b = 0;
+            marker.color.a = 1;
+        } else {
+            marker.color.r = 1;
+            marker.color.g = 0;
+            marker.color.b = 0;
+            marker.color.a = 1.0;
+            succ = false;
+        }
+        rviz_markers_->markers.push_back(marker);
+    }
+    return succ;
+
+}
+
+void Mediator::mediate(){
+    ROS_INFO("assigne result to first robot...");
+    std::vector<pcl::PointXYZ> grCenter = Abstract_mediator::generate_Ground(tf2::Vector3(0,0,0), 3.0f, 0.1f);
+    std::vector<std::vector<tf2::Transform>> filter_per_robot;
+
+    pcl::octree::OctreePointCloudSearch< pcl::PointXYZ >first_cloud(0.4f);
+    first_cloud.setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[0]));
+    first_cloud.addPointsFromInputCloud();
+    std::vector<tf2::Transform> ground_per_robot;
+    for(pcl::PointXYZ& p : grCenter){
+        double min_x, min_y, min_z, max_x, max_y, max_z;
+        first_cloud.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
+        bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
+        if (isInBox && first_cloud.isVoxelOccupiedAtPoint(p)) {
+        std::vector< int > pointIdxVec;
+        if (first_cloud.voxelSearch(p, pointIdxVec)) ground_per_robot.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(p.x, p.y, p.z)));
+        }
+    }
+
+    for (int i = objects_[0].size()-1; i > 0; i--){
+        if(objects_[0][i].getOrigin().distance(objects_[1].back().getOrigin()) == 0) objects_[1].pop_back();
+    }
+
+    /*
+    for (int i = 0; i < result_vector_.size();i++){
+        result_clouds.push_back(pcl::octree::OctreePointCloudSearch< pcl::PointXYZ >(0.4f));
+        result_clouds[i].setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[i]));
+        result_clouds[i].addPointsFromInputCloud();
+        std::vector<tf2::Transform> ground_per_robot;
+        for(pcl::PointXYZ& p : grCenter){
+            double min_x, min_y, min_z, max_x, max_y, max_z;
+            result_clouds[i].getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
+            bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
+            if (isInBox && result_clouds[i].isVoxelOccupiedAtPoint(p)) {
+            std::vector< int > pointIdxVec;
+            if (result_clouds[i].voxelSearch(p, pointIdxVec)) ground_per_robot.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(p.x, p.y, p.z)));
+            }
+        }
+        filter_per_robot.push_back(ground_per_robot);
+    }    
+    
+
+    ros::Rate loop_rate(10);
+    for (int x = 0; x < robots_.size(); x++){
+        Robot* ceti = dynamic_cast<Robot*>(robots_[x]);
+        for (int i = 0; i < filter_per_robot[x].size(); i++){
+            robots_[x]->set_tf(filter_per_robot[x][i]);
+            for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
+                for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear();
+                rviz_markers_->markers.clear();
+                ceti->rotate(0.0872665f);
+                ceti->notify();
+                if (check_collision(x)) {
+                        ROS_INFO("robot %i found its position", x);
+
+                } else {
+                    continue;
+                }
+                for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub_->publish(*markers);
+                pub_->publish(*rviz_markers_);
+                loop_rate.sleep();
+            }
+
+        }
+    }
+    */
+
+    
+    ros::Rate loop_rate(10);
+    Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
+    for(int j = 0; j <= 3; j++){
+        for (int i = 0; i < ground_per_robot.size(); i++){
+            robots_[0]->set_tf(ground_per_robot[i]);
+            for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
+                for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) markers->markers.clear();
+                rviz_markers_->markers.clear();
+                ceti1->rotate(0.0872665f);
+                ceti1->notify();
+                if (check_collision(0)){
+                    approximation(ceti1);
+                }   else {
+                    continue;
+                }
+                for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers())pub_->publish(*markers);
+                pub_->publish(*rviz_markers_);
+            }
+        }
+        add_wing(ceti1);
+    }
+}
+
+void Mediator::approximation(Robot* robot){
+    ros::Rate loop_rate(1);
+    std::vector<pcl::PointXYZ> relative_ground;
+    pcl::octree::OctreePointCloudSearch< pcl::PointXYZ > cloud(0.4f);
+    cloud.setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[1]));
+    cloud.addPointsFromInputCloud();
+
+    Robot* ceti = dynamic_cast<Robot*>(robots_[1]);
+    for (int i = 0; i <= 3; i++){
+        for (Abstract_robot_element* fields : robot->access_fields()) {
+            ceti->set_tf(fields->world_tf());
+            for ( float p = 0; p < 2*M_PI; p += M_PI/2){
+                ceti->rotate(M_PI/2);
+                ceti->notify();
+                if (check_collision(1)) {
+                    ROS_INFO("should work");
+                    } else {
+                        continue;
+                    }
+                loop_rate.sleep();
+            }
+            //relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ()));
+        }
+        add_wing(ceti);
+    }
+    ceti->reset();
+}
+
+

src/mtc/src/impl/wing_rviz_decorator.cpp

@@ -15,7 +15,7 @@ void Wing_rviz_decorator::input_filter(tf2::Transform& tf) {
     visualization_msgs::Marker marker;
     marker.header.frame_id = "map";
     marker.header.stamp = ros::Time();
-    marker.ns = "some name";
+    marker.ns = "ceti_table ";
     marker.id = *((int*)(&markers_));
     marker.type = visualization_msgs::Marker::CUBE;
     marker.action = visualization_msgs::Marker::ADD;