fixes

.catkin_tools/profiles/default/devel_collisions.txt

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

.catkin_tools/profiles/default/packages/ceti/devel_manifest.txt

 ceti
 /home/matteo/reachability/devel/.private/ceti/cmake.lock /home/matteo/reachability/devel/./cmake.lock
-/home/matteo/reachability/devel/.private/ceti/share/ceti/cmake/cetiConfig.cmake /home/matteo/reachability/devel/share/ceti/cmake/cetiConfig.cmake
 /home/matteo/reachability/devel/.private/ceti/share/ceti/cmake/cetiConfig-version.cmake /home/matteo/reachability/devel/share/ceti/cmake/cetiConfig-version.cmake
+/home/matteo/reachability/devel/.private/ceti/share/ceti/cmake/cetiConfig.cmake /home/matteo/reachability/devel/share/ceti/cmake/cetiConfig.cmake
 /home/matteo/reachability/devel/.private/ceti/lib/pkgconfig/ceti.pc /home/matteo/reachability/devel/lib/pkgconfig/ceti.pc

.catkin_tools/profiles/default/packages/ceti_double/devel_manifest.txt

 ceti_double
 /home/matteo/reachability/devel/.private/ceti_double/cmake.lock /home/matteo/reachability/devel/./cmake.lock
-/home/matteo/reachability/devel/.private/ceti_double/share/ceti_double/cmake/ceti_doubleConfig-version.cmake /home/matteo/reachability/devel/share/ceti_double/cmake/ceti_doubleConfig-version.cmake
 /home/matteo/reachability/devel/.private/ceti_double/share/ceti_double/cmake/ceti_doubleConfig.cmake /home/matteo/reachability/devel/share/ceti_double/cmake/ceti_doubleConfig.cmake
+/home/matteo/reachability/devel/.private/ceti_double/share/ceti_double/cmake/ceti_doubleConfig-version.cmake /home/matteo/reachability/devel/share/ceti_double/cmake/ceti_doubleConfig-version.cmake
 /home/matteo/reachability/devel/.private/ceti_double/lib/pkgconfig/ceti_double.pc /home/matteo/reachability/devel/lib/pkgconfig/ceti_double.pc

.catkin_tools/profiles/default/packages/dual_panda_moveit_config/devel_manifest.txt

 dual_panda_moveit_config
 /home/matteo/reachability/devel/.private/dual_panda_moveit_config/cmake.lock /home/matteo/reachability/devel/./cmake.lock
-/home/matteo/reachability/devel/.private/dual_panda_moveit_config/share/dual_panda_moveit_config/cmake/dual_panda_moveit_configConfig-version.cmake /home/matteo/reachability/devel/share/dual_panda_moveit_config/cmake/dual_panda_moveit_configConfig-version.cmake
 /home/matteo/reachability/devel/.private/dual_panda_moveit_config/share/dual_panda_moveit_config/cmake/dual_panda_moveit_configConfig.cmake /home/matteo/reachability/devel/share/dual_panda_moveit_config/cmake/dual_panda_moveit_configConfig.cmake
+/home/matteo/reachability/devel/.private/dual_panda_moveit_config/share/dual_panda_moveit_config/cmake/dual_panda_moveit_configConfig-version.cmake /home/matteo/reachability/devel/share/dual_panda_moveit_config/cmake/dual_panda_moveit_configConfig-version.cmake
 /home/matteo/reachability/devel/.private/dual_panda_moveit_config/lib/pkgconfig/dual_panda_moveit_config.pc /home/matteo/reachability/devel/lib/pkgconfig/dual_panda_moveit_config.pc

.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt

 mtc
 /home/matteo/reachability/devel/.private/mtc/cmake.lock /home/matteo/reachability/devel/./cmake.lock
-/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtc-msg-paths.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtc-msg-paths.cmake
 /home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig-version.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig-version.cmake
 /home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig.cmake
-/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtc-msg-extras.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtc-msg-extras.cmake
-/home/matteo/reachability/devel/.private/mtc/share/roseus/ros/mtc/manifest.l /home/matteo/reachability/devel/share/roseus/ros/mtc/manifest.l
-/home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps.so /home/matteo/reachability/devel/lib/libmoveit_grasps.so
 /home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps_filter.so /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so
