Skip to content
Snippets Groups Projects
Commit 6cac5bb7 authored by KingMaZito's avatar KingMaZito
Browse files

object detection done

parent fe7afbb0
Branches
No related tags found
No related merge requests found
Pipeline #14693 failed
/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
......@@ -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;
......
......@@ -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();
}
}
......
......@@ -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;
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment