laser_people_detection_fusion_alg_node.cpp
Go to the documentation of this file.
00001 #include "laser_people_detection_fusion_alg_node.h"
00002 #include<tf/tf.h>
00003 
00004 LaserPeopleDetectionFusionAlgNode::LaserPeopleDetectionFusionAlgNode(void) :
00005   algorithm_base::IriBaseAlgorithm<LaserPeopleDetectionFusionAlgorithm>(),
00006   tf_listener_(ros::Duration(10.f)),
00007   target_frame_("/front_laser"),
00008   fixed_frame_("/base_link")
00009 {
00010   //init class attributes if necessary
00011   //this->loop_rate_ = 2;//in [Hz]
00012   people1ready=false;
00013   people2ready=false;
00014 
00015   // [init publishers]
00016   this->people_publisher_ = this->public_node_handle_.advertise<iri_nav_msgs::PoseWithCovarianceStampedArray>("people", 100);
00017   
00018   // [init subscribers]
00019   this->people2_subscriber_ = this->public_node_handle_.subscribe("people2", 100, &LaserPeopleDetectionFusionAlgNode::people2_callback, this);
00020   this->people1_subscriber_ = this->public_node_handle_.subscribe("people1", 100, &LaserPeopleDetectionFusionAlgNode::people1_callback, this);
00021   
00022   // [init services]
00023   
00024   // [init clients]
00025   
00026   // [init action servers]
00027   
00028   // [init action clients]
00029   
00030   std::string tf_prefix;
00031   public_node_handle_.param<std::string>("tf_prefix", tf_prefix, "");
00032   
00033   target_frame_ = tf_prefix + target_frame_;
00034   fixed_frame_  = tf_prefix  + fixed_frame_;
00035 
00036 }
00037 
00038 LaserPeopleDetectionFusionAlgNode::~LaserPeopleDetectionFusionAlgNode(void)
00039 {
00040   // [free dynamic memory]
00041 }
00042 
00043 void LaserPeopleDetectionFusionAlgNode::mainNodeThread(void)
00044 {
00045   // [fill msg structures]
00046 
00047   if(people1ready)
00048   {
00049     this->people1_mutex_.enter(); 
00050     PoseWithCovarianceStampedArray_msg_.header = people1_.header;
00051     for(unsigned int i=0; i<people1_.poses.size(); i++)
00052     {
00053       PoseWithCovarianceStampedArray_msg_.poses.push_back(people1_.poses[i]);      
00054     }
00055     people1ready=false;
00056     people1_.poses.clear();
00057     this->people1_mutex_.exit(); 
00058   }
00059   if(people2ready)
00060   {
00061     this->people2_mutex_.enter(); 
00062     if(PoseWithCovarianceStampedArray_msg_.poses.size()==0)
00063     {
00064       //fill header if it was empty (if no people1 received before), modifying frame_id
00065       PoseWithCovarianceStampedArray_msg_.header          = people2_.header;
00066       PoseWithCovarianceStampedArray_msg_.header.frame_id = target_frame_;
00067     }
00068 
00069     try
00070     {
00071       ros::Time target_time;
00072       target_time = people2_.header.stamp;
00073       // wait for a transformation between the source frame (from people2) and the target_frame (front_laser) to be available in the target time (people1.stamp)
00074       bool tf_exists = tf_listener_.waitForTransform(people2_.header.frame_id, target_frame_, target_time, ros::Duration(10), ros::Duration(0.01));
00075       if(tf_exists)
00076       {
00077         for(unsigned int i=0; i<people2_.poses.size(); i++)
00078         {
00079           geometry_msgs::PoseStamped poseIn;
00080           geometry_msgs::PoseStamped poseOut;
00081 
00082           //poseIn is the i pose of people2, with people2 header (frame, stamp)
00083           poseIn.header = people2_.header;
00084           poseIn.pose   = people2_.poses[i].pose;
00085           poseIn.pose.orientation.z = 1.0;
00086 
00087           //transforms poseIn from its frame (rear_laser) to the target frame (front_laser), generating poseOut
00088           tf_listener_.transformPose(target_frame_, target_time, poseIn, fixed_frame_, poseOut);
00089 
00090           people2_.poses[i].pose = poseOut.pose;
00091           people2_.poses[i].covariance = people2_.poses[i].covariance;
00092 
00093           PoseWithCovarianceStampedArray_msg_.poses.push_back(people2_.poses[i]);
00094         }
00095       }
00096       else
00097       {
00098         ROS_INFO("LaserPeopleDetectionFusionAlgNode::No transform: %s-->%s", fixed_frame_.c_str(), target_frame_.c_str());
00099       }
00100     }
00101     catch (tf::TransformException &ex)
00102     {
00103       ROS_INFO("LaserPeopleDetectionFusionAlgNode:: %s",ex.what());
00104     }
00105     people2ready=false;
00106     people2_.poses.clear();
00107     this->people2_mutex_.exit(); 
00108   }
00109 
00110   if(PoseWithCovarianceStampedArray_msg_.poses.size()!=0)
00111   {
00112     this->people_publisher_.publish(this->PoseWithCovarianceStampedArray_msg_);
00113     this->PoseWithCovarianceStampedArray_msg_.poses.clear();
00114   }
00115 
00116   //this->PoseWithCovarianceStampedArray_msg_.data = my_var;
00117   
00118   // [fill srv structure and make request to the server]
00119   
00120   // [fill action structure and make request to the action server]
00121 
00122   // [publish messages]
00123   //this->people_publisher_.publish(this->PoseWithCovarianceStampedArray_msg_);
00124 }
00125 
00126 /*  [subscriber callbacks] */
00127 
00128 void LaserPeopleDetectionFusionAlgNode::people1_callback(const iri_nav_msgs::PoseWithCovarianceStampedArray::ConstPtr& msg) 
00129 { 
00130   //ROS_INFO("LaserPeopleDetectionFusionAlgNode::people1_callback: New Message Received"); 
00131 
00132   //use appropiate mutex to shared variables if necessary 
00133   //this->alg_.lock(); 
00134   this->people1_mutex_.enter(); 
00135   if(msg->poses.size()!=0){
00136     people1_.header = msg->header;
00137     people1_.poses  = msg->poses;
00138     people1ready= true;
00139   }
00140   //std::cout << msg->data << std::endl; 
00141 
00142   //unlock previously blocked shared variables 
00143   //this->alg_.unlock(); 
00144   this->people1_mutex_.exit(); 
00145 }
00146 
00147 void LaserPeopleDetectionFusionAlgNode::people2_callback(const iri_nav_msgs::PoseWithCovarianceStampedArray::ConstPtr& msg) 
00148 { 
00149   //ROS_INFO("LaserPeopleDetectionFusionAlgNode::people2_callback: New Message Received"); 
00150 
00151   //use appropiate mutex to shared variables if necessary 
00152   //this->alg_.lock(); 
00153   this->people2_mutex_.enter(); 
00154   if(msg->poses.size()!=0){
00155     people2_.header = msg->header;
00156     people2_.poses  = msg->poses;
00157     people2ready = true;
00158   }
00159   //std::cout << msg->data << std::endl;
00160 
00161   //unlock previously blocked shared variables 
00162   //this->alg_.unlock(); 
00163   this->people2_mutex_.exit(); 
00164 }
00165 
00166 /*  [service callbacks] */
00167 
00168 /*  [action callbacks] */
00169 
00170 /*  [action requests] */
00171 
00172 void LaserPeopleDetectionFusionAlgNode::node_config_update(Config &config, uint32_t level)
00173 {
00174   this->alg_.lock();
00175 
00176   this->alg_.unlock();
00177 }
00178 
00179 void LaserPeopleDetectionFusionAlgNode::addNodeDiagnostics(void)
00180 {
00181 }
00182 
00183 /* main function */
00184 int main(int argc,char *argv[])
00185 {
00186   return algorithm_base::main<LaserPeopleDetectionFusionAlgNode>(argc, argv, "laser_people_detection_fusion_alg_node");
00187 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


iri_laser_people_detection_fusion
Author(s): fherrero
autogenerated on Sat Sep 21 2013 10:42:57