+/home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps.so /home/matteo/reachability/devel/lib/libmoveit_grasps.so
+/home/matteo/reachability/devel/.private/mtc/lib/pkgconfig/mtc.pc /home/matteo/reachability/devel/lib/pkgconfig/mtc.pc
+/home/matteo/reachability/devel/.private/mtc/lib/mtc/mtc2taskspace /home/matteo/reachability/devel/lib/mtc/mtc2taskspace
 /home/matteo/reachability/devel/.private/mtc/lib/mtc/base_routine /home/matteo/reachability/devel/lib/mtc/base_routine
 /home/matteo/reachability/devel/.private/mtc/lib/mtc/moveit_grasps_grasp_pipeline /home/matteo/reachability/devel/lib/mtc/moveit_grasps_grasp_pipeline
-/home/matteo/reachability/devel/.private/mtc/lib/mtc/mtc2taskspace /home/matteo/reachability/devel/lib/mtc/mtc2taskspace
-/home/matteo/reachability/devel/.private/mtc/lib/python3/dist-packages/mtc/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/mtc/__init__.py
-/home/matteo/reachability/devel/.private/mtc/lib/pkgconfig/mtc.pc /home/matteo/reachability/devel/lib/pkgconfig/mtc.pc

.catkin_tools/profiles/default/packages/panda_moveit_config/devel_manifest.txt

 panda_moveit_config
 /home/matteo/reachability/devel/.private/panda_moveit_config/cmake.lock /home/matteo/reachability/devel/./cmake.lock
-/home/matteo/reachability/devel/.private/panda_moveit_config/share/panda_moveit_config/cmake/panda_moveit_configConfig-version.cmake /home/matteo/reachability/devel/share/panda_moveit_config/cmake/panda_moveit_configConfig-version.cmake
 /home/matteo/reachability/devel/.private/panda_moveit_config/share/panda_moveit_config/cmake/panda_moveit_configConfig.cmake /home/matteo/reachability/devel/share/panda_moveit_config/cmake/panda_moveit_configConfig.cmake
+/home/matteo/reachability/devel/.private/panda_moveit_config/share/panda_moveit_config/cmake/panda_moveit_configConfig-version.cmake /home/matteo/reachability/devel/share/panda_moveit_config/cmake/panda_moveit_configConfig-version.cmake
 /home/matteo/reachability/devel/.private/panda_moveit_config/lib/pkgconfig/panda_moveit_config.pc /home/matteo/reachability/devel/lib/pkgconfig/panda_moveit_config.pc

.catkin_tools/profiles/default/packages/reachability/devel_manifest.txt

 reachability
 /home/matteo/reachability/devel/.private/reachability/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/reachability/share/roseus/ros/reachability/manifest.l /home/matteo/reachability/devel/share/roseus/ros/reachability/manifest.l
 /home/matteo/reachability/devel/.private/reachability/share/reachability/cmake/reachabilityConfig.cmake /home/matteo/reachability/devel/share/reachability/cmake/reachabilityConfig.cmake
-/home/matteo/reachability/devel/.private/reachability/share/reachability/cmake/reachabilityConfig-version.cmake /home/matteo/reachability/devel/share/reachability/cmake/reachabilityConfig-version.cmake
 /home/matteo/reachability/devel/.private/reachability/share/reachability/cmake/reachability-msg-extras.cmake /home/matteo/reachability/devel/share/reachability/cmake/reachability-msg-extras.cmake
 /home/matteo/reachability/devel/.private/reachability/share/reachability/cmake/reachability-msg-paths.cmake /home/matteo/reachability/devel/share/reachability/cmake/reachability-msg-paths.cmake
