srs_user_tests: srs_user_tests: bb_overlap.cpp Source 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                         im_.pose_rec = false;
00041                         im_.scale_rec = false;
00042 
00043                         ros::param::param("~publish_debug_markers",publish_debug_markers_,true);
00044 
00045                         if (publish_debug_markers_) {
00046 
00047                           id_.points_pub = nh.advertise<visualization_msgs::Marker>("ideal_bb_points",1);
00048                           im_.points_pub = nh.advertise<visualization_msgs::Marker>("actual_bb_points",1);
00049                           points_proc_pub_ = nh.advertise<visualization_msgs::Marker>("proc_points",1);
00050 
00051                         }
00052 
00053                         ros::param::param("~bb/lwh/x",id_.bb.lwh.x,0.2);
00054                         ros::param::param("~bb/lwh/y",id_.bb.lwh.y,0.1);
00055                         ros::param::param("~bb/lwh/z",id_.bb.lwh.z,0.4);
00056 
00057                         ros::param::param("~bb/position/x",id_.bb.pose.position.x,-0.15);
00058                         ros::param::param("~bb/position/y",id_.bb.pose.position.y,0.6);
00059                         ros::param::param("~bb/position/z",id_.bb.pose.position.z,0.5);
00060 
00061                         ros::param::param("~bb/orientation/x",id_.bb.pose.orientation.x,-8.79007958941e-07);
00062                         ros::param::param("~bb/orientation/y",id_.bb.pose.orientation.y,3.27846130193e-07);
00063                         ros::param::param("~bb/orientation/z",id_.bb.pose.orientation.z,0.429966424682);
00064                         ros::param::param("~bb/orientation/w",id_.bb.pose.orientation.w,0.90284486721);
00065 
00066                         ros::param::param("~gripper/position/x",gripper_pose_.position.x,0.0);
00067                         ros::param::param("~gripper/position/y",gripper_pose_.position.y,0.0);
00068                         ros::param::param("~gripper/position/z",gripper_pose_.position.z,0.0);
00069 
00070                         double tmp;
00071                         ros::param::param("~bb/success_min_dur",tmp,2.0);
00072                         bb_success_min_dur_ = ros::WallDuration(tmp);
00073 
00074                         ros::param::param("~gr/success_min_dur",tmp,2.0);
00075                         gr_success_min_dur_ = ros::WallDuration(tmp);
00076 
00077                         ros::param::param("~bb/success_val",tmp,0.8);
00078                         bb_success_val_ = tmp;
00079 
00080                         ros::param::param("~gr/success_val",tmp,0.05);
00081                         gr_success_val_ = tmp;
00082 
00083                         bb_success_first_ = ros::WallTime(0);
00084                         bb_success_last_ = ros::WallTime(0);
00085                         bb_success_tmp_ = ros::WallTime(0);
00086 
00087                         gr_success_first_ = ros::WallTime(0);
00088                         gr_success_last_ = ros::WallTime(0);
00089                         gr_success_tmp_ = ros::WallTime(0);
00090 
00091                         // TODO read this (+ pose) from params....
00092                         /*id_.bb.lwh.x = 0.2; // length
00093                         id_.bb.lwh.y = 0.1; // width
00094                         id_.bb.lwh.z = 0.4; // height*/
00095 
00096                         /*id_.bb.pose.position.x = -0.15;
00097                         id_.bb.pose.position.y = 0.6;
00098                         id_.bb.pose.position.z = 0.5;*/
00099 
00100                         /*id_.bb.pose.orientation.x = -8.79007958941e-07;
00101                         id_.bb.pose.orientation.y = 3.27846130193e-07;
00102                         id_.bb.pose.orientation.z = 0.429966424682;
00103                         id_.bb.pose.orientation.w = 0.90284486721;*/
00104 
00105                         id_.vol = id_.bb.lwh.x * id_.bb.lwh.y * id_.bb.lwh.z;
00106 
00107                         id_.marker.color.g = 1.0;
00108                         id_.marker.color.a = 0.6;
00109                         id_.marker.header.frame_id = "/map";
00110 
00111                         id_.marker.pose.position = id_.bb.pose.position;
00112                         id_.marker.pose.position.z += id_.bb.lwh.z/2.0;
00113 
00114                         id_.marker.pose.orientation = id_.bb.pose.orientation;
00115 
00116                         id_.marker.type = visualization_msgs::Marker::CUBE;
00117                         id_.marker.scale.x = id_.bb.lwh.x*2.0;
00118                         id_.marker.scale.y = id_.bb.lwh.y*2.0;
00119                         id_.marker.scale.z = id_.bb.lwh.z;
00120 
00121                         gripper_marker_.color.g = 1.0;
00122                         gripper_marker_.color.a = 0.6;
00123                         gripper_marker_.header.frame_id = "/map";
00124                         gripper_marker_.pose = gripper_pose_;
00125                         gripper_marker_.type = visualization_msgs::Marker::SPHERE;
00126                         gripper_marker_.scale.x = 0.05;
00127                         gripper_marker_.scale.y = 0.05;
00128                         gripper_marker_.scale.z = 0.05;
00129                         gripper_marker_.lifetime = ros::Duration(1.5);
00130 
00131                         // generate eight points for ideal position
00132                         update_points(id_.bb,id_.points);
00133 
00134                         sub_im_feedback_ = nh.subscribe<visualization_msgs::InteractiveMarkerFeedback>("/interaction_primitives/feedback",1,&BBOverlap::im_feedback_cb,this);
00135                         sub_im_scale_ = nh.subscribe<srs_interaction_primitives::ScaleChanged>("/interaction_primitives/unknown_object/update/scale_changed",1,&BBOverlap::im_scale_cb,this);
00136                         sub_gripper_update_ = nh.subscribe<visualization_msgs::InteractiveMarkerUpdate>("/planning_scene_warehouse_viewer_controls/update",1,&BBOverlap::gripper_im_cb,this);
00137 
00138                         arm_state_ok_ = false;
00139                         sub_arm_state_ = nh.subscribe<srs_assisted_arm_navigation_msgs::AssistedArmNavigationState>("/but_arm_manip/state",1,&BBOverlap::arm_nav_state_cb,this);
00140 
00141                         timer_ = nh.createTimer(ros::Duration(0.05),&BBOverlap::timer_cb,this);
00142 
00143                         ROS_INFO("Initialized.");
00144 
00145                 }
00146 
00147 void BBOverlap::arm_nav_state_cb(const srs_assisted_arm_navigation_msgs::AssistedArmNavigationStateConstPtr& msg) {
00148 
00149         ROS_INFO_ONCE("Assisted arm navigation state received.");
00150         arm_state_ok_ = msg->position_reachable;
00151 
00152 }
00153 
00154 void BBOverlap::update_points(tbb bb, tpoints &pp) {
00155 
00156         pp.clear();
00157 
00158         tpoint p(0.0, 0.0, 0.0);
00159 
00160         Eigen::Quaternionf q(bb.pose.orientation.w, bb.pose.orientation.x, bb.pose.orientation.y, bb.pose.orientation.z);
00161         Eigen::Affine3f tr(q);
00162 
00163         Eigen::Quaternionf q1(id_.bb.pose.orientation.w, id_.bb.pose.orientation.x, id_.bb.pose.orientation.y, id_.bb.pose.orientation.z);
00164         Eigen::Affine3f tr2(q1.inverse());
00165 
00166         // 1
00167         p(0) = bb.lwh.x;
00168     p(1) = bb.lwh.y;
00169     p(2) = -0.5 * bb.lwh.z;
00170     p = tr*p;
00171     p = tr2*p;
00172     p(0) += bb.pose.position.x;
00173     p(1) += bb.pose.position.y;
00174     p(2) += bb.pose.position.z;
00175     p(2) += 0.5 * bb.lwh.z;
00176     pp.push_back(p);
00177 
00178     // 2
00179     p(0) = bb.lwh.x;
00180     p(1) = - bb.lwh.y;
00181     p(2) = -0.5 * bb.lwh.z;
00182     p = tr*p;
00183     p = tr2*p;
00184     p(0) += bb.pose.position.x;
00185         p(1) += bb.pose.position.y;
00186         p(2) += bb.pose.position.z;
00187         p(2) += 0.5 * bb.lwh.z;
00188         pp.push_back(p);
00189 
00190         // 3
00191         p(0) = - bb.lwh.x;
00192         p(1) = + bb.lwh.y;
00193         p(2) = -0.5 * bb.lwh.z;
00194         p = tr*p;
00195         p = tr2*p;
00196         p(0) += bb.pose.position.x;
00197         p(1) += bb.pose.position.y;
00198         p(2) += bb.pose.position.z;
00199         p(2) += 0.5 * bb.lwh.z;
00200     pp.push_back(p);
00201 
00202     // 4
00203         p(0) = - bb.lwh.x;
00204         p(1) = - bb.lwh.y;
00205         p(2) = -0.5 * bb.lwh.z;
00206         p = tr*p;
00207         p = tr2*p;
00208         p(0) += bb.pose.position.x;
00209         p(1) += bb.pose.position.y;
00210         p(2) += bb.pose.position.z;
00211         p(2) += 0.5 * bb.lwh.z;
00212         pp.push_back(p);
00213 
00214         // 5
00215         p(0) = bb.lwh.x;
00216     p(1) = bb.lwh.y;
00217     p(2) = 0.5*bb.lwh.z;
00218     p = tr*p;
00219     p = tr2*p;
00220     p(0) += bb.pose.position.x;
00221         p(1) += bb.pose.position.y;
00222         p(2) += bb.pose.position.z;
00223         p(2) += bb.lwh.z*0.5;
00224     pp.push_back(p);
00225 
00226     // 6
00227         p(0) = bb.lwh.x;
00228         p(1) = - bb.lwh.y;
00229         p(2) = 0.5*bb.lwh.z;
00230         p = tr*p;
00231         p = tr2*p;
00232         p(0) += bb.pose.position.x;
00233         p(1) += bb.pose.position.y;
00234         p(2) += bb.pose.position.z;
00235         p(2) += bb.lwh.z*0.5;
00236         pp.push_back(p);
00237 
00238         // 7
00239         p(0) = - bb.lwh.x;
00240     p(1) = bb.lwh.y;
00241     p(2) = 0.5*bb.lwh.z;
00242     p = tr*p;
00243     p = tr2*p;
00244     p(0) += bb.pose.position.x;
00245         p(1) += bb.pose.position.y;
00246         p(2) += bb.pose.position.z;
00247         p(2) += bb.lwh.z*0.5;
00248     pp.push_back(p);
00249 
00250     // 8
00251         p(0) = - bb.lwh.x;
00252         p(1) = - bb.lwh.y;
00253         p(2) = 0.5*bb.lwh.z;
00254         p = tr*p;
00255         p = tr2*p;
00256         p(0) += bb.pose.position.x;
00257         p(1) += bb.pose.position.y;
00258         p(2) += bb.pose.position.z;
00259         p(2) += bb.lwh.z*0.5;
00260         pp.push_back(p);
00261 
00262 }
00263 
00264 void BBOverlap::gripper_im_cb(const visualization_msgs::InteractiveMarkerUpdateConstPtr& msg) {
00265 
00266         if (msg->poses.size()==1 && msg->poses[0].name=="MPR 0_end_control") {
00267 
00268                 tfl_.waitForTransform("/map",msg->poses[0].header.frame_id,ros::Time(0),ros::Duration(5.0));
00269 
00270                 geometry_msgs::PoseStamped tmp;
00271 
00272                 tmp.header.stamp = ros::Time(0);
00273                 tmp.header.frame_id = msg->poses[0].header.frame_id;
00274                 tmp.pose = msg->poses[0].pose;
00275 
00276                 try {
00277 
00278                         tfl_.transformPose("/map",tmp,tmp);
00279 
00280                 } catch(tf::TransformException& ex){
00281 
00282                    std::cerr << "Transform error: " << ex.what() << std::endl;
00283 
00284                    ROS_ERROR("Exception on TF transf.: %s",ex.what());
00285 
00286                    return;
00287                 }
00288 
00289                 ROS_INFO_ONCE("First gripper pose received.");
00290                 gripper_pose_rec_ = true;
00291                 gripper_pose_curr_ = tmp.pose;
00292 
00293         }
00294 
00295 }
00296 
00297 void BBOverlap::publish_points(tpoints points, ros::Publisher &pub, std_msgs::ColorRGBA c) {
00298 
00299         if (!publish_debug_markers_) return;
00300 
00301         visualization_msgs::Marker m;
00302 
00303         m.header.frame_id = "/map";
00304         m.header.stamp = ros::Time::now();
00305         m.type = visualization_msgs::Marker::SPHERE_LIST;
00306         m.action = visualization_msgs::Marker::ADD;
00307         m.color = c;
00308         m.lifetime = ros::Duration(1.5);
00309 
00310         m.scale.x = 0.05;
00311         m.scale.y = 0.05;
00312         m.scale.z = 0.05;
00313 
00314         m.pose.orientation.x = 0.0;
00315         m.pose.orientation.y = 0.0;
00316         m.pose.orientation.z = 0.0;
00317         m.pose.orientation.w = 1.0;
00318 
00319         for (unsigned int i=0; i < points.size(); i++) {
00320 
00321                 geometry_msgs::Point p;
00322 
00323                 p.x = points[i](0);
00324                 p.y = points[i](1);
00325                 p.z = points[i](2);
00326 
00327                 m.points.push_back(p);
00328 
00329         }
00330 
00331         if (pub.getNumSubscribers() > 0) {
00332 
00333                 pub.publish(m);
00334                 ROS_INFO_ONCE("Publishing %d spheres.",(int)m.points.size());
00335         }
00336 
00337 }
00338 
00339 
00340 
00341 BBOverlap::~BBOverlap() {
00342 
00343 
00344 }
00345 
00346 
00347 void BBOverlap::im_feedback_cb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& msg) {
00348 
00349 
00350         if (msg->marker_name == "unknown_object") {
00351 
00352                 ROS_INFO_ONCE("IM feedback received");
00353 
00354                 boost::mutex::scoped_lock(im_.mutex);
00355 
00356                 im_.bb.pose = msg->pose;
00357                 im_.bb.pose.position.z -= im_.bb.lwh.z/2.0;
00358 
00359                 im_.pose_rec = true;
00360 
00361         }
00362 
00363 }
00364 
00365 // computes volume for AABB
00366 double BBOverlap::points_volume(const tpoints &p) {
00367 
00368         double x = fabs((double)p[4](0) - (double)p[3](0));
00369         double y = fabs((double)p[4](1) - (double)p[3](1));
00370         double z = fabs((double)p[4](2) - (double)p[3](2));
00371 
00372         return x*y*z;
00373 
00374         //return fabs( (double)p[4](0)*(double)p[4](1)*(double)p[4](2) );
00375 
00376 }
00377 
00378 
00379 void BBOverlap::im_scale_cb(const srs_interaction_primitives::ScaleChangedConstPtr& msg) {
00380 
00381         ROS_INFO_ONCE("Scale received");
00382 
00383         boost::mutex::scoped_lock(im_.mutex);
00384 
00385         im_.bb.lwh = msg->new_scale;
00386         im_.bb.lwh.x *= 0.5;
00387         im_.bb.lwh.y *= 0.5;
00388 
00389         im_.scale_rec = true;
00390 
00391 }
00392 
00393 double BBOverlap::rmin(double val1, double val2) {
00394 
00395         if (val1<val2) return val1;
00396         else return val2;
00397 
00398 }
00399 
00400 double BBOverlap::rmax(double val1, double val2) {
00401 
00402         if (val1>val2) return val1;
00403         else return val2;
00404 
00405 }
00406 
00407 
00408 void BBOverlap::timer_cb(const ros::TimerEvent&) {
00409 
00410         ROS_INFO_ONCE("Timer triggered");
00411 
00412         boost::mutex::scoped_lock(im_.mutex);
00413 
00414         if (id_.pub.getNumSubscribers() > 0) {
00415 
00416                 id_.marker.header.stamp = ros::Time::now();
00417                 id_.pub.publish(id_.marker);
00418 
00419         }
00420 
00421         bool suc = false;
00422         double overlap = 0.0;
00423         ros::WallTime now = ros::WallTime::now();
00424 
00425         // we have received something...
00426         if (im_.pose_rec && im_.scale_rec) {
00427 
00428 
00429                 geometry_msgs::Pose pos = im_.bb.pose;
00430 
00431                 Eigen::Quaternionf q1(id_.bb.pose.orientation.w, id_.bb.pose.orientation.x, id_.bb.pose.orientation.y, id_.bb.pose.orientation.z);
00432                 Eigen::Affine3f tr2(q1.inverse());
00433 
00434                 tpoint p(pos.position.x, pos.position.y, pos.position.z);
00435 
00436                 p = tr2*p;
00437 
00438                 pos.position.x = p(0);
00439                 pos.position.y = p(1);
00440                 pos.position.z = p(2);
00441 
00442                 tbb tmp;
00443 
00444                 tmp.pose = pos;
00445                 tmp.lwh = im_.bb.lwh;
00446 
00447                 // generate new points
00448                 update_points(tmp,im_.points);
00449 
00450 
00451                 // take ideal and actual points in /map coord. system
00452                 tpoints id_p = id_.points;
00453                 tpoints im_p = im_.points;
00454 
00455                 tpoint p2(id_.bb.pose.position.x, id_.bb.pose.position.y, id_.bb.pose.position.z);
00456 
00457                 p2 = tr2*p2;
00458 
00459                 // shift it to 0,0
00460                 for (unsigned int i=0; i<im_p.size(); i++) {
00461 
00462                         id_p[i](0) -= id_.bb.pose.position.x;
00463                         id_p[i](1) -= id_.bb.pose.position.y;
00464                         id_p[i](2) -= id_.bb.pose.position.z;
00465 
00466                         im_p[i](0) -= p2(0);
00467                         im_p[i](1) -= p2(1);
00468                         im_p[i](2) -= p2(2);
00469 
00470                 }
00471 
00472                 double pdx = id_p[0](0);
00473                 double pdy = id_p[0](1);
00474                 double pdz = id_p[0](2);
00475 
00476 
00477                 // correction - shift one corner to 0,0
00478                 for (unsigned int i=0; i<im_p.size(); i++) {
00479 
00480                         if (id_p[i](0) < pdx) pdx = id_p[i](0);
00481                         if (id_p[i](1) < pdy) pdy = id_p[i](1);
00482                         if (id_p[i](2) < pdz) pdz = id_p[i](2);
00483 
00484                 }
00485 
00486                 /*tpoint p3(dx,dy,dz);
00487 
00488                 p3 = tr2*p3;*/
00489 
00490                 for (unsigned int i=0; i<im_p.size(); i++) {
00491 
00492                         id_p[i](0) -= pdx;
00493                         id_p[i](1) -= pdy;
00494                         id_p[i](2) -= pdz;
00495 
00496                         im_p[i](0) -= pdx;
00497                         im_p[i](1) -= pdy;
00498                         im_p[i](2) -= pdz;
00499 
00500                 }
00501 
00502                 if (points_volume(id_p) <= 0.0) {
00503 
00504                         ROS_ERROR("ID vol negative or zero!");
00505 
00506                 }
00507 
00508                 //publish_points(tpoints points, ros::Publisher &pub, std_msgs::ColorRGBA c)
00509                 std_msgs::ColorRGBA c;
00510                 c.a = 0.6;
00511                 c.g = 1.0;
00512 
00513                 publish_points(id_p, id_.points_pub, c);
00514 
00515                 c.g = 0.0;
00516                 c.b = 1.0;
00517                 publish_points(im_p, im_.points_pub, c);
00518 
00519                 // axis aligned
00520                 tpoints imaa_p = im_p;
00521 
00522                 double min_x = imaa_p[0](0);
00523                 double max_x = imaa_p[0](0);
00524 
00525                 double min_y = imaa_p[0](1);
00526                 double max_y = imaa_p[0](1);
00527 
00528                 double min_z = imaa_p[0](2);
00529                 double max_z = imaa_p[0](2);
00530 
00531        /*                       x               y               z
00532                 idp(0)=[0.400000,0.200000,0.000000]
00533                 idp(1)=[0.400000,0.000000,0.000000]
00534                 idp(2)=[0.000000,0.200000,0.000000]
00535                 idp(3)=[0.000000,0.000000,0.000000]
00536                 idp(4)=[0.400000,0.200000,0.400000]
00537                 idp(5)=[0.400000,0.000000,0.400000]
00538                 idp(6)=[0.000000,0.200000,0.400000]
00539                 idp(7)=[0.000000,0.000000,0.400000]*/
00540 
00541                 for (unsigned int i=0; i<im_p.size(); i++) {
00542 
00543                         if (min_x > imaa_p[i](0)) min_x =  imaa_p[i](0);
00544                         if (max_x < imaa_p[i](0)) max_x =  imaa_p[i](0);
00545 
00546                         if (min_y > imaa_p[i](1)) min_y =  imaa_p[i](1);
00547                         if (max_y < imaa_p[i](1)) max_y =  imaa_p[i](1);
00548 
00549                         if (min_z > imaa_p[i](2)) min_z =  imaa_p[i](2);
00550                         if (max_z < imaa_p[i](2)) max_z =  imaa_p[i](2);
00551 
00552                 }
00553 
00554                 imaa_p[0] = tpoint(max_x,max_y,min_z);
00555                 imaa_p[1] = tpoint(max_x,min_y,min_z);
00556                 imaa_p[2] = tpoint(min_x,max_y,min_z);
00557                 imaa_p[3] = tpoint(min_x,min_y,min_z);
00558                 imaa_p[4] = tpoint(max_x,max_y,max_z);
00559                 imaa_p[5] = tpoint(max_x,min_y,max_z);
00560                 imaa_p[6] = tpoint(min_x,max_y,max_z);
00561                 imaa_p[7] = tpoint(min_x,min_y,max_z);
00562 
00563                 c.g = 0.5;
00564                 c.b = 0.5;
00565                 publish_points(imaa_p, points_proc_pub_, c);
00566 
00567 
00568                 //tpoints ov_p;
00569 
00570                 double ov_p_vol = 0.0;
00571 
00572                 double dx = 0.0;
00573                 double dy = 0.0;
00574                 double dz = 0.0;
00575 
00576                 // compute "overlap bb"
00577                 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)) ) {
00578 
00579                         dx = rmin((double)id_p[0](0),(double)imaa_p[0](0)) - rmax((double)id_p[2](0),(double)imaa_p[2](0));
00580 
00581                         dy = rmin((double)id_p[0](1),(double)imaa_p[0](1)) - rmax((double)id_p[1](1),(double)imaa_p[1](1));
00582 
00583                         dz = rmin((double)id_p[4](2),(double)imaa_p[4](2)) - rmax((double)id_p[0](2),(double)imaa_p[0](2));
00584 
00585                         // x*y*z
00586                         ov_p_vol = dx*dy*dz;
00587 
00588                 };
00589 
00590                 double max_v = rmax(points_volume(id_p),points_volume(imaa_p));
00591 
00592                 overlap = ov_p_vol / max_v;
00593 
00594                 if (overlap > 1.0) overlap = 1.0; // just for a case...
00595 
00596 
00597                 if (overlap > bb_success_val_) {
00598 
00599                         if (bb_success_tmp_ == ros::WallTime(0)) bb_success_tmp_ = now;
00600                         else {
00601 
00602                                 if ((now - bb_success_tmp_) > ros::WallDuration(bb_success_min_dur_)) {
00603 
00604                                         if (bb_success_first_ == ros::WallTime(0)) bb_success_first_ = now;
00605                                         bb_success_last_ = now;
00606                                         suc = true;
00607 
00608                                 }
00609 
00610                         }
00611 
00612                 } else {
00613 
00614                         bb_success_tmp_ = ros::WallTime(0);
00615 
00616                 }
00617 
00618 
00619         } // if we have something
00620 
00621         double gripper_dist = -1.0;
00622 
00623         bool gr_suc = false;
00624 
00625         if (gripper_pose_rec_) {
00626 
00627                 double tmp;
00628 
00629                 tmp = pow(gripper_pose_.position.x - gripper_pose_curr_.position.x,2.0)
00630                                 + pow(gripper_pose_.position.y - gripper_pose_curr_.position.y,2.0)
00631                                 + pow(gripper_pose_.position.z - gripper_pose_curr_.position.z,2.0);
00632 
00633                 gripper_dist = pow(fabs(tmp),0.5);
00634 
00635                 if ( (gripper_dist < gr_success_val_) && arm_state_ok_) {
00636 
00637                         if (gr_success_tmp_ == ros::WallTime(0)) gr_success_tmp_ = now;
00638                         else {
00639 
00640                                 if ((now - gr_success_tmp_) > ros::WallDuration(gr_success_min_dur_)) {
00641 
00642                                         if (gr_success_first_ == ros::WallTime(0)) gr_success_first_ = now;
00643                                         gr_success_last_ = now;
00644                                         gr_suc = true;
00645 
00646                                 }
00647 
00648                         }
00649 
00650                 } else gr_success_tmp_ = ros::WallTime(0);
00651 
00652         }
00653 
00654         if ( (ros::WallTime::now() - last_log_out_) > ros::WallDuration(2.0) ) {
00655 
00656                 //ROS_INFO("ID vol: %f, MAA vol: %f",points_volume(id_p),points_volume(imaa_p));
00657 
00658                 //ROS_INFO("dx: %f, dy: %f, dz: %f, vol: %f",dx,dy,dz,ov_p_vol);
00659                 //ROS_INFO("overlap: %f%% (%f / %f)",overlap*100.0,ov_p_vol,max_v);
00660 
00661                 boost::posix_time::ptime f_s = bb_success_first_.toBoost();
00662                 boost::posix_time::ptime l_s = bb_success_last_.toBoost();
00663 
00664                 boost::posix_time::ptime f_gr = gr_success_first_.toBoost();
00665                 boost::posix_time::ptime l_gr = gr_success_last_.toBoost();
00666 
00667                 if (suc) printf("BB OVERLAP: %03.1f%% (success)\n",overlap*100);
00668                 else printf("BB OVERLAP: %03.1f%% (fail)\n",overlap*100);
00669 
00670                 double gdx = gripper_pose_.position.x - gripper_pose_curr_.position.x;
00671                 double gdy = gripper_pose_.position.y - gripper_pose_curr_.position.y;
00672                 double gdz = gripper_pose_.position.z - gripper_pose_curr_.position.z;
00673 
00674                 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());
00675                 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());
00676 
00677                 if (gr_suc) printf("GR Distance: %f [dx: %.2f, dy: %.2f, dz: %.2f] (success)\n",gripper_dist,gdx,gdy,gdz);
00678                 else printf("GR Distance: %f [dx: %.2f, dy: %.2f, dz: %.2f] (fail)\n",gripper_dist,gdx,gdy,gdz);
00679 
00680                 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());
00681                 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());
00682                 printf("\n");
00683 
00684                 last_log_out_ = ros::WallTime::now();
00685 
00686 
00687         } // if
00688 
00689 
00690         // publish ideal position of gripper
00691         if (gripper_pub_.getNumSubscribers() != 0) {
00692 
00693                 ROS_INFO_ONCE("Publishing gripper ideal position.");
00694 
00695                 gripper_marker_.header.stamp = ros::Time::now();
00696                 gripper_pub_.publish(gripper_marker_);
00697 
00698 
00699         }
00700 
00701 
00702 }
00703 
00704 
00705 int main(int argc, char** argv)
00706 {
00707 
00708 
00709       ROS_INFO("Starting");
00710       ros::init(argc, argv, "bb_overlap");
00711 
00712       BBOverlap *bb = new BBOverlap();
00713 
00714       ros::spin();
00715 
00716       delete bb;
00717 
00718       return 0;
00719 
00720 }