bb_overlap.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id:$
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Zdenek Materna (imaterna@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: dd/mm/2012
00013  *
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  *
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  *
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00028 #include "srs_user_tests/bb_overlap.h"
00029 
00030 using namespace srs_user_tests;
00031 
00032 
00033 BBOverlap::BBOverlap() {
00034 
00035                         ros::NodeHandle nh("~");
00036 
00037                         id_.pub = nh.advertise<visualization_msgs::Marker>("ideal_bb_pose",1);
00038                         gripper_pub_ = nh.advertise<visualization_msgs::Marker>("gripper_target_pose",1);
00039 
00040 
00041                         srv_move_ = nh.advertiseService("move_bb", &BBOverlap::moveX,this);
00042 
00043                         im_.pose_rec = false;
00044                         im_.scale_rec = false;
00045                         
00046                         gr_suc_ = false;
00047                         bb_suc_ = false;
00048 
00049                         ros::param::param("~publish_debug_markers",publish_debug_markers_,true);
00050 
00051                         if (publish_debug_markers_) {
00052 
00053                           id_.points_pub = nh.advertise<visualization_msgs::Marker>("ideal_bb_points",1);
00054                           im_.points_pub = nh.advertise<visualization_msgs::Marker>("actual_bb_points",1);
00055                           points_proc_pub_ = nh.advertise<visualization_msgs::Marker>("proc_points",1);
00056 
00057                         }
00058 
00059                         ros::param::param("~bb/lwh/x",id_.bb.lwh.x,0.0);
00060                         ros::param::param("~bb/lwh/y",id_.bb.lwh.y,0.0);
00061                         ros::param::param("~bb/lwh/z",id_.bb.lwh.z,0.0);
00062 
00063                         ros::param::param("~bb/position/x",id_.bb.pose.position.x,0.0);
00064                         ros::param::param("~bb/position/y",id_.bb.pose.position.y,0.0);
00065                         ros::param::param("~bb/position/z",id_.bb.pose.position.z,0.0);
00066 
00067                         ros::param::param("~bb/orientation/x",id_.bb.pose.orientation.x,0.0);
00068                         ros::param::param("~bb/orientation/y",id_.bb.pose.orientation.y,0.0);
00069                         ros::param::param("~bb/orientation/z",id_.bb.pose.orientation.z,0.0);
00070                         ros::param::param("~bb/orientation/w",id_.bb.pose.orientation.w,1.0);
00071 
00072                         ros::param::param("~gripper/position/x",gripper_pose_.position.x,0.0);
00073                         ros::param::param("~gripper/position/y",gripper_pose_.position.y,0.0);
00074                         ros::param::param("~gripper/position/z",gripper_pose_.position.z,0.0);
00075 
00076                         // relative to absolute position
00077                         gripper_pose_.position.x += id_.bb.pose.position.x;
00078                         gripper_pose_.position.y += id_.bb.pose.position.y;
00079                         gripper_pose_.position.z += id_.bb.pose.position.z;
00080                         //gripper_pose_.position.z += id_.bb.lwh.z/2.0;
00081 
00082                         double tmp;
00083                         ros::param::param("~bb/success_min_dur",tmp,2.0);
00084                         bb_success_min_dur_ = ros::Duration(tmp);
00085 
00086                         ros::param::param("~gr/success_min_dur",tmp,2.0);
00087                         gr_success_min_dur_ = ros::Duration(tmp);
00088 
00089                         ros::param::param("~bb/success_val",tmp,0.8);
00090                         bb_success_val_ = tmp;
00091 
00092                         ros::param::param("~gr/success_val",tmp,0.05);
00093                         gr_success_val_ = tmp;
00094 
00095                         bb_success_first_ = ros::Time(0);
00096                         bb_success_last_ = ros::Time(0);
00097                         bb_success_tmp_ = ros::Time(0);
00098 
00099                         gr_success_first_ = ros::Time(0);
00100                         gr_success_last_ = ros::Time(0);
00101                         gr_success_tmp_ = ros::Time(0);
00102 
00103                         // TODO read this (+ pose) from params....
00104                         /*id_.bb.lwh.x = 0.2; // length
00105                         id_.bb.lwh.y = 0.1; // width
00106                         id_.bb.lwh.z = 0.4; // height*/
00107 
00108                         /*id_.bb.pose.position.x = -0.15;
00109                         id_.bb.pose.position.y = 0.6;
00110                         id_.bb.pose.position.z = 0.5;*/
00111 
00112                         /*id_.bb.pose.orientation.x = -8.79007958941e-07;
00113                         id_.bb.pose.orientation.y = 3.27846130193e-07;
00114                         id_.bb.pose.orientation.z = 0.429966424682;
00115                         id_.bb.pose.orientation.w = 0.90284486721;*/
00116 
00117                         id_.vol = id_.bb.lwh.x * id_.bb.lwh.y * id_.bb.lwh.z;
00118 
00119 
00120                         id_.marker.header.frame_id = "/map";
00121 
00122                         id_.marker.pose.position = id_.bb.pose.position;
00123                         id_.marker.pose.position.z += id_.bb.lwh.z/2.0;
00124 
00125                         id_.marker.pose.orientation = id_.bb.pose.orientation;
00126 
00127                         id_.marker.color.g = 1.0;
00128                         id_.marker.color.a = 0.6;
00129 
00130                         id_.marker.type = visualization_msgs::Marker::CUBE;
00131                         id_.marker.scale.x = id_.bb.lwh.x*2.0;
00132                         id_.marker.scale.y = id_.bb.lwh.y*2.0;
00133                         id_.marker.scale.z = id_.bb.lwh.z;
00134 
00135 
00136                         gripper_marker_.color.r = 1.0;
00137                         gripper_marker_.color.g = 140.0/255.0;
00138                         gripper_marker_.color.b = 0.0;
00139                         gripper_marker_.color.a = 0.6;
00140                         gripper_marker_.header.frame_id = "/map";
00141                         gripper_marker_.pose = gripper_pose_;
00142                         gripper_marker_.type = visualization_msgs::Marker::SPHERE;
00143                         gripper_marker_.scale.x = 0.05;
00144                         gripper_marker_.scale.y = 0.05;
00145                         gripper_marker_.scale.z = 0.05;
00146                         gripper_marker_.lifetime = ros::Duration(1.5);
00147 
00148                         // generate eight points for ideal position
00149                         update_points(id_.bb,id_.points);
00150 
00151                         sub_im_feedback_ = nh.subscribe<visualization_msgs::InteractiveMarkerFeedback>("/interaction_primitives/feedback",1,&BBOverlap::im_feedback_cb,this);
00152                         sub_im_scale_ = nh.subscribe<srs_interaction_primitives::ScaleChanged>("/interaction_primitives/unknown_object/update/scale_changed",1,&BBOverlap::im_scale_cb,this);
00153                         sub_gripper_update_ = nh.subscribe<visualization_msgs::InteractiveMarkerUpdate>("/planning_scene_warehouse_viewer_controls/update",1,&BBOverlap::gripper_im_cb,this);
00154 
00155                         arm_state_ok_ = false;
00156                         sub_arm_state_ = nh.subscribe<srs_assisted_arm_navigation_msgs::AssistedArmNavigationState>("/but_arm_manip/state",1,&BBOverlap::arm_nav_state_cb,this);
00157 
00158 
00159                         timer_ = nh.createTimer(ros::Duration(0.05),&BBOverlap::timer_cb,this);
00160 
00161                         ROS_INFO("Initialized.");
00162 
00163                 }
00164 
00165 bool BBOverlap::moveX(SetFloat::Request& req, SetFloat::Response& res) {
00166 
00167         bool update_marker = false;
00168 
00169 
00170         if (req.axis=="x") {
00171 
00172                 id_.bb.pose.position.x += req.v;
00173                 update_marker = true;
00174 
00175         }
00176 
00177         if (req.axis=="y") {
00178 
00179                 id_.bb.pose.position.y += req.v;
00180                 update_marker = true;
00181 
00182         }
00183 
00184         if (req.axis=="z") {
00185 
00186                 tf::Quaternion rot = tf::createQuaternionFromRPY(0.0,0.0,req.v);
00187 
00188                 tf::Quaternion g; // current IM marker pose
00189                 tf::quaternionMsgToTF(id_.bb.pose.orientation, g);
00190 
00191                 g = rot*g;
00192                 tf::quaternionTFToMsg(g,id_.bb.pose.orientation);
00193                 update_marker = true;
00194 
00195         }
00196 
00197         if (update_marker) {
00198 
00199                 id_.marker.pose.position = id_.bb.pose.position;
00200                 id_.marker.pose.position.z += id_.bb.lwh.z/2.0;
00201 
00202                 id_.marker.pose.orientation = id_.bb.pose.orientation;
00203 
00204                 return true;
00205 
00206         }
00207 
00208         ROS_WARN("Use x,y or z as axis!");
00209         return false;
00210 
00211 }
00212 
00213 
00214 void BBOverlap::arm_nav_state_cb(const srs_assisted_arm_navigation_msgs::AssistedArmNavigationStateConstPtr& msg) {
00215 
00216         ROS_INFO_ONCE("Assisted arm navigation state received.");
00217         arm_state_ok_ = msg->position_reachable;
00218 
00219 }
00220 
00221 void BBOverlap::update_points(tbb bb, tpoints &pp) {
00222 
00223         pp.clear();
00224 
00225         tpoint p(0.0, 0.0, 0.0);
00226 
00227         Eigen::Quaternionf q(bb.pose.orientation.w, bb.pose.orientation.x, bb.pose.orientation.y, bb.pose.orientation.z);
00228         Eigen::Affine3f tr(q);
00229 
00230         Eigen::Quaternionf q1(id_.bb.pose.orientation.w, id_.bb.pose.orientation.x, id_.bb.pose.orientation.y, id_.bb.pose.orientation.z);
00231         Eigen::Affine3f tr2(q1.inverse());
00232 
00233         // 1
00234         p(0) = bb.lwh.x;
00235     p(1) = bb.lwh.y;
00236     p(2) = -0.5 * bb.lwh.z;
00237     p = tr*p;
00238     p = tr2*p;
00239     p(0) += bb.pose.position.x;
00240     p(1) += bb.pose.position.y;
00241     p(2) += bb.pose.position.z;
00242     p(2) += 0.5 * bb.lwh.z;
00243     pp.push_back(p);
00244 
00245     // 2
00246     p(0) = bb.lwh.x;
00247     p(1) = - bb.lwh.y;
00248     p(2) = -0.5 * bb.lwh.z;
00249     p = tr*p;
00250     p = tr2*p;
00251     p(0) += bb.pose.position.x;
00252         p(1) += bb.pose.position.y;
00253         p(2) += bb.pose.position.z;
00254         p(2) += 0.5 * bb.lwh.z;
00255         pp.push_back(p);
00256 
00257         // 3
00258         p(0) = - bb.lwh.x;
00259         p(1) = + bb.lwh.y;
00260         p(2) = -0.5 * bb.lwh.z;
00261         p = tr*p;
00262         p = tr2*p;
00263         p(0) += bb.pose.position.x;
00264         p(1) += bb.pose.position.y;
00265         p(2) += bb.pose.position.z;
00266         p(2) += 0.5 * bb.lwh.z;
00267     pp.push_back(p);
00268 
00269     // 4
00270         p(0) = - bb.lwh.x;
00271         p(1) = - bb.lwh.y;
00272         p(2) = -0.5 * bb.lwh.z;
00273         p = tr*p;
00274         p = tr2*p;
00275         p(0) += bb.pose.position.x;
00276         p(1) += bb.pose.position.y;
00277         p(2) += bb.pose.position.z;
00278         p(2) += 0.5 * bb.lwh.z;
00279         pp.push_back(p);
00280 
00281         // 5
00282         p(0) = bb.lwh.x;
00283     p(1) = bb.lwh.y;
00284     p(2) = 0.5*bb.lwh.z;
00285     p = tr*p;
00286     p = tr2*p;
00287     p(0) += bb.pose.position.x;
00288         p(1) += bb.pose.position.y;
00289         p(2) += bb.pose.position.z;
00290         p(2) += bb.lwh.z*0.5;
00291     pp.push_back(p);
00292 
00293     // 6
00294         p(0) = bb.lwh.x;
00295         p(1) = - bb.lwh.y;
00296         p(2) = 0.5*bb.lwh.z;
00297         p = tr*p;
00298         p = tr2*p;
00299         p(0) += bb.pose.position.x;
00300         p(1) += bb.pose.position.y;
00301         p(2) += bb.pose.position.z;
00302         p(2) += bb.lwh.z*0.5;
00303         pp.push_back(p);
00304 
00305         // 7
00306         p(0) = - bb.lwh.x;
00307     p(1) = bb.lwh.y;
00308     p(2) = 0.5*bb.lwh.z;
00309     p = tr*p;
00310     p = tr2*p;
00311     p(0) += bb.pose.position.x;
00312         p(1) += bb.pose.position.y;
00313         p(2) += bb.pose.position.z;
00314         p(2) += bb.lwh.z*0.5;
00315     pp.push_back(p);
00316 
00317     // 8
00318         p(0) = - bb.lwh.x;
00319         p(1) = - bb.lwh.y;
00320         p(2) = 0.5*bb.lwh.z;
00321         p = tr*p;
00322         p = tr2*p;
00323         p(0) += bb.pose.position.x;
00324         p(1) += bb.pose.position.y;
00325         p(2) += bb.pose.position.z;
00326         p(2) += bb.lwh.z*0.5;
00327         pp.push_back(p);
00328 
00329 }
00330 
00331 void BBOverlap::gripper_im_cb(const visualization_msgs::InteractiveMarkerUpdateConstPtr& msg) {
00332 
00333         if (msg->poses.size()==1 && msg->poses[0].name=="MPR 0_end_control") {
00334 
00335                 tfl_.waitForTransform("/map",msg->poses[0].header.frame_id,ros::Time(0),ros::Duration(5.0));
00336 
00337                 geometry_msgs::PoseStamped tmp;
00338 
00339                 tmp.header.stamp = ros::Time(0);
00340                 tmp.header.frame_id = msg->poses[0].header.frame_id;
00341                 tmp.pose = msg->poses[0].pose;
00342 
00343                 try {
00344 
00345                         tfl_.transformPose("/map",tmp,tmp);
00346 
00347                 } catch(tf::TransformException& ex){
00348 
00349                    std::cerr << "Transform error: " << ex.what() << std::endl;
00350 
00351                    ROS_ERROR("Exception on TF transf.: %s",ex.what());
00352 
00353                    return;
00354                 }
00355 
00356                 ROS_INFO_ONCE("First gripper pose received.");
00357                 gripper_pose_rec_ = true;
00358                 gripper_pose_curr_ = tmp.pose;
00359 
00360         }
00361 
00362 }
00363 
00364 void BBOverlap::publish_points(tpoints points, ros::Publisher &pub, std_msgs::ColorRGBA c) {
00365 
00366         if (!publish_debug_markers_) return;
00367 
00368         visualization_msgs::Marker m;
00369 
00370         m.header.frame_id = "/map";
00371         m.header.stamp = ros::Time::now();
00372         m.type = visualization_msgs::Marker::SPHERE_LIST;
00373         m.action = visualization_msgs::Marker::ADD;
00374         m.color = c;
00375         m.lifetime = ros::Duration(1.5);
00376 
00377         m.scale.x = 0.05;
00378         m.scale.y = 0.05;
00379         m.scale.z = 0.05;
00380 
00381         m.pose.orientation.x = 0.0;
00382         m.pose.orientation.y = 0.0;
00383         m.pose.orientation.z = 0.0;
00384         m.pose.orientation.w = 1.0;
00385 
00386         for (unsigned int i=0; i < points.size(); i++) {
00387 
00388                 geometry_msgs::Point p;
00389 
00390                 p.x = points[i](0);
00391                 p.y = points[i](1);
00392                 p.z = points[i](2);
00393 
00394                 m.points.push_back(p);
00395 
00396         }
00397 
00398         if (pub.getNumSubscribers() > 0) {
00399 
00400                 pub.publish(m);
00401                 ROS_INFO_ONCE("Publishing %d spheres.",(int)m.points.size());
00402         }
00403 
00404 }
00405 
00406 /*int BBOverlap::getch()
00407 {
00408     int r;
00409     unsigned char c;
00410     if ((r = read(0, &c, sizeof(c))) < 0) {
00411         return r;
00412     } else {
00413         return c;
00414     }
00415 }*/
00416 
00417 
00418 BBOverlap::~BBOverlap() {
00419 
00420 }
00421 
00422 
00423 void BBOverlap::im_feedback_cb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& msg) {
00424 
00425 
00426         if (msg->marker_name == "unknown_object") {
00427 
00428                 ROS_INFO_ONCE("IM feedback received");
00429 
00430                 boost::mutex::scoped_lock(im_.mutex);
00431 
00432                 im_.bb.pose = msg->pose;
00433                 im_.bb.pose.position.z -= im_.bb.lwh.z/2.0;
00434 
00435                 im_.pose_rec = true;
00436 
00437         }
00438 
00439 }
00440 
00441 // computes volume for AABB
00442 double BBOverlap::points_volume(const tpoints &p) {
00443 
00444         double x = fabs((double)p[4](0) - (double)p[3](0));
00445         double y = fabs((double)p[4](1) - (double)p[3](1));
00446         double z = fabs((double)p[4](2) - (double)p[3](2));
00447 
00448         return x*y*z;
00449 
00450         //return fabs( (double)p[4](0)*(double)p[4](1)*(double)p[4](2) );
00451 
00452 }
00453 
00454 
00455 void BBOverlap::im_scale_cb(const srs_interaction_primitives::ScaleChangedConstPtr& msg) {
00456 
00457         ROS_INFO_ONCE("Scale received");
00458 
00459         boost::mutex::scoped_lock(im_.mutex);
00460 
00461         im_.bb.lwh = msg->new_scale;
00462         im_.bb.lwh.x *= 0.5;
00463         im_.bb.lwh.y *= 0.5;
00464 
00465         im_.scale_rec = true;
00466 
00467 }
00468 
00469 double BBOverlap::rmin(double val1, double val2) {
00470 
00471         if (val1<val2) return val1;
00472         else return val2;
00473 
00474 }
00475 
00476 double BBOverlap::rmax(double val1, double val2) {
00477 
00478         if (val1>val2) return val1;
00479         else return val2;
00480 
00481 }
00482 
00483 
00484 void BBOverlap::timer_cb(const ros::TimerEvent&) {
00485 
00486 
00487 
00488         ROS_INFO_ONCE("Timer triggered");
00489 
00490         boost::mutex::scoped_lock(im_.mutex);
00491 
00492         if (id_.pub.getNumSubscribers() > 0) {
00493 
00494                 ROS_INFO_ONCE("Publishing BB ideal position.");
00495 
00496                 id_.marker.header.stamp = ros::Time::now();
00497                 id_.pub.publish(id_.marker);
00498 
00499         }
00500 
00501         bool suc = false;
00502         double overlap = 0.0;
00503         ros::Time now = ros::Time::now();
00504 
00505         // we have received something...
00506         if (im_.pose_rec && im_.scale_rec) {
00507 
00508 
00509                 geometry_msgs::Pose pos = im_.bb.pose;
00510 
00511                 Eigen::Quaternionf q1(id_.bb.pose.orientation.w, id_.bb.pose.orientation.x, id_.bb.pose.orientation.y, id_.bb.pose.orientation.z);
00512                 Eigen::Affine3f tr2(q1.inverse());
00513 
00514                 tpoint p(pos.position.x, pos.position.y, pos.position.z);
00515 
00516                 p = tr2*p;
00517 
00518                 pos.position.x = p(0);
00519                 pos.position.y = p(1);
00520                 pos.position.z = p(2);
00521 
00522                 tbb tmp;
00523 
00524                 tmp.pose = pos;
00525                 tmp.lwh = im_.bb.lwh;
00526 
00527                 // generate new points
00528                 update_points(tmp,im_.points);
00529 
00530 
00531                 // take ideal and actual points in /map coord. system
00532                 tpoints id_p = id_.points;
00533                 tpoints im_p = im_.points;
00534 
00535                 tpoint p2(id_.bb.pose.position.x, id_.bb.pose.position.y, id_.bb.pose.position.z);
00536 
00537                 p2 = tr2*p2;
00538 
00539                 // shift it to 0,0
00540                 for (unsigned int i=0; i<im_p.size(); i++) {
00541 
00542                         id_p[i](0) -= id_.bb.pose.position.x;
00543                         id_p[i](1) -= id_.bb.pose.position.y;
00544                         id_p[i](2) -= id_.bb.pose.position.z;
00545 
00546                         im_p[i](0) -= p2(0);
00547                         im_p[i](1) -= p2(1);
00548                         im_p[i](2) -= p2(2);
00549 
00550                 }
00551 
00552                 double pdx = id_p[0](0);
00553                 double pdy = id_p[0](1);
00554                 double pdz = id_p[0](2);
00555 
00556 
00557                 // correction - shift one corner to 0,0
00558                 for (unsigned int i=0; i<im_p.size(); i++) {
00559 
00560                         if (id_p[i](0) < pdx) pdx = id_p[i](0);
00561                         if (id_p[i](1) < pdy) pdy = id_p[i](1);
00562                         if (id_p[i](2) < pdz) pdz = id_p[i](2);
00563 
00564                 }
00565 
00566                 /*tpoint p3(dx,dy,dz);
00567 
00568                 p3 = tr2*p3;*/
00569 
00570                 for (unsigned int i=0; i<im_p.size(); i++) {
00571 
00572                         id_p[i](0) -= pdx;
00573                         id_p[i](1) -= pdy;
00574                         id_p[i](2) -= pdz;
00575 
00576                         im_p[i](0) -= pdx;
00577                         im_p[i](1) -= pdy;
00578                         im_p[i](2) -= pdz;
00579 
00580                 }
00581 
00582                 if (points_volume(id_p) <= 0.0) {
00583 
00584                         ROS_ERROR("ID vol negative or zero!");
00585 
00586                 }
00587 
00588                 //publish_points(tpoints points, ros::Publisher &pub, std_msgs::ColorRGBA c)
00589                 std_msgs::ColorRGBA c;
00590                 c.a = 0.6;
00591                 c.g = 1.0;
00592 
00593                 publish_points(id_p, id_.points_pub, c);
00594 
00595                 c.g = 0.0;
00596                 c.b = 1.0;
00597                 publish_points(im_p, im_.points_pub, c);
00598 
00599                 // axis aligned
00600                 tpoints imaa_p = im_p;
00601 
00602                 double min_x = imaa_p[0](0);
00603                 double max_x = imaa_p[0](0);
00604 
00605                 double min_y = imaa_p[0](1);
00606                 double max_y = imaa_p[0](1);
00607 
00608                 double min_z = imaa_p[0](2);
00609                 double max_z = imaa_p[0](2);
00610 
00611        /*                       x               y               z
00612                 idp(0)=[0.400000,0.200000,0.000000]
00613                 idp(1)=[0.400000,0.000000,0.000000]
00614                 idp(2)=[0.000000,0.200000,0.000000]
00615                 idp(3)=[0.000000,0.000000,0.000000]
00616                 idp(4)=[0.400000,0.200000,0.400000]
00617                 idp(5)=[0.400000,0.000000,0.400000]
00618                 idp(6)=[0.000000,0.200000,0.400000]
00619                 idp(7)=[0.000000,0.000000,0.400000]*/
00620 
00621                 for (unsigned int i=0; i<im_p.size(); i++) {
00622 
00623                         if (min_x > imaa_p[i](0)) min_x =  imaa_p[i](0);
00624                         if (max_x < imaa_p[i](0)) max_x =  imaa_p[i](0);
00625 
00626                         if (min_y > imaa_p[i](1)) min_y =  imaa_p[i](1);
00627                         if (max_y < imaa_p[i](1)) max_y =  imaa_p[i](1);
00628 
00629                         if (min_z > imaa_p[i](2)) min_z =  imaa_p[i](2);
00630                         if (max_z < imaa_p[i](2)) max_z =  imaa_p[i](2);
00631 
00632                 }
00633 
00634                 imaa_p[0] = tpoint(max_x,max_y,min_z);
00635                 imaa_p[1] = tpoint(max_x,min_y,min_z);
00636                 imaa_p[2] = tpoint(min_x,max_y,min_z);
00637                 imaa_p[3] = tpoint(min_x,min_y,min_z);
00638                 imaa_p[4] = tpoint(max_x,max_y,max_z);
00639                 imaa_p[5] = tpoint(max_x,min_y,max_z);
00640                 imaa_p[6] = tpoint(min_x,max_y,max_z);
00641                 imaa_p[7] = tpoint(min_x,min_y,max_z);
00642 
00643                 c.g = 0.5;
00644                 c.b = 0.5;
00645                 publish_points(imaa_p, points_proc_pub_, c);
00646 
00647 
00648                 //tpoints ov_p;
00649 
00650                 double ov_p_vol = 0.0;
00651 
00652                 double dx = 0.0;
00653                 double dy = 0.0;
00654                 double dz = 0.0;
00655 
00656                 // compute "overlap bb"
00657                 if ( (max_z > 0.0 && min_z < id_p[4](2)) && (max_x > 0.0 && min_x <  id_p[4](0)) && (max_y > 0.0 && min_y <  id_p[4](1)) ) {
00658 
00659                         dx = rmin((double)id_p[0](0),(double)imaa_p[0](0)) - rmax((double)id_p[2](0),(double)imaa_p[2](0));
00660 
00661                         dy = rmin((double)id_p[0](1),(double)imaa_p[0](1)) - rmax((double)id_p[1](1),(double)imaa_p[1](1));
00662 
00663                         dz = rmin((double)id_p[4](2),(double)imaa_p[4](2)) - rmax((double)id_p[0](2),(double)imaa_p[0](2));
00664 
00665                         // x*y*z
00666                         ov_p_vol = dx*dy*dz;
00667 
00668                 };
00669 
00670                 double max_v = rmax(points_volume(id_p),points_volume(imaa_p));
00671 
00672                 overlap = ov_p_vol / max_v;
00673 
00674                 if (overlap > 1.0) overlap = 1.0; // just for a case...
00675 
00676 
00677                 if (overlap > bb_success_val_) {
00678 
00679                         if (bb_success_tmp_ == ros::Time(0)) bb_success_tmp_ = now;
00680 
00681                         if ((now - bb_success_tmp_) > ros::Duration(bb_success_min_dur_)) {
00682 
00683                                 if (bb_success_first_ == ros::Time(0)) bb_success_first_ = now;
00684                                 
00685                                 suc = true;
00686 
00687                                 if (bb_suc_ == false) {
00688                                         
00689                                   bb_success_last_ = now;
00690                                   bb_suc_ = true;       
00691 
00692                                 }
00693 
00694                         }
00695 
00696 
00697                 } else {
00698 
00699                         bb_success_tmp_ = ros::Time(0);
00700                         bb_suc_ = false;
00701 
00702                 }
00703 
00704 
00705         } // if we have something
00706 
00707         double gripper_dist = -1.0;
00708 
00709         bool gr_suc = false;
00710 
00711         if (gripper_pose_rec_) {
00712 
00713                 double tmp;
00714 
00715                 tmp = pow(gripper_pose_.position.x - gripper_pose_curr_.position.x,2.0)
00716                                 + pow(gripper_pose_.position.y - gripper_pose_curr_.position.y,2.0)
00717                                 + pow(gripper_pose_.position.z - gripper_pose_curr_.position.z,2.0);
00718 
00719                 gripper_dist = pow(fabs(tmp),0.5);
00720 
00721                 if ( (gripper_dist < gr_success_val_) && arm_state_ok_) {
00722 
00723                         if (gr_success_tmp_ == ros::Time(0)) gr_success_tmp_ = now;
00724 
00725                                 if ((now - gr_success_tmp_) > ros::Duration(gr_success_min_dur_)) {
00726 
00727                                         if (gr_success_first_ == ros::Time(0)) gr_success_first_ = now;
00728                                         
00729                                         gr_suc = true;
00730                                         
00731                                         if (gr_suc_ == false) {
00732                                         
00733                                   gr_success_last_ = now;
00734                                   gr_suc_ = true;       
00735 
00736                                 }
00737 
00738                                 }
00739 
00740 
00741 
00742                 } else {
00743                 
00744                   
00745                   gr_success_tmp_ = ros::Time(0);
00746                   gr_suc_ = false;
00747                   
00748                   }
00749 
00750         }
00751 
00752         if ( (ros::Time::now() - last_log_out_) > ros::Duration(2.0) ) {
00753 
00754                 //ROS_INFO("ID vol: %f, MAA vol: %f",points_volume(id_p),points_volume(imaa_p));
00755 
00756                 //ROS_INFO("dx: %f, dy: %f, dz: %f, vol: %f",dx,dy,dz,ov_p_vol);
00757                 //ROS_INFO("overlap: %f%% (%f / %f)",overlap*100.0,ov_p_vol,max_v);
00758 
00759                 /*boost::posix_time::ptime f_s = bb_success_first_.toBoost();
00760                 boost::posix_time::ptime l_s = bb_success_last_.toBoost();
00761 
00762                 boost::posix_time::ptime f_gr = gr_success_first_.toBoost();
00763                 boost::posix_time::ptime l_gr = gr_success_last_.toBoost();*/
00764 
00765                 if (suc) printf("BB OVERLAP: %03.1f%% (success)\n",overlap*100);
00766                 else printf("BB OVERLAP: %03.1f%% (fail)\n",overlap*100);
00767 
00768                 double gdx = gripper_pose_.position.x - gripper_pose_curr_.position.x;
00769                 double gdy = gripper_pose_.position.y - gripper_pose_curr_.position.y;
00770                 double gdz = gripper_pose_.position.z - gripper_pose_curr_.position.z;
00771 
00772                 //printf("BB First success: %02d:%02d:%02d\n",f_s.time_of_day().hours(), f_s.time_of_day().minutes(), f_s.time_of_day().seconds());
00773                 //printf("BB Last success: %02d:%02d:%02d\n", l_s.time_of_day().hours(), l_s.time_of_day().minutes(), l_s.time_of_day().seconds());
00774                 printf("BB First success: %04d\n",(int)floor(bb_success_first_.toSec()));
00775                 printf("BB Last success: %04d\n", (int)floor(bb_success_last_.toSec()));
00776 
00777                 if (gr_suc) printf("GR Distance: %f [dx: %.2f, dy: %.2f, dz: %.2f] (success)\n",gripper_dist,gdx,gdy,gdz);
00778                 else printf("GR Distance: %f [dx: %.2f, dy: %.2f, dz: %.2f] (fail)\n",gripper_dist,gdx,gdy,gdz);
00779 
00780                 /*printf("GR First success: %02d:%02d:%02d\n",f_gr.time_of_day().hours(), f_gr.time_of_day().minutes(), f_gr.time_of_day().seconds());
00781                 printf("GR Last success: %02d:%02d:%02d\n", l_gr.time_of_day().hours(), l_gr.time_of_day().minutes(), l_gr.time_of_day().seconds());*/
00782                 printf("GR First success: %04d\n",(int)floor(gr_success_first_.toSec()));
00783                 printf("GR Last success: %04d\n", (int)floor(gr_success_last_.toSec()));
00784                 printf("\n");
00785 
00786                 last_log_out_ = ros::Time::now();
00787 
00788 
00789         } // if
00790 
00791 
00792         // publish ideal position of gripper
00793         if (gripper_pub_.getNumSubscribers() != 0) {
00794 
00795                 ROS_INFO_ONCE("Publishing gripper ideal position.");
00796 
00797                 gripper_marker_.header.stamp = ros::Time::now();
00798                 gripper_pub_.publish(gripper_marker_);
00799 
00800 
00801         }
00802 
00803 
00804 }
00805 
00806 
00807 int main(int argc, char** argv)
00808 {
00809 
00810 
00811       ROS_INFO("Starting");
00812       ros::init(argc, argv, "bb_overlap");
00813 
00814 
00815       BBOverlap *bb = new BBOverlap();
00816 
00817       ros::spin();
00818 
00819       delete bb;
00820 
00821       return 0;
00822 
00823 }


srs_user_tests
Author(s): SRS team
autogenerated on Mon Oct 6 2014 08:53:11