laser_people_map_filter_alg_node.cpp
Go to the documentation of this file.
00001 #include "laser_people_map_filter_alg_node.h"
00002 
00003 LaserPeopleMapFilterAlgNode::LaserPeopleMapFilterAlgNode(void) :
00004   algorithm_base::IriBaseAlgorithm<LaserPeopleMapFilterAlgorithm>(),
00005   tf_listener_(ros::Duration(10.f))
00006 {
00007   //init class attributes if necessary
00008   this->loop_rate_ = 60;//in [Hz]
00009 
00010   this->mapReceived=false;
00011   this->peopleReceived=false;
00012   this->neighborRadius = 0.25; //m
00013 
00014   this->markerWidth=0.5;
00015   this->markerHeight=0.2;
00016   this->markerR=1.0;
00017   this->markerG=1.0;
00018   this->markerB=0.0;
00019   this->markerA=0.75;
00020 
00021   this->frame="noframe";
00022 
00023   this->public_node_handle_.getParam("neighborRadius", this->neighborRadius);
00024 
00025   // [init publishers]
00026   this->peopleMarkers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("peopleMarkers", 100);
00027   this->mapFilteredPeople_publisher_ = this->public_node_handle_.advertise<iri_perception_msgs::detectionArray>("peopleFiltered", 100);
00028   
00029   // [init subscribers]
00030   this->map_subscriber_ = this->public_node_handle_.subscribe("map", 100, &LaserPeopleMapFilterAlgNode::map_callback, this);
00031   this->people_subscriber_ = this->public_node_handle_.subscribe("people", 100, &LaserPeopleMapFilterAlgNode::people_callback, this);
00032   
00033   // [init services]
00034   
00035   // [init clients]
00036   
00037   // [init action servers]
00038   
00039   // [init action clients]
00040 }
00041 
00042 LaserPeopleMapFilterAlgNode::~LaserPeopleMapFilterAlgNode(void)
00043 {
00044   // [free dynamic memory]
00045 }
00046 
00047 void LaserPeopleMapFilterAlgNode::mainNodeThread(void)
00048 {
00049   if(this->mapReceived && this->peopleReceived)
00050   {
00051     //ROS_INFO("LaserPeopleMapFilterAlgNode::mainNodeThread: in poses number %i", people.detection.size());
00052     iri_perception_msgs::detectionArray empty;
00053     this->detectionArray_msg_ = empty; 
00054     for(unsigned int i=0; i<this->people.detection.size() ; i++)
00055     {
00056       float x = this->people.detection[i].pose.pose.position.x;
00057       float y = this->people.detection[i].pose.pose.position.y;
00058       float res = this->map.info.resolution;
00059       int width = this->map.info.width;
00060       //int height= this->map.info.height;
00061       float x0 = this->map.info.origin.position.x;
00062       float y0 = this->map.info.origin.position.y;
00063       //this->map.info.origin.orientation;
00064       int grid_x = (unsigned int)((x - x0) / res);
00065       int grid_y = (unsigned int)((y - y0) / res);
00066       int index = grid_y*width + grid_x;
00067       int neighborDistance = int(this->neighborRadius/res);
00068       if(this->checkCellNearObstacle(index, neighborDistance))
00069       {
00070         //ROS_INFO("LaserPeopleMapFilterAlgNode::mainNodeThread: validated pose %d", i);
00071         //if it's valid, add pose
00072         this->detectionArray_msg_.detection.push_back(this->peopleIn.detection[i]);
00073         this->detectionArray_msg_.header= this->peopleIn.header;
00074       }
00075       else
00076       {
00077         //ROS_INFO("LaserPeopleMapFilterAlgNode::mainNodeThread: skipped people pose");
00078       }
00079     }
00080     this->peopleReceived=false;
00081     //ROS_INFO("LaserPeopleMapFilterAlgNode::mainNodeThread: out poses number %d", this->detectionArray_msg_.detection.size());
00082     if(this->detectionArray_msg_.detection.size()!=0)
00083     {
00084       this->getPeopleMarkers(this->detectionArray_msg_, this->MarkerArray_msg_);
00085       this->peopleMarkers_publisher_.publish(this->MarkerArray_msg_);
00086       this->mapFilteredPeople_publisher_.publish(this->detectionArray_msg_);
00087     }
00088   }
00089 
00090   // [fill msg structures]
00091   //this->MarkerArray_msg_.data = my_var;
00092   //this->detectionArray_msg_.data = my_var;
00093   
00094   // [fill srv structure and make request to the server]
00095   
00096   // [fill action structure and make request to the action server]
00097 
00098   // [publish messages]
00099   //this->peopleMarkers_publisher_.publish(this->MarkerArray_msg_);
00100   //this->mapFilteredPeople_publisher_.publish(this->detectionArray_msg_);
00101 }
00102 
00103 /*  [subscriber callbacks] */
00104 void LaserPeopleMapFilterAlgNode::map_callback(const nav_msgs::OccupancyGrid::ConstPtr& msg) 
00105 { 
00106   ROS_DEBUG("LaserPeopleMapFilterAlgNode::map_callback: New Message Received"); 
00107 
00108   //use appropiate mutex to shared variables if necessary 
00109   this->alg_.lock(); 
00110   //this->map_mutex_.enter(); 
00111 
00112   //std::cout << msg->data << std::endl; 
00113   if(!mapReceived){
00114     //ROS_INFO("LaserPeopleMapFilterAlgNode::map_callback: map Received"); 
00115     this->map.header = msg->header;
00116     this->map.info   = msg->info;
00117     this->map.data   = msg->data;
00118     this->mapReceived = true;
00119     this->frame = msg->header.frame_id;
00120   }
00121 
00122   //unlock previously blocked shared variables 
00123   this->alg_.unlock(); 
00124   //this->map_mutex_.exit(); 
00125 }
00126 
00127 void LaserPeopleMapFilterAlgNode::people_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg) 
00128 { 
00129   ROS_DEBUG("LaserPeopleMapFilterAlgNode::people_callback: New Message Received"); 
00130 
00131   //use appropiate mutex to shared variables if necessary 
00132   this->alg_.lock(); 
00133   //this->people_mutex_.enter(); 
00134 
00135   //std::cout << msg->data << std::endl;
00136   if(this->mapReceived)
00137   {
00138     this->peopleIn.header = msg->header;
00139     this->peopleIn.detection  = msg->detection;
00140     this->people.header   = msg->header;
00141     this->people.detection    = msg->detection;
00142     this->peopleReceived  = true;
00143 
00144     //if detections are not in map frame, transform them
00145     //ROS_INFO("poses frame, map frame: %s, %s",msg->header.frame_id.c_str(), this->frame.c_str());
00146     if(msg->header.frame_id != this->frame)
00147     {
00148       try
00149       {
00150         std::string source_frame = msg->header.frame_id;
00151         std::string target_frame = this->frame;
00152         ros::Time   target_time  = msg->header.stamp;
00153         bool tf_exists = tf_listener_.waitForTransform(target_frame,  source_frame, target_time, ros::Duration(5), ros::Duration(0.01));
00154         if(tf_exists)
00155         {
00156           geometry_msgs::PoseStamped poseOut;
00157           geometry_msgs::PoseStamped poseIn;
00158           for(unsigned int i=0; i<msg->detection.size(); i++)
00159           {
00160             //ROS_INFO("transforming pose");
00161             poseIn.header = msg->header;
00162             poseIn.pose   = msg->detection[i].pose.pose;
00163             poseIn.pose.orientation.w = 1;
00164             tf_listener_.transformPose( target_frame,     poseIn,     poseOut);
00165             this->people.detection[i].pose.pose = poseOut.pose;
00166             //ROS_INFO("LaserPeopleMapFilterAlgNode::people_callback: map pose %d: %f, %f", i , poseOut.pose.position.x, poseOut.pose.position.y);
00167           }
00168           this->people.header.frame_id = poseOut.header.frame_id;
00169         }
00170         else
00171         {
00172           ROS_INFO("LaserPeopleMapFilterAlgNode::No transform: %s-->%s", source_frame.c_str(), target_frame.c_str());
00173           this->peopleReceived = false;
00174         }
00175       }
00176       catch (tf::TransformException &ex)
00177       {
00178         ROS_INFO("LaserPeopleMapFilterAlgNode:: %s",ex.what());
00179         this->peopleReceived = false;
00180       }
00181     }
00182     if(!peopleReceived)
00183       ROS_INFO("LaserPeopleMapFilterAlgNode::people_callback: unable to use people detections");
00184   }
00185 
00186   //unlock previously blocked shared variables 
00187   this->alg_.unlock(); 
00188   //this->people_mutex_.exit(); 
00189 }
00190 
00191 /*  [service callbacks] */
00192 
00193 /*  [action callbacks] */
00194 
00195 /*  [action requests] */
00196 
00197 void LaserPeopleMapFilterAlgNode::node_config_update(Config &config, uint32_t level)
00198 {
00199   this->alg_.lock();
00200   this->neighborRadius    = config.neighborRadius;
00201   this->markerWidth       = config.markerWidth;
00202   this->markerHeight      = config.markerHeight;
00203   this->markerR           = config.markerR;
00204   this->markerG           = config.markerG;
00205   this->markerB           = config.markerB;
00206   this->markerA           = config.markerA;
00207   this->alg_.unlock();
00208 }
00209 
00210 void LaserPeopleMapFilterAlgNode::addNodeDiagnostics(void)
00211 {
00212 }
00213 
00214 /* main function */
00215 int main(int argc,char *argv[])
00216 {
00217   return algorithm_base::main<LaserPeopleMapFilterAlgNode>(argc, argv, "laser_people_map_filter_alg_node");
00218 }
00219 
00220 bool LaserPeopleMapFilterAlgNode::checkCellNearObstacle(int index, int neighborDistance)
00221 {
00222   //returns if the index cell is valid or not (obstacles near = not valid)
00223   bool valid = true;
00224   int      W = this->map.info.width;
00225   int      N = neighborDistance;
00226   //ROS_INFO("LaserPeopleMapFilterAlgNode::checkCellNearObstacle: neighbor distance %i ", neighborDistance); 
00227 
00228   // go over the distance-N neighbors
00229   for(int j=-N; j<N; j++)
00230    {
00231      for(int i=-N; i<N; i++)
00232      {
00233        int indexi = i*W+j+index;
00234        // check if indexi is in the map.data size
00235        if(indexi >0 && indexi < signed(map.data.size()))
00236        {
00237          //if the indexi cell is not free (!=0) the main cell is not valid
00238          if(this->map.data[indexi]!=0)
00239          {
00240            valid=false;
00241            break;
00242          }
00243        }
00244      }
00245      if(!valid)
00246        break;
00247    }
00248 
00249   return valid;
00250 }
00251 
00252 void LaserPeopleMapFilterAlgNode::getPeopleMarkers(iri_perception_msgs::detectionArray & peopleList, visualization_msgs::MarkerArray & people)
00253 {
00254   static unsigned int lastSize=0;
00255   people.markers.resize(std::max(uint(peopleList.detection.size()), lastSize));
00256   lastSize = peopleList.detection.size();
00257 
00258   for(unsigned int i=0; i<peopleList.detection.size(); i++)
00259   {
00260     people.markers[i].scale.x = this->markerWidth;
00261     people.markers[i].scale.y = this->markerWidth;
00262     people.markers[i].scale.z = this->markerHeight + peopleList.detection[i].probability;
00263 
00264     people.markers[i].pose.position.x = peopleList.detection[i].pose.pose.position.x;
00265     people.markers[i].pose.position.y = peopleList.detection[i].pose.pose.position.y;
00266     people.markers[i].pose.position.z = people.markers[i].scale.z/2.0 -0.44;
00267 
00268     people.markers[i].header.frame_id = peopleList.header.frame_id;
00269     people.markers[i].header.stamp    = peopleList.header.stamp;
00270 
00271     people.markers[i].id = i;
00272     people.markers[i].type = visualization_msgs::Marker::CYLINDER;
00273     people.markers[i].action = visualization_msgs::Marker::ADD;
00274 
00275     people.markers[i].lifetime = ros::Duration(0.5f);
00276 
00277     people.markers[i].pose.orientation.x = 0.0;
00278     people.markers[i].pose.orientation.y = 0.0;
00279     people.markers[i].pose.orientation.z = 0.0;
00280     people.markers[i].pose.orientation.w = 1.0;
00281 
00282     people.markers[i].color.r = this->markerR;
00283     people.markers[i].color.g = this->markerG;
00284     people.markers[i].color.b = this->markerB;
00285     people.markers[i].color.a = this->markerA;
00286   }
00287 
00288   //Delete extra previous markers;
00289   for(unsigned int i=peopleList.detection.size(); i<people.markers.size(); i++)
00290   {
00291     people.markers[i].action = visualization_msgs::Marker::DELETE;
00292   }
00293 }


iri_laser_people_map_filter
Author(s): fherrero
autogenerated on Fri Dec 6 2013 21:52:17