srs_leg_detector.cpp
Go to the documentation of this file.
00001 
00002 
00003 #include "ros/ros.h"
00004 
00005 #include "laser_processor.h"
00006 #include "calc_leg_features.h"
00007 
00008 #include "opencv/cxcore.h"
00009 #include "opencv/cv.h"
00010 #include "opencv/ml.h"
00011 
00012 //#include "rosrecord/Player.h"
00013 //#include "rosbag/bag.h"
00014 
00015 #include "srs_msgs/PositionMeasurement.h"
00016 #include "srs_msgs/HS_distance.h"
00017 
00018 #include "sensor_msgs/LaserScan.h"
00019 #include "std_msgs/Header.h"
00020 
00021 #include "tf/transform_listener.h"
00022 #include "tf/message_filter.h"
00023 #include "message_filters/subscriber.h"
00024 
00025 #include "srs_people_tracking_filter/tracker_kalman.h"
00026 //#include "srs_people_tracking_filter/tracker_particle.h" // particle
00027 #include "srs_people_tracking_filter/gaussian_pos_vel.h"
00028 
00029 #include "srs_people_tracking_filter/state_pos_vel.h"
00030 #include "srs_people_tracking_filter/rgb.h"
00031 
00032 #include <algorithm>
00033 
00034 
00035 //#include "actionlib/client/simple_action_client.h" - prevously used to pause the DM no longer needed
00036 //#include "srs_decision_making/ExecutionAction.h"
00037 
00038 #include "nav_msgs/OccupancyGrid.h"
00039 #include "nav_msgs/GetMap.h"
00040 
00041 // services
00042 #include <srs_leg_detector/DetectLegs.h>
00043 
00044 //typedef actionlib::SimpleActionClient <srs_decision_making::ExecutionAction> Client; - pausing no longer needed
00045 
00046 using namespace std;
00047 using namespace laser_processor;
00048 using namespace ros;
00049 using namespace tf;
00050 using namespace estimation;
00051 using namespace BFL;
00052 using namespace MatrixWrapper;
00053 
00054 
00055 static const double no_observation_timeout_s = 0.7;
00056 static const double max_second_leg_age_s     = 2.0;
00057 static const double max_track_jump_m         = 1; //1.0;
00058 static const double max_meas_jump_m          = 1; //0.75; // 1.0
00059 static const double leg_pair_separation_m    = 0.5;
00060 //static const string fixed_frame              = "odom_combined";
00061 static const string fixed_frame              = "/map";
00062 
00063 static const double det_dist__for_pause      = 0.5; // the distance to the person when the robot decides to pause
00064 static const double det_dist__for_resume      = 2.5; // the distance to the person when the robot decides to resume
00065 //
00066 //static const unsigned int num_particles=100; // particle
00067 
00068 
00069 
00070 
00071 
00072 class SavedFeature
00073 {
00074 public:
00075         static int nextid;
00076         TransformListener& tfl_;
00077 
00078         BFL::StatePosVel sys_sigma_;
00079           TrackerKalman filter_;
00080         //TrackerParticle filter_;  // particle
00081 
00082         string id_;
00083         string object_id;
00084         ros::Time time_;
00085         ros::Time meas_time_;
00086 
00087         Stamped<Point> position_;
00088         float dist_to_person_;
00089 
00090         //  leg tracker
00091         SavedFeature(Stamped<Point> loc, TransformListener& tfl)
00092         : tfl_(tfl),
00093           sys_sigma_(Vector3(0.05, 0.05, 0.05), Vector3(1.0, 1.0, 1.0)),
00094             filter_("tracker_name",sys_sigma_)
00095          // filter_("tracker_name",num_particles,sys_sigma_) // particle
00096         {
00097                 char id[100];
00098                 snprintf(id,100,"legtrack%d", nextid++);
00099                 id_ = std::string(id);
00100 
00101                 object_id = "";
00102                 time_ = loc.stamp_;
00103                 meas_time_ = loc.stamp_;
00104 
00105                 try {
00106                         tfl_.transformPoint(fixed_frame, loc, loc);
00107                 } catch(...) {
00108                         ROS_WARN("TF exception spot 6.");
00109                 }
00110                 StampedTransform pose( btTransform(Quaternion(0.0, 0.0, 0.0, 1.0), loc), loc.stamp_, id_, loc.frame_id_);
00111                 tfl_.setTransform(pose);
00112 
00113                 StatePosVel prior_sigma(Vector3(0.1,0.1,0.1), Vector3(0.0000001, 0.0000001, 0.0000001));
00114         //      cout<<loc.m_floats[0]<<", "<<loc.m_floats[1]<<","<<loc.m_floats[2]<<endl;
00115                 filter_.initialize(loc, prior_sigma, time_.toSec());
00116 
00117                 StatePosVel est;
00118                 filter_.getEstimate(est);
00119 
00120                 updatePosition();
00121         }
00122 
00123         void propagate(ros::Time time)
00124         {
00125                 time_ = time;
00126 
00127                 filter_.updatePrediction(time.toSec());
00128 
00129                 updatePosition();
00130         }
00131 
00132         void update(Stamped<Point> loc)
00133         {
00134                 StampedTransform pose( btTransform(Quaternion(0.0, 0.0, 0.0, 1.0), loc), loc.stamp_, id_, loc.frame_id_);
00135                 tfl_.setTransform(pose);
00136 
00137                 meas_time_ = loc.stamp_;
00138                 time_ = meas_time_;
00139 
00140                 SymmetricMatrix cov(3);
00141                 cov = 0.0;
00142                 cov(1,1) = 0.0025;
00143                 cov(2,2) = 0.0025;
00144                 cov(3,3) = 0.0025;
00145 
00146                 filter_.updateCorrection(loc, cov);
00147 
00148                 updatePosition();
00149         }
00150 
00151         double getLifetime()
00152         {
00153                 return filter_.getLifetime();
00154         }
00155 
00156 private:
00157         void updatePosition()
00158         {
00159                 StatePosVel est;
00160                 filter_.getEstimate(est);
00161 
00162                 position_[0] = est.pos_[0];
00163                 position_[1] = est.pos_[1];
00164                 position_[2] = est.pos_[2];
00165                 position_.stamp_ = time_;
00166                 position_.frame_id_ = fixed_frame;
00167         }
00168 };
00169 
00170 int SavedFeature::nextid = 0;
00171 
00172 
00173 
00174 
00175 
00176 class Legs
00177 {
00178 public:
00179         Stamped<Point> loc_;
00180         string type_;
00181         Legs(Stamped<Point> loc, string type)
00182         : loc_(loc)
00183         , type_(type)
00184         {};
00185 
00186 };
00187 
00188 class Pair
00189 {
00190 public:
00191         Point pos1_;
00192         Point pos2_;
00193         float dist_sep_m;
00194         Pair(Point pos1,Point pos2,float dist)
00195         :pos1_(pos1)
00196         ,pos2_(pos2)
00197         ,dist_sep_m(dist)
00198         {};
00199 };
00200 
00201 class MatchedFeature
00202 {
00203 public:
00204         Legs* candidate_;
00205         SavedFeature* closest_;
00206         float distance_;
00207 
00208         MatchedFeature(Legs* candidate, SavedFeature* closest, float distance)
00209         : candidate_(candidate)
00210         , closest_(closest)
00211         , distance_(distance)
00212         {}
00213 
00214         inline bool operator< (const MatchedFeature& b) const
00215         {
00216                 return (distance_ <  b.distance_);
00217         }
00218 };
00219 
00220 
00221 int g_argc;
00222 char** g_argv;
00223 string scan_topic = "scan_front";
00224 
00225 
00226 
00227 // actual legdetector node
00228 class LegDetector
00229 {
00230  
00231  
00232 // srs_decision_making::ExecutionGoal goal;  // goal that will be sent to the actionserver - - pausing no longer needed
00233  
00234 
00235  bool pauseSent;
00236  int counter;
00237  srs_msgs::HS_distance  distance_msg;
00238  
00239 // Client client; // actionLib client for connection with DM action server - - pausing no longer needed
00240 
00241 
00242  ros::ServiceClient client_map;  // clent for the getMap service
00243  nav_msgs::GetMap srv_map;
00244         
00245  int ind_x, ind_y;
00246 
00247  short int map_data []; 
00248 
00249  double tmp;
00250 
00251  int indtmp;
00252   
00253 
00254 public:
00255         NodeHandle nh_;
00256 
00257         TransformListener tfl_;
00258 
00259         ScanMask mask_;
00260 
00261         int mask_count_;
00262 
00263         CvRTrees forest;
00264 
00265         float connected_thresh_;
00266 
00267         int feat_count_;
00268 
00269         char save_[100];
00270 
00271         list<SavedFeature*> saved_features_;
00272         boost::mutex saved_mutex_;
00273 
00274         int feature_id_;
00275         
00276         // topics
00277         ros::Publisher leg_cloud_pub_ , leg_detections_pub_ , human_distance_pub_ ;  //ROS topic publishers
00278 
00279         ros::Publisher tracker_measurements_pub_;
00280 
00281         message_filters::Subscriber<srs_msgs::PositionMeasurement> people_sub_;
00282         message_filters::Subscriber<sensor_msgs::LaserScan> laser_sub_;
00283         tf::MessageFilter<srs_msgs::PositionMeasurement> people_notifier_;
00284         tf::MessageFilter<sensor_msgs::LaserScan> laser_notifier_;
00285 
00286         // services
00287         ros::ServiceServer service_server_detect_legs_;
00288 
00289         // detections data for service_server_detect_legs_ service
00290         vector<geometry_msgs::Point32> detected_legs;
00291 
00292 
00293         
00294                    
00295 
00296 
00297 
00298         LegDetector(ros::NodeHandle nh) :
00299                 nh_(nh),
00300                 mask_count_(0),
00301                 connected_thresh_(0.06),
00302                 feat_count_(0),
00303                 people_sub_(nh_,"people_tracker_filter",10),
00304         //      //laser_sub_(nh_,"scan",10),
00305                 laser_sub_(nh_,scan_topic,10),
00306         //      laser_sub_(nh_,"scan_front",10),
00307                 people_notifier_(people_sub_,tfl_,fixed_frame,10),
00308                 laser_notifier_(laser_sub_,tfl_,fixed_frame,10),
00309                 detected_legs ()
00310         //        client ("srs_decision_making_actions",true) - - pausing no longer needed
00311                 
00312         {
00313                 
00314                 if (g_argc > 1) {
00315                         forest.load(g_argv[1]);
00316                         feat_count_ = forest.get_active_var_mask()->cols;
00317                         printf("Loaded forest with %d features: %s\n", feat_count_, g_argv[1]);
00318                 } else {
00319                         printf("Please provide a trained random forests classifier as an input.\n");
00320                         shutdown();
00321                 }
00322 
00323                 // advertise topics
00324                 //    leg_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud>("kalman_filt_cloud",10);
00325                 leg_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud>("tracked_people",10);
00326                 leg_detections_pub_ = nh_.advertise<sensor_msgs::PointCloud>("leg_detections_cloud",10);
00327                 tracker_measurements_pub_ = nh_.advertise<srs_msgs::PositionMeasurement>("people_tracker_measurements",1);
00328                 human_distance_pub_= nh_.advertise<srs_msgs::HS_distance>("HS_distance",10);                
00329  
00330                 //              people_notifier_.registerCallback(boost::bind(&LegDetector::peopleCallback, this, _1));
00331                 people_notifier_.setTolerance(ros::Duration(0.01));
00332                 laser_notifier_.registerCallback(boost::bind(&LegDetector::laserCallback, this, _1));
00333                 laser_notifier_.setTolerance(ros::Duration(0.01));
00334                  
00335                 //services
00336                 service_server_detect_legs_ = nh_.advertiseService("detect_legs", &LegDetector::detectLegsCallback, this);
00337 
00338                 
00339 
00340                 feature_id_ = 0;
00341                 pauseSent = false;
00342                 counter = 1;
00343                 
00344                 client_map = nh_.serviceClient<nav_msgs::GetMap>("/static_map"); // geting the clent for the map ready
00345                        
00346                 if (client_map.call(srv_map)) {  // call to srv_map OK
00347                   printf ("The cells in the ocupancy grid with size %i are:\n", srv_map.response.map.data.size());
00348                   printf (" width %i are:\n", srv_map.response.map.info.width);
00349                   printf (" height %i are:\n", srv_map.response.map.info.height);
00350                   printf (" resolution %f is:\n", srv_map.response.map.info.resolution);
00351                   geometry_msgs::Pose pose = srv_map.response.map.info.origin;
00352                   printf (" x position is %f \n", pose.position.x);
00353                   printf (" y position is %f \n", pose.position.y);
00354                   printf (" z position is %f \n", pose.position.z);
00355                   printf (" x orientation is %f \n", pose.orientation.x);
00356                   printf (" y orientation is %f \n", pose.orientation.y);
00357                   printf (" z orientation is %f \n", pose.orientation.z);
00358                   printf (" w orientation is %f \n", pose.orientation.w);
00359 
00360                    
00361               
00362                   // printing the map
00363                   for (int i = 0 ; i<320 ; i++) {
00364                     for (int k = 0; k<320 ; k++) {
00365                     if   (srv_map.response.map.data [i*320+k]<0) printf (".");
00366                     else if (srv_map.response.map.data [i*320+k]>=0 && srv_map.response.map.data [i*320+k]<100) printf ("#");
00367                     else if (srv_map.response.map.data [i*320+k]==100) printf ("@");
00368                    }
00369                    printf ("\n");
00370                   }
00371   
00372                 } 
00373                 else
00374                 {
00375                    ROS_ERROR("Failed to call service GetMap");
00376                 }
00377        }
00378 
00379 
00380         ~LegDetector()
00381         {
00382         }
00383 
00384 
00385 // actionlib Pause Call
00386    bool sendActionLibGoalPause()
00387         {
00388 
00389         return true; //- - pausing no longer needed
00390 
00391          /* if (pauseSent) // Pause Action has been  sent already
00392                 return true;
00393 
00394          
00395               
00396 
00397        //      Client client ("srs_decision_making_actions",true);
00398 
00399 
00400                if (!client.waitForServer(ros::Duration(10))) // ros::Duration(5)
00401                                    
00402  
00403                    {
00404                       printf(" Unable to establish connection with ActionServer in DM !!! Sending goals to DM when person is detected is disabled\n");
00405                       return false;
00406                    }   
00407    
00408        
00409        //         goal.action="pause"; - - pausing no longer needed
00410        //         goal.parameter=""; - - pausing no longer needed
00411         //        goal.priority=counter++; -- - pausing no longer needed
00412         //        client.sendGoal(goal); - - pausing no longer needed
00413                 pauseSent = true;
00414            //wait for the action to return
00415                 bool finished_before_timeout = client.waitForResult(ros::Duration(2));
00416 
00417                if (finished_before_timeout)
00418                 {
00419                  actionlib::SimpleClientGoalState state = client.getState();
00420                  ROS_INFO("Action call to DM finished with state: %s",state.toString().c_str()); 
00421                 return true;
00422                 }
00423               else
00424                 ROS_INFO("Action call to DM did not finish before timeout");
00425               
00426             return false;
00427 */
00428        }
00429 
00430 
00431 
00432 
00433 // alerts when the distance is bigger that a specified treshold              
00434 void measure_distance (double dist) {  
00435                       printf ("Distance %f \n" , dist);  // the distance to the detected human
00436                        
00437                        distance_msg.distance = dist*100;
00438                        human_distance_pub_.publish(distance_msg);
00439                       
00440                        if ( !pauseSent && dist < det_dist__for_pause )
00441                           {
00442                             printf ("Local user too close ! Sending Pause ActionLibGoal to the server. Waiting for Action server responce \n"); 
00443                             sendActionLibGoalPause();
00444                            } 
00445                         else if ( dist > det_dist__for_resume && pauseSent ) {
00446                             printf ("Local user is far away now ! Sending Resume ActionLibGoal to the server. Waiting for Action server responce \n"); 
00447                             sendActionLibGoalResume();
00448 
00449                            }
00450 
00451 }
00452 
00453 // actionlib Resume Call
00454  bool sendActionLibGoalResume()
00455         {
00456 
00457            return true; // pause no longer needed
00458         /*
00459 
00460          if (!pauseSent ) // Resume Action has been  sent already
00461                 return true;
00462 
00463 
00464             
00465               
00466 
00467    //          Client client ("srs_decision_making_actions",true);
00468 
00469 
00470                 if (!client.waitForServer())  // ros::Duration(5)
00471                                    
00472  
00473                    {
00474                       printf(" Unable to establish connection with ActionServer in DM !!! Sending goals to DM when person is detected is disabled\n");
00475                       return false;
00476                    }   
00477    
00478 
00479                 goal.action="resume";
00480                 goal.parameter="";
00481                 goal.priority=counter++;
00482                 client.sendGoal(goal);
00483                 pauseSent = false;
00484            //wait for the action to return
00485                 bool finished_before_timeout = client.waitForResult(ros::Duration(2));
00486 
00487                if (finished_before_timeout)
00488                 {
00489                  actionlib::SimpleClientGoalState state = client.getState();
00490                  ROS_INFO("Action call to DM finished with state: %s",state.toString().c_str()); 
00491                 return true;
00492                 }
00493               else
00494                 ROS_INFO("Action call to DM did not finish before timeout");
00495               
00496             return false;
00497        */
00498 
00499        }
00500 
00501 
00502 
00503       // calback for the DetectLegs service        
00504         bool detectLegsCallback(srs_leg_detector::DetectLegs::Request &req, srs_leg_detector::DetectLegs::Response &res)
00505         {
00506         
00507 
00508          res.leg_list.header.stamp = ros::Time::now();
00509 
00510 
00511          geometry_msgs::Point32 pt1,pt2,pt3,pt4;
00512 
00513          pt1.x=2.0; pt1.y=2.0;
00514          pt2.x=-2.0; pt2.y=2.0;
00515          pt3.x=2.0; pt3.y=-2.0;
00516          pt4.x=-2.0; pt4.y=-2.0;
00517           
00518          //res.leg_list.points = detected_legs;
00519          res.leg_list.points.push_back(pt1);    
00520          res.leg_list.points.push_back(pt2);
00521          res.leg_list.points.push_back(pt3);
00522          res.leg_list.points.push_back(pt4);
00523 
00524          return true;
00525         }
00526 
00527 
00528 
00529         void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
00530         {
00531                 geometry_msgs::Point32 pt_temp; // used in building the detected_legs vector
00532                 detected_legs.clear(); //to be ready for the new detections
00533                 float map_value;
00534                 ScanProcessor processor(*scan, mask_);
00535 
00536                 processor.splitConnected(connected_thresh_);
00537                 processor.removeLessThan(5);
00538 
00539                 CvMat* tmp_mat = cvCreateMat(1,feat_count_,CV_32FC1);
00540 
00541                 // if no measurement matches to a tracker in the last <no_observation_timeout>  seconds: erase tracker
00542                 ros::Time purge = scan->header.stamp + ros::Duration().fromSec(-no_observation_timeout_s);
00543                 list<SavedFeature*>::iterator sf_iter = saved_features_.begin();
00544                 while (sf_iter != saved_features_.end())
00545                 {
00546                         if ((*sf_iter)->meas_time_ < purge)
00547                         {
00548                                 delete (*sf_iter);
00549                                 saved_features_.erase(sf_iter++);
00550                         }
00551                         else
00552                                 ++sf_iter;
00553                 }
00554 
00555 
00556                 // System update of trackers, and copy updated ones in propagate list
00557                 list<SavedFeature*> propagated;
00558                 for (list<SavedFeature*>::iterator sf_iter = saved_features_.begin();
00559                                 sf_iter != saved_features_.end();
00560                                 sf_iter++)
00561                 {
00562                         (*sf_iter)->propagate(scan->header.stamp);
00563                         propagated.push_back(*sf_iter);
00564                 }
00565 
00566                 // Detection step: build up the set of leg candidates clusters
00567                 list<SampleSet*> leg_candidates;
00568                 for (list<SampleSet*>::iterator i = processor.getClusters().begin();
00569                                 i != processor.getClusters().end();
00570                                 i++)
00571                 {
00572                         vector<float> f = calcLegFeatures(*i, *scan);
00573 
00574                         for (int k = 0; k < feat_count_; k++)
00575                                 tmp_mat->data.fl[k] = (float)(f[k]);
00576 
00577                         if (forest.predict( tmp_mat ) > 0)
00578                         {
00579                                 leg_candidates.push_back(*i); // adds a new element
00580                         }
00581                 }
00582                 // build list of positions
00583                 list<Point> positions;
00584                 for (list<SampleSet*>::iterator i = leg_candidates.begin();
00585                                 i != leg_candidates.end();
00586                                 i++)
00587                 {
00588                         Point pos=(*i)->center();
00589                         positions.push_back(pos);
00590              
00591                         measure_distance ((*i)->center().distance(Point(0,0,0)));  // measures the distance from the robot to the detected human and pause if needed
00592                   
00593 
00594                   
00595 
00596                 }
00597 
00598                 // Build up the set of pair of closest positions
00599                 list<Pair*> candidates;
00600 
00601 
00602                 while (positions.size()!=0){
00603                         if (positions.size()==1){
00604                                 list<Point>::iterator it1=positions.begin();
00605 
00606                                 list<Pair*>::iterator new_c=candidates.insert(candidates.end(),new Pair((*it1),(*it1),float(0)));
00607 
00608                                 positions.erase(it1);
00609 
00610                         }
00611                         else{
00612                                 list<Point>::iterator it1=positions.begin();
00613                                 list<Point>::iterator it2=it1;
00614                                 ++it2;
00615 
00616                                 float closest_dist;
00617                                 closest_dist=(*it1).distance(*it2);
00618 
00619                                 float dist;
00620                                 list<Point>::iterator it3;
00621                                 it3=it2;
00622                                 ++it2;
00623                                 for (;it2!= positions.end();it2++){
00624                                         dist=(*it1).distance(*it2);
00625 
00626                                         if (dist<closest_dist){
00627                                                 closest_dist=dist;
00628                                                 it3=it2;
00629                                         }
00630                                 }
00631 
00632                                 list<Pair*>::iterator new_c=candidates.insert(candidates.end(),new Pair((*it1),(*it3),closest_dist));
00633                                 positions.erase(it1);
00634                                 positions.erase(it3);
00635 
00636                         }
00637 
00638                 }
00639 
00640                 //Build the set of pair of legs and single legs
00641                 list<Legs*> legs;
00642 
00643                 for(list<Pair*>::iterator i = candidates.begin();i != candidates.end();i++){
00644 
00645                         if((*i)->dist_sep_m==0){
00646                                 Stamped<Point> loc1((*i)->pos1_,scan->header.stamp, scan->header.frame_id);
00647                                 list<Legs*>::iterator new_pair=legs.insert(legs.end(),new Legs(loc1,"single"));
00648 
00649                         } else {
00650                                 if ((*i)->dist_sep_m<=leg_pair_separation_m){
00651                                         Stamped<Point> loc((((*i)->pos1_).operator +=((*i)->pos2_)).operator /=(btScalar(2)),scan->header.stamp, scan->header.frame_id);
00652                                         list<Legs*>::iterator new_pair=legs.insert(legs.end(),new Legs(loc,"pair"));
00653                                 }
00654                                 else{
00655                                         Stamped<Point> loc1((*i)->pos1_,scan->header.stamp, scan->header.frame_id);
00656                                         Stamped<Point> loc2((*i)->pos2_,scan->header.stamp, scan->header.frame_id);
00657                                         list<Legs*>::iterator new_s=legs.insert(legs.end(),new Legs(loc1,"single"));
00658                                         new_s=legs.insert(legs.end(),new Legs(loc2,"single"));
00659                                 }
00660                         }
00661 
00662                 }
00663 
00664 
00665         // For each candidate, find the closest tracker (within threshold) and add to the match list
00666         // If no tracker is found, start a new one
00667         multiset<MatchedFeature> matches;
00668 
00669         vector<geometry_msgs::Point32> detections_visualize(legs.size()); // used to visuallise the leg candidates
00670 
00671         int i = 0;
00672         for (list<Legs*>::iterator cf_iter = legs.begin();cf_iter != legs.end(); cf_iter++,i++) {
00673                 
00674                Stamped<Point> loc=(*cf_iter)->loc_;
00675                 try {
00676                         tfl_.transformPoint(fixed_frame, loc, loc);
00677                 } catch(...) {
00678                         ROS_WARN("TF exception spot 3.");
00679                 }
00680                 
00681                // int y111 = (*loc);
00682                 
00683 
00684                 
00685 
00686 
00687                 ind_x = (loc[0] / srv_map.response.map.info.resolution + srv_map.response.map.info.width / 2);  //x index in the ocupancy map
00688                 ind_y = (loc[1] / srv_map.response.map.info.resolution + srv_map.response.map.info.height / 2); //y index in the ocupancy map
00689 
00690                 indtmp = ind_y*160+ind_x; //printing for checking
00691                 printf ("map index: %i \n",indtmp );
00692 
00693                 printf ( "map x: %f, map y: %f, index_x: %i, index_y:%i, comb index:%i ",loc[0], loc[1], ind_x , ind_y, ind_y*160+ind_x );
00694                 map_value =  srv_map.response.map.data [ind_y*160+ind_x];
00695                 printf ("map_value: %f \n",map_value); 
00696           
00697                 if (map_value == 100)  { 
00698                   printf ("the point is on occupied cell of the map \n");
00699                  }
00700 
00701                 if (map_value < 0)  {
00702                   printf ("the point is outside the map \n");
00703                  } 
00704                  
00705                 if (map_value > 0 && map_value < 100)  {
00706                   printf ("GOOD - the point is inside the map \n");
00707                    if (pauseSent)
00708 
00709                                   detections_visualize[i].z=1.0;
00710 
00711                                 else
00712 
00713                                   detections_visualize[i].z=0.0;
00714 
00715 
00716                  } else {
00717                   detections_visualize[i].z =-1.0;
00718                   }
00719 
00720                            
00721  
00722                 
00723                 detections_visualize[i].x =  loc[0];
00724                 detections_visualize[i].y = loc[1];
00725                 
00726 
00727                   
00728 
00729 
00730                 list<SavedFeature*>::iterator closest = propagated.end();
00731                 float closest_dist = max_track_jump_m;
00732 
00733                 for (list<SavedFeature*>::iterator pf_iter = propagated.begin();
00734                                 pf_iter != propagated.end();
00735                                 pf_iter++)
00736                 {
00737                         // find the closest distance between candidate and trackers
00738                         float dist = loc.distance((*pf_iter)->position_);
00739                         if ( dist < closest_dist )
00740                         {
00741                                 closest = pf_iter;
00742                                 closest_dist = dist;
00743                         }
00744                 }
00745                 // Nothing close to it, start a new track
00746                 if (closest == propagated.end())
00747                 {
00748                         list<SavedFeature*>::iterator new_saved = saved_features_.insert(saved_features_.end(), new SavedFeature(loc, tfl_));
00749                 }
00750                 // Add the candidate, the tracker and the distance to a match list
00751                 else
00752                         matches.insert(MatchedFeature(*cf_iter,*closest,closest_dist));
00753         }
00754 
00755 
00756 
00757 
00758         // loop through _sorted_ matches list
00759         // find the match with the shortest distance for each tracker
00760         while (matches.size() > 0)
00761         {
00762                 multiset<MatchedFeature>::iterator matched_iter = matches.begin();
00763                 bool found = false;
00764                 list<SavedFeature*>::iterator pf_iter = propagated.begin();
00765                 while (pf_iter != propagated.end())
00766                 {
00767                         // update the tracker with this candidate
00768                         if (matched_iter->closest_ == *pf_iter)
00769                         {
00770                                 // Transform candidate to fixed frame
00771                                 Stamped<Point> loc(matched_iter->candidate_->loc_, scan->header.stamp, scan->header.frame_id);
00772                                 try {
00773                                         tfl_.transformPoint(fixed_frame, loc, loc);
00774                                 } catch(...) {
00775                                         ROS_WARN("TF exception spot 4.");
00776                                 }
00777 
00778                                 // Update the tracker with the candidate location
00779                                 matched_iter->closest_->update(loc);
00780 
00781                                 // remove this match and
00782                                 matches.erase(matched_iter);
00783                                 propagated.erase(pf_iter++);
00784                                 found = true;
00785                                 break;
00786                         }
00787                         // still looking for the tracker to update
00788                         else
00789                         {
00790                                 pf_iter++;
00791                         }
00792                 }
00793 
00794                 // didn't find tracker to update, because it was deleted above
00795                 // try to assign the candidate to another tracker
00796                 if (!found)
00797                 {
00798                         Stamped<Point> loc(matched_iter->candidate_->loc_, scan->header.stamp, scan->header.frame_id);
00799                         try {
00800                                 tfl_.transformPoint(fixed_frame, loc, loc);
00801                         } catch(...) {
00802                                 ROS_WARN("TF exception spot 5.");
00803                         }
00804 
00805                         list<SavedFeature*>::iterator closest = propagated.end();
00806                         float closest_dist = max_track_jump_m;
00807 
00808                         for (list<SavedFeature*>::iterator remain_iter = propagated.begin();
00809                                         remain_iter != propagated.end();
00810                                         remain_iter++)
00811                         {
00812                                 float dist = loc.distance((*remain_iter)->position_);
00813                                 if ( dist < closest_dist )
00814                                 {
00815                                         closest = remain_iter;
00816                                         closest_dist = dist;
00817                                 }
00818                         }
00819 
00820                         // no tracker is within a threshold of this candidate
00821                         // so create a new tracker for this candidate
00822                         if (closest == propagated.end())
00823                                 list<SavedFeature*>::iterator new_saved = saved_features_.insert(saved_features_.end(), new SavedFeature(loc, tfl_));
00824                         else
00825                                 matches.insert(MatchedFeature(matched_iter->candidate_,*closest,closest_dist));
00826                         matches.erase(matched_iter);
00827                 }
00828         }
00829 
00830                         cvReleaseMat(&tmp_mat); tmp_mat = 0;
00831 
00832                         vector<geometry_msgs::Point32> filter_visualize(saved_features_.size());
00833                         
00834 
00835                         vector<float> weights(saved_features_.size());
00836                         sensor_msgs::ChannelFloat32 channel;
00837                         
00838                         i = 0;
00839 
00840                         for (list<SavedFeature*>::iterator sf_iter = saved_features_.begin();
00841                                         sf_iter != saved_features_.end();
00842                                         sf_iter++,i++)
00843                         {
00844                                 // reliability
00845                                 StatePosVel est;
00846                                 (*sf_iter)->filter_.getEstimate(est);
00847                                 double reliability = fmin(1.0, fmax(0.1, est.vel_.length() / 0.5));
00848 
00849                                 // publish filter result
00850                                 
00851                                 filter_visualize[i].x = est.pos_[0];
00852                                 filter_visualize[i].y = est.pos_[1];
00853                                 //      filter_visualize[i].z = est.pos_[2];
00854                                 if (pauseSent)
00855                                   filter_visualize[i].z=1.0;
00856                                 else
00857                                   filter_visualize[i].z=0.0;
00858 
00859 
00860 
00861 
00862                                 // fill up the detected_legs vector for later use
00863                                  
00864                                 pt_temp.x =  est.pos_[0];
00865                                 pt_temp.y =  est.pos_[1];
00866                                 pt_temp.z = 0;
00867 
00868                                 detected_legs.push_back(pt_temp);
00869                                 
00870 
00871 
00872                                 weights[i] = *(float*)&(rgb[min(998, max(1, (int)trunc( reliability*999.0 )))]);
00873 
00874                                 srs_msgs::PositionMeasurement pos;
00875                                 pos.header.stamp = (*sf_iter)->time_;
00876                                 pos.header.frame_id = fixed_frame;
00877                                 pos.name = "leg_detector";
00878                                 pos.object_id = (*sf_iter)->object_id;
00879                                 pos.pos.x = est.pos_[0];
00880                                 pos.pos.y = est.pos_[1];
00881                                 pos.pos.z = est.pos_[2];
00882                                 pos.covariance[0] = pow(0.3 / reliability,2.0);
00883                                 pos.covariance[1] = 0.0;
00884                                 pos.covariance[2] = 0.0;
00885                                 pos.covariance[3] = 0.0;
00886                                 pos.covariance[4] = pow(0.3 / reliability,2.0);
00887                                 pos.covariance[5] = 0.0;
00888                                 pos.covariance[6] = 0.0;
00889                                 pos.covariance[7] = 0.0;
00890                                 pos.covariance[8] = 10000.0;
00891                                 pos.initialization = 0;
00892 
00893                                 // If I've already seen this leg, publish its position.
00894                                 if ((*sf_iter)->object_id != "")
00895                                         tracker_measurements_pub_.publish(pos);
00896                         }
00897 
00898                         // visualize all trackers
00899                         
00900                         channel.name = "rgb";
00901                         channel.values = weights;
00902                         sensor_msgs::PointCloud  people_cloud;  //from the filter
00903                         sensor_msgs::PointCloud  detections_cloud;  //directly from detections
00904 
00905                         people_cloud.channels.push_back(channel);
00906                         people_cloud.header.frame_id = fixed_frame;//scan_.header.frame_id;
00907                         people_cloud.header.stamp = scan->header.stamp;
00908                         people_cloud.points  = filter_visualize;
00909                         leg_cloud_pub_.publish(people_cloud);
00910 
00911                         detections_cloud.channels.push_back(channel);
00912                         detections_cloud.header.frame_id = fixed_frame;//scan_.header.frame_id;
00913                         detections_cloud.header.stamp = scan->header.stamp;
00914                         detections_cloud.points  = detections_visualize;
00915                         leg_detections_pub_.publish(detections_cloud);
00916 
00917 
00918 
00919 }
00920 
00921 };
00922 
00923 
00924 
00925 
00926 
00927 int main(int argc, char **argv)
00928 {
00929         ros::init(argc, argv,"laser_processor");
00930         g_argc = argc;
00931         g_argv = argv;
00932       
00933         if (g_argc > 2) {
00934                         scan_topic = g_argv[2];
00935                         
00936                         printf("Listening on topic %s : \n", g_argv[2]);
00937                 } else {
00938                         printf("Please provide the input topic as a parameter,e.g. scan_front. Assuming scan_front ! \n");
00939                         shutdown();
00940                 }
00941        
00942       
00943        
00944 
00945         
00946 
00947 
00948         ros::NodeHandle nh;
00949         LegDetector ld(nh);
00950         
00951 
00952         ros::spin();
00953         
00954 
00955         return 0;
00956 }
00957 


srs_leg_detector
Author(s): Alex Noyvirt
autogenerated on Mon Oct 6 2014 09:16:48