soome fixes

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 8726
+/home/matteo/reachability/devel/./cmake.lock 8732
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 66
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 66

src/mtc/include/impl/abstract_mediator.h

@@ -3,16 +3,57 @@
 
 #include "ros/ros.h"
 #include "impl/abstract_robot.h"
+#include "impl/abstract_robot_element.h"
+#include "impl/robot.h"
 
 class Abstract_mediator{
     protected:
     std::vector<Abstract_robot*> robots_;
     std::vector<std::vector<tf2::Vector3>> objects_;
+    std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
+    ros::Publisher* pub_;
+
 
 
     public:
-    Abstract_mediator(std::vector<std::vector<tf2::Vector3>> objects) : objects_(objects){};
-    virtual 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::Vector3>> 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 mediate()=0;
+
 };
 
 

src/mtc/include/impl/abstract_robot.h

@@ -7,6 +7,9 @@
 #include "impl/abstract_strategy.h"
 #include "impl/abstract_robot_element.h"
 
+#define right tf2::Vector3(0.7f, 0.5f, 0.01f)
+#define left tf2::Vector3(0.7f, 0.5f, 0.01f)
+#define mid tf2::Vector3(0.5f, 0.7f, 0.01f)
 
 class Abstract_robot {
     protected:

src/mtc/include/impl/mediator.h

@@ -3,10 +3,25 @@
 
 #include "ros/ros.h"
 #include "impl/abstract_mediator.h"
+#include "impl/abstract_robot.h"
+#include "impl/robot.h"
 
 class Mediator : public Abstract_mediator{
     public:
-    Mediator(std::vector<std::vector<tf2::Vector3>> objects) : Abstract_mediator(objects){};
+    Mediator(std::vector<std::vector<tf2::Vector3>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
+    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();
+                ceti->rotate(-M_PI/4);
+                ceti->notify();
+                for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub_->publish(*markers);
+                loop_rate.sleep();
+            }
+        }
+    }
 };
 
 

src/mtc/src/base_routine.cpp

@@ -19,9 +19,6 @@
 #include "impl/wing_rviz_decorator.cpp"
 #include "impl/field_rviz_decorator.cpp"
 
-#define right tf2::Vector3(0.7f, 0.5f, 0.01f)
-#define left tf2::Vector3(0.7f, 0.5f, 0.01f)
-#define mid tf2::Vector3(0.5f, 0.7f, 0.01f)
 
 
 
@@ -99,20 +96,9 @@ int main(int argc, char **argv){
     map_loader->base_calculation();
     */
 
-    Abstract_mediator* mediator = new Mediator(map_loader->task_grasps());
+    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);
-
-    ros::Rate loop_rate(1);
-    ros::Publisher* pub = new ros::Publisher(n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1));
-
-    while (ros::ok()){
-        fields->markers.clear(); 
-        wings->markers.clear();
-        ceti->rotate(-M_PI/4);
-        ceti->notify();
-        for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub->publish(*markers);
-        loop_rate.sleep();
-    }
+    mediator->mediate();
     
     free(rviz_right);
     free(rviz_mid);
@@ -121,7 +107,6 @@ int main(int argc, char **argv){
     // free(map_loader);
     // free(strategy);
 
-    free(pub);
     free(robo);
     free(right_wing);
     free(left_wing);