area collision calculation

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 9044
+/home/matteo/reachability/devel/./cmake.lock 9221
 /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_element.h

@@ -14,7 +14,7 @@ class Abstract_robot_element {
     public:
     Abstract_robot_element() = default;
     inline void set_relative_tf(tf2::Transform tf) { relative_tf_= tf;}
-    virtual tf2::Transform& relative_tf(){ return relative_tf_;}
+    inline tf2::Transform& relative_tf(){ return relative_tf_;}
     inline void calc_world_tf(tf2::Transform& tf) {world_tf_= tf * relative_tf_;}
     inline void set_world_tf(tf2::Transform& tf) { world_tf_ = tf;}
     inline tf2::Transform& world_tf() { return world_tf_;}

src/mtc/include/impl/mediator.h

@@ -13,13 +13,13 @@ class Mediator : public Abstract_mediator{
     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 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.id = j;
                 marker.type = visualization_msgs::Marker::CUBE;
                 marker.action = visualization_msgs::Marker::ADD;
                 marker.pose.position.x = objects_[0][j].getOrigin().getX();
@@ -47,11 +47,12 @@ class Mediator : public Abstract_mediator{
                 }
                 rviz_markers_->markers.push_back(marker);
             }
-        }
+        //}
+        ROS_INFO("alle punkte durch");
     }
 
     void mediate() override {
-        ros::Rate loop_rate(1);
+        ros::Rate loop_rate(0.1);
         for (Abstract_robot* robot : robots_){
             Robot* ceti = dynamic_cast<Robot*>(robot);
             while (ros::ok()){
@@ -60,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

@@ -44,24 +44,44 @@ 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);
+    }
+
 
     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());
+            std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();            
+            while (it != observers_.end()) {
+                Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
+                Wing* ceti = dynamic_cast<Wing*>(deco->wing());
+                tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
+                tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
+                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(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);
+                ROS_INFO("sum  %f", sum);
+
+                if(full_area == sum) return true;
                 ++it;
+
             }
         }
-        return result;
+        return false;
     }
 
     inline std::vector<Abstract_robot_element*>& observers() { return observers_;}

src/mtc/include/impl/wing.h

@@ -12,8 +12,10 @@ class Wing  : public Abstract_robot_element{
     public:
     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)));
+        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))); //+-
+        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_;}

src/mtc/src/impl/wing_rviz_decorator.cpp

@@ -64,5 +64,32 @@ void Wing_rviz_decorator::output_filter() {
     marker.color.a = 1.0; // Don't forget to set the alpha!
 
     markers_->markers.push_back(marker);
+
+    visualization_msgs::Marker bounds;
+    bounds.header.frame_id = "map";
+    bounds.header.stamp = ros::Time();
+    bounds.ns = "bounds";
+    bounds.id = *((int*)(&wing));
+    bounds.type = visualization_msgs::Marker::POINTS;
+    bounds.action = visualization_msgs::Marker::ADD;
+    bounds.scale.x = 0.01f;
+    bounds.scale.y = 0.01f;
+    bounds.scale.z = 0.01f;
+    bounds.color.r = 0.0;
+    bounds.color.g = 0.0;
+    bounds.color.b = 1.0;
+    bounds.color.a = 1.0;
+    bounds.pose.orientation.w = 1;
+
+    for (int i = 0; i < wing->bounds().size(); i++){
+        tf2::Transform point_posistion = wing->world_tf() * wing->bounds()[i];
+        geometry_msgs::Point point;
+        point.x = point_posistion.getOrigin().getX();
+        point.y = point_posistion.getOrigin().getY();
+        point.z = point_posistion.getOrigin().getZ();
+        bounds.points.push_back(point);
+    }
+    markers_->markers.push_back(bounds);
+
 }