diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 7a9ecf99a3c3799c4ab9ecf2cb48a884baaf93d9..cfc16c0b608b2ee28bcf67b6ccfbed7c153c4bd7 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 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
diff --git a/src/mtc/include/impl/abstract_robot.h b/src/mtc/include/impl/abstract_robot.h
index 9a51dd3dae2c46af8e2e86664e1cc23ac6e22a0d..003c4ec3142499af5c5ce1db5207082cae987aa7 100644
--- a/src/mtc/include/impl/abstract_robot.h
+++ b/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;
diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h
index 989149e7eba61b88ea3ae93e55bb2d739da9a3ab..e566d6c7b19a7243455894e10231833d722c6c49 100644
--- a/src/mtc/include/impl/mediator.h
+++ b/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();
}
}
diff --git a/src/mtc/include/impl/robot.h b/src/mtc/include/impl/robot.h
index 67487315c31fae9db3b805f7edd562e3895b899d..d67cb8019d8b1eafc21d461c10ec800be377c871 100644
--- a/src/mtc/include/impl/robot.h
+++ b/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()) {
@@ -63,22 +76,17 @@ class Robot : public Abstract_robot{
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);
+ float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
- if(full_area == sum) return true;
- ++it;
+ ROS_INFO("sum %f", sum);
+ if ((std::floor(sum*100)/100.f) <= full_area) {
+ return true;
+ } else {
+ ++it;
+ }
}
}
return false;
diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp
index 5ab0be51916afe83e48374b6323650b2d236b251..5394ae70714f7389f1e55c0b97a60a7ecbe0accb 100644
--- a/src/mtc/src/base_routine.cpp
+++ b/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);