diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index f136bc542023bbb0c01c23dfbbd4c7b0f470d48c..7a9ecf99a3c3799c4ab9ecf2cb48a884baaf93d9 100644 --- a/.catkin_tools/profiles/default/devel_collisions.txt +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -1,4 +1,4 @@ /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 diff --git a/src/mtc/include/impl/abstract_robot_element.h b/src/mtc/include/impl/abstract_robot_element.h index ee6c2f20a991b4fdcdd433fabdf9c931447510c1..7d77c0578e53b46026d521148a6b1fd2117dd025 100644 --- a/src/mtc/include/impl/abstract_robot_element.h +++ b/src/mtc/include/impl/abstract_robot_element.h @@ -14,12 +14,12 @@ 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_;} - virtual void update(tf2::Transform& tf)=0; + virtual void update(tf2::Transform& tf)= 0; }; diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h index d16a2ad3e6920b86c28e61e97a1267f650308c75..989149e7eba61b88ea3ae93e55bb2d739da9a3ab 100644 --- a/src/mtc/include/impl/mediator.h +++ b/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(); } } diff --git a/src/mtc/include/impl/robot.h b/src/mtc/include/impl/robot.h index 1d385d693e6a5b376d3adff8652279c47d7b99dc..67487315c31fae9db3b805f7edd562e3895b899d 100644 --- a/src/mtc/include/impl/robot.h +++ b/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_;} diff --git a/src/mtc/include/impl/wing.h b/src/mtc/include/impl/wing.h index 2cc5d568c756281d5f65eab695ea6ed0b535d452..cc44f795ad6a3127b747b0e173998c672911e8c5 100644 --- a/src/mtc/include/impl/wing.h +++ b/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_;} diff --git a/src/mtc/src/impl/wing_rviz_decorator.cpp b/src/mtc/src/impl/wing_rviz_decorator.cpp index 5f4e323b21d08fb3921c7523b847928605e93aed..b19e446ba8d980489362e5322316065b46ce01f0 100644 --- a/src/mtc/src/impl/wing_rviz_decorator.cpp +++ b/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); + }