-/home/matteo/reachability/devel/.private/reachability/share/roseus/ros/reachability/manifest.l /home/matteo/reachability/devel/share/roseus/ros/reachability/manifest.l
-/home/matteo/reachability/devel/.private/reachability/lib/reachability/map_analyse /home/matteo/reachability/devel/lib/reachability/map_analyse
-/home/matteo/reachability/devel/.private/reachability/lib/reachability/visualization /home/matteo/reachability/devel/lib/reachability/visualization
-/home/matteo/reachability/devel/.private/reachability/lib/reachability/single_pick /home/matteo/reachability/devel/lib/reachability/single_pick
-/home/matteo/reachability/devel/.private/reachability/lib/reachability/reach_map /home/matteo/reachability/devel/lib/reachability/reach_map
+/home/matteo/reachability/devel/.private/reachability/share/reachability/cmake/reachabilityConfig-version.cmake /home/matteo/reachability/devel/share/reachability/cmake/reachabilityConfig-version.cmake
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/inverse_map /home/matteo/reachability/devel/lib/reachability/inverse_map
-/home/matteo/reachability/devel/.private/reachability/lib/reachability/pick_and_place /home/matteo/reachability/devel/lib/reachability/pick_and_place
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/task_viewer /home/matteo/reachability/devel/lib/reachability/task_viewer
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/eval_analyse /home/matteo/reachability/devel/lib/reachability/eval_analyse
-/home/matteo/reachability/devel/.private/reachability/lib/python3/dist-packages/reachability/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/reachability/__init__.py
+/home/matteo/reachability/devel/.private/reachability/lib/reachability/reach_map /home/matteo/reachability/devel/lib/reachability/reach_map
+/home/matteo/reachability/devel/.private/reachability/lib/reachability/single_pick /home/matteo/reachability/devel/lib/reachability/single_pick
+/home/matteo/reachability/devel/.private/reachability/lib/reachability/map_analyse /home/matteo/reachability/devel/lib/reachability/map_analyse
+/home/matteo/reachability/devel/.private/reachability/lib/reachability/visualization /home/matteo/reachability/devel/lib/reachability/visualization
+/home/matteo/reachability/devel/.private/reachability/lib/reachability/pick_and_place /home/matteo/reachability/devel/lib/reachability/pick_and_place
 /home/matteo/reachability/devel/.private/reachability/lib/pkgconfig/reachability.pc /home/matteo/reachability/devel/lib/pkgconfig/reachability.pc
+/home/matteo/reachability/devel/.private/reachability/lib/python3/dist-packages/reachability/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/reachability/__init__.py

src/mtc/include/impl/abstract_map_loader.h

@@ -16,7 +16,7 @@ class Abstract_map_loader{
     protected:
     std::vector<tf2::Transform> inv_map_;
     std::vector<tf2::Transform> map_;
-    std::vector<std::vector<tf2::Vector3>> task_grasps_;
+    std::vector<std::vector<tf2::Transform>> task_grasps_;
     std::vector<std::vector<std::vector<tf2::Quaternion>>> target_rot_;
     std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_cloud_;
 
@@ -32,8 +32,8 @@ class Abstract_map_loader{
     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::Vector3>>& lists_in_list) {task_grasps_ = lists_in_list;}
-    inline std::vector<std::vector<tf2::Vector3>> task_grasps() {return task_grasps_;}
+    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 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;}

src/mtc/include/impl/abstract_mediator.h

@@ -6,52 +6,24 @@
 #include "impl/abstract_robot_element.h"
 #include "impl/robot.h"
 
+#define box_size tf2::Vector3(0.0318f, 0.0636f, 0.091f)
+
 class Abstract_mediator{
     protected:
     std::vector<Abstract_robot*> robots_;
-    std::vector<std::vector<tf2::Vector3>> objects_;
+    std::vector<std::vector<tf2::Transform>> objects_;
     std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
     ros::Publisher* pub_;
 
 
 
     public:
-    Abstract_mediator(std::vector<std::vector<tf2::Vector3>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){}
+    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());
-        Robot* ceti = dynamic_cast<Robot*>(robot);
-        std::vector<tf2::Transform> plane;
-        plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.4f, 0.4f, ceti->tf().getOrigin().getZ())));
-        plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, 0.4f, ceti->tf().getOrigin().getZ())));
-        plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.4f, -0.4f, ceti->tf().getOrigin().getZ())));
-        plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, -0.4f, ceti->tf().getOrigin().getZ())));
-        std::vector<std::vector<tf2::Transform>> planes;
-
-        for(Abstract_robot_element* wing : ceti->observers()){
-            plane.clear();
-            if (wing->relative_tf().getOrigin().getY() > 0){
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(right.getX()/2.f, right.getY()/2.f,0)));
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(right.getX()/2.f, right.getY()/-2.f,0)));
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(right.getX()/-2.f, right.getY()/2.f,0)));
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(right.getX()/-2.f, right.getY()/-2.f,0)));
-            } else 
-            if (wing->relative_tf().getOrigin().getY() < 0){
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(left.getX()/2.f, left.getY()/2.f,0)));
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(left.getX()/2.f, left.getY()/-2.f,0)));
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(left.getX()/-2.f, left.getY()/2.f,0)));
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(left.getX()/-2.f, left.getY()/-2.f,0)));
-            } else
-            if (wing->relative_tf().getOrigin().getY() == 0){
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(mid.getX()/2.f, mid.getY()/2.f,0)));
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(mid.getX()/2.f, mid.getY()/-2.f,0)));
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(mid.getX()/-2.f, mid.getY()/2.f,0)));
-                plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(mid.getX()/-2.f, mid.getY()/-2.f,0)));
-            }
-            planes.push_back(plane);
-        }
-        relative_bounds_.push_back(planes);
     }
     
+    virtual void check_collision()=0;
     virtual void mediate()=0;
 
 };

src/mtc/include/impl/abstract_robot.h

@@ -15,16 +15,25 @@ class Abstract_robot {
     protected:
     std::string name_;
     tf2::Transform tf_;
+    std::vector<std::vector<tf2::Transform>> bounds_;
 
 
     public:
-    Abstract_robot(std::string name, tf2::Transform tf) : name_(name), tf_(tf){};
+    Abstract_robot(std::string name, tf2::Transform tf) : name_(name), tf_(tf) {
+        std::vector<tf2::Transform> plane;
+        plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.4f, 0.4f, tf.getOrigin().getZ())));
+        //plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, 0.4f, ceti->tf().getOrigin().getZ())));
+        //plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.4f, -0.4f, ceti->tf().getOrigin().getZ())));
+        plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, -0.4f, tf.getOrigin().getZ())));
+        bounds_.push_back(plane);
+    };
     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(1,0,0,0), t); tf_= tf_* tf;};
 
     virtual void notify()= 0;
+    virtual bool check_single_object_collision(tf2::Transform& obj)= 0;
 };
 
 

src/mtc/include/impl/abstract_robot_element.h

@@ -11,7 +11,6 @@ class Abstract_robot_element {
     tf2::Transform relative_tf_;
     tf2::Transform world_tf_;
 
-
     public:
     Abstract_robot_element() = default;
     inline void set_relative_tf(tf2::Transform tf) { relative_tf_= tf;}

src/mtc/include/impl/mediator.h

@@ -7,17 +7,62 @@
 #include "impl/robot.h"
 
 class Mediator : public Abstract_mediator{
+    protected:
+    visualization_msgs::MarkerArray* rviz_markers_ = new visualization_msgs::MarkerArray();
+
     public:
-    Mediator(std::vector<std::vector<tf2::Vector3>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
+    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 = *((int*)(&marker));
+                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);
+            }
+        }
+    }
+
     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();
+
                 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();
             }
         }

src/mtc/include/impl/robot.h

@@ -5,6 +5,7 @@
 #include "impl/abstract_robot.h"
 #include "impl/abstract_robot_element.h"
 #include "impl/abstract_robot_element_decorator.h"
+#include "impl/wing.h"
 #include "impl/wing_rviz_decorator.h"
 #include "impl/field.h"
 
@@ -45,11 +46,30 @@ class Robot : public Abstract_robot{
     inline std::vector<visualization_msgs::MarkerArray*> rviz_markers(){return rviz_markers_;}
 
     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 {
+        bool result = true;
+        if (!observers_.empty()){
+            std::vector<Abstract_robot_element*>::iterator it = observers_.begin();            
+            while (result && it != observers_.end()) {
+                Wing* ceti = dynamic_cast<Wing*>(it);
+                tf2::Transform point_1 = ceti->bounds()[0] * ceti->world_tf();
+                tf2::Transform point_2 = ceti->bounds()[1] * ceti->world_tf();
+                result = result && (obj.getOrigin().getX() > point_1.getOrigin().getX()) && 
+                        (obj.getOrigin().getX() < point_2.getOrigin().getX()) &&
+                        (obj.getOrigin().getY() > point_1.getOrigin().getY()) &&
+                        (obj.getOrigin().getY() < point_2.getOrigin().getY());
+                ++it;
+            }
+        }
+        return result;
+    }
+
     inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
     inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
     void register_observers(Abstract_robot_element* wd){
         Abstract_robot_element_decorator* decorator = dynamic_cast<Abstract_robot_element_decorator*>(wd);
         observers_.push_back(wd);
+        std::vector<tf2::Transform> plane;
         bool found = false;
         int index = 0;
 

src/mtc/include/impl/wing.h

@@ -7,10 +7,16 @@
 class Wing  : public Abstract_robot_element{
     private:
     tf2::Vector3 size_;
+    std::vector<tf2::Transform> bounds_;
 
     public:
-    Wing(tf2::Transform tf, tf2::Vector3 size) : size_(size){ relative_tf_ = tf;}
+    Wing(tf2::Transform tf, tf2::Vector3 size) : size_(size){ 
+        relative_tf_ = tf;
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/2.f, size_.getY()/2.f,0)));
+        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/-2.f,0)));
+        }
     inline tf2::Vector3 size(){ return size_;}
+    inline std::vector<tf2::Transform> bounds() {return bounds_;}
     void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
 };
 

src/mtc/src/base_routine.cpp

@@ -91,10 +91,10 @@ int main(int argc, char **argv){
         objects1->markers.push_back(marker);
         objects.push_back(obj);
     }
-    
-    map_loader->set_strategy(strategy);
-    map_loader->base_calculation();
     */
+    //map_loader->set_strategy(strategy);
+    //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->connect_robots(robo);

src/mtc/src/impl/base_by_rotation.cpp

@@ -43,7 +43,8 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) {
             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 base = tf2::Transform(var->target_rot()[i][j][x], var->task_grasps()[i][j]) * var->inv_map()[y];
+                    tf2::Transform target = var->task_grasps()[i][j]; target.setRotation(var->target_rot()[i][j][x]);
+                    tf2::Transform base = target * var->inv_map()[y];
                     target_clouds[i][j]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ()));
                 }
             }

src/mtc/src/impl/map_loader.cpp

@@ -25,14 +25,14 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){
     ROS_INFO("map saved...");
 
     ROS_INFO("saving tasks...");
-    std::vector<std::vector<tf2::Vector3>> task_;
+    std::vector<std::vector<tf2::Transform>> task_;
     for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = task.begin(); it != task.end(); ++it){
-        std::vector<tf2::Vector3> task_per_robot;
+        std::vector<tf2::Transform> task_per_robot;
         for (int i = 0; i < it->second.size(); i++){
             std::string str = it->second[i];
             std::sregex_token_iterator to_it(str.begin(), str.end(),reg,-1);
             std::vector<std::string> temp{to_it, {}};
-            task_per_robot.push_back(tf2::Vector3(std::stof(temp[0]), std::stof(temp[1]), std::stof(temp[2])));
+            task_per_robot.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(std::stof(temp[0]), std::stof(temp[1]), std::stof(temp[2]))));
         } 
         task_.push_back(task_per_robot);
     }
@@ -79,7 +79,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.125f);
+            pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.4f);
             octree.setInputCloud(target_cloud_[i][j]);
             octree.addPointsFromInputCloud();
             double min_x, min_y, min_z, max_x, max_y, max_z;
@@ -93,12 +93,10 @@ void Map_loader::base_calculation(){
                 }
 
             }
-            ROS_INFO(".");
         }
     }
     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].size());
-    ROS_INFO("start cloud quantization...");
+    for(int k = 0; k < base_target_map[i][j].size(); k++) ROS_INFO("%i ", base_target_map[i][j][k]);
 }