object detection done

.catkin_tools/profiles/default/devel_collisions.txt

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

src/mtc/include/impl/abstract_robot.h

@@ -15,22 +15,20 @@ class Abstract_robot {
     protected:
     std::string name_;
     tf2::Transform tf_;
-    std::vector<std::vector<tf2::Transform>> bounds_;
+    std::vector<tf2::Transform> bounds_;
 
 
     public:
     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);
+        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()))); // --
     };
     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;};
+    inline void translate(tf2::Vector3 t) {tf2::Transform tf(tf2::Quaternion(0,0,0,1), t); tf_= tf_* tf;};
 
     virtual void notify()= 0;
     virtual bool check_single_object_collision(tf2::Transform& obj)= 0;

src/mtc/include/impl/mediator.h

@@ -52,7 +52,7 @@ class Mediator : public Abstract_mediator{
     }
 
     void mediate() override {
-        ros::Rate loop_rate(0.1);
+        ros::Rate loop_rate(1);
         for (Abstract_robot* robot : robots_){
             Robot* ceti = dynamic_cast<Robot*>(robot);
             while (ros::ok()){
@@ -61,9 +61,9 @@ class Mediator : public Abstract_mediator{
 
                 ceti->rotate(-M_PI/4);
                 ceti->notify();
-                //check_collision();
+                check_collision();
                 for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub_->publish(*markers);
-                //pub_->publish(*rviz_markers_);
+                pub_->publish(*rviz_markers_);
                 loop_rate.sleep();
             }
         }

src/mtc/include/impl/robot.h

@@ -45,15 +45,28 @@ class Robot : public Abstract_robot{
     inline void add_rviz_markers(visualization_msgs::MarkerArray* marker) {rviz_markers_.push_back(marker);}
     inline std::vector<visualization_msgs::MarkerArray*> rviz_markers(){return rviz_markers_;}
     inline float area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C){
-        return std::abs((
-                        A.getOrigin().getX() * (B.getOrigin().getY() - C.getOrigin().getY()) +
-                        B.getOrigin().getX() * (C.getOrigin().getY() - A.getOrigin().getY()) +
-                        C.getOrigin().getX() * (A.getOrigin().getY() - B.getOrigin().getY()))/2.0f);
+        return std::abs(
+                        (B.getOrigin().getX() * A.getOrigin().getY()) - (A.getOrigin().getX() * B.getOrigin().getY()) +
+                        (C.getOrigin().getX() * B.getOrigin().getY()) - (B.getOrigin().getX() * C.getOrigin().getY()) +
+                        (A.getOrigin().getX() * C.getOrigin().getY()) - (C.getOrigin().getX() * A.getOrigin().getY()))*0.5f;
     }
 
 
     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 {
+        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);
+
+
+            if ((std::floor(sum*100)/100.f) <= full_area) return true;
+        }
+
         if (!observers_.empty()){
             std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();            
             while (it != observers_.end()) {
@@ -65,20 +78,15 @@ class Robot : public Abstract_robot{
                 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(obj, A,B) + area_calculation(obj, B,C) + area_calculation(obj, C,D) + area_calculation(obj, A, D);
-
-                ROS_INFO("world coordinates %f %f %f", ceti->world_tf().getOrigin().getX(), ceti->world_tf().getOrigin().getY(), ceti->world_tf().getOrigin().getZ());
-                ROS_INFO("bounds1 %f %f %f", ceti->bounds()[0].getOrigin().getX(), ceti->bounds()[0].getOrigin().getY(), ceti->bounds()[0].getOrigin().getZ());
-                ROS_INFO("bounds2 %f %f %f", ceti->bounds()[1].getOrigin().getX(), ceti->bounds()[1].getOrigin().getY(), ceti->bounds()[1].getOrigin().getZ());
-                ROS_INFO("point 1 %f %f %f", A.getOrigin().getX(), A.getOrigin().getY(), A.getOrigin().getZ());
-                ROS_INFO("point 2 %f %f %f", B.getOrigin().getX(), B.getOrigin().getY(), B.getOrigin().getZ());
-                ROS_INFO("point to calculate %f %f %f", obj.getOrigin().getX(), obj.getOrigin().getY(), obj.getOrigin().getZ());
-                ROS_INFO("full area %f ", full_area);
+                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);
 
-                if(full_area == sum) return true;
+                if ((std::floor(sum*100)/100.f) <= full_area) {
+                    return true;
+                } else {
                     ++it;
-
+                }
             }
         }
         return false;

src/mtc/src/base_routine.cpp

@@ -58,11 +58,12 @@ int main(int argc, char **argv){
     ceti->register_observers(rviz_left);
     ceti->register_observers(rviz_mid);
 
+    /*
     for (int i = 0; i < ceti->access_fields().size(); i++){
         ceti->access_fields()[i] = new Field_rviz_decorator(ceti->access_fields()[i], fields);
     }
 
-    
+    */
     /*
     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);