dynamics_objects_localization_alg_node.cpp
Go to the documentation of this file.
00001 #include "dynamics_objects_localization_alg_node.h"
00002 
00003 DynamicsObjectsLocalizationAlgNode::DynamicsObjectsLocalizationAlgNode(void) :
00004   algorithm_base::IriBaseAlgorithm<DynamicsObjectsLocalizationAlgorithm>()
00005 {
00006   //init class attributes if necessary
00007   //this->loop_rate_ = 2;//in [Hz]
00008 
00009   // [init publishers]
00010   this->scan_filtered_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan_filtered", 100);
00011   
00012   // [init subscribers]
00013   this->people_subscriber_ = this->public_node_handle_.subscribe("people", 100, &DynamicsObjectsLocalizationAlgNode::people_callback, this);
00014   this->scan_subscriber_ = this->public_node_handle_.subscribe("scan", 100, &DynamicsObjectsLocalizationAlgNode::scan_callback, this);
00015   
00016   // [init services]
00017   
00018   // [init clients]
00019   
00020   // [init action servers]
00021   
00022   // [init action clients]
00023 
00024 }
00025 
00026 DynamicsObjectsLocalizationAlgNode::~DynamicsObjectsLocalizationAlgNode(void)
00027 {
00028   // [free dynamic memory]
00029 }
00030 
00031 void DynamicsObjectsLocalizationAlgNode::mainNodeThread(void)
00032 {
00033   // [fill msg structures]
00034         // this function delete people segments in scan laser information
00035         deleteSegments();
00036   
00037   // [fill srv structure and make request to the server]
00038   
00039   // [fill action structure and make request to the action server]
00040 
00041         
00042 
00043   // [publish messages]
00044   this->scan_filtered_publisher_.publish(this->LaserScan_msg_);
00045 }
00046 
00047 /*  [subscriber callbacks] */
00048 void DynamicsObjectsLocalizationAlgNode::people_callback(const iri_nav_msgs::PoseWithCovarianceStampedArray::ConstPtr& msg) 
00049 { 
00050   //ROS_INFO("DynamicsObjectsLocalizationAlgNode::people_callback: New Message Received"); 
00051 
00052   //use appropiate mutex to shared variables if necessary 
00053   this->alg_.lock(); 
00054   this->people_mutex_.enter(); 
00055 
00056         this->people_ = *msg;
00057 
00058   //unlock previously blocked shared variables 
00059   this->alg_.unlock(); 
00060   this->people_mutex_.exit();
00061 
00062 }
00063 void DynamicsObjectsLocalizationAlgNode::scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
00064 { 
00065   //ROS_INFO("DynamicsObjectsLocalizationAlgNode::scan_callback: New Message Received"); 
00066 
00067   //use appropiate mutex to shared variables if necessary 
00068   this->alg_.lock(); 
00069   this->scan_mutex_.enter();
00070 
00071         this->laser_ = *msg;
00072 
00073   //std::cout << msg->data << std::endl; 
00074 
00075   //unlock previously blocked shared variables 
00076   this->alg_.unlock(); 
00077   this->scan_mutex_.exit(); 
00078 
00079 }
00080 
00081 /*  [service callbacks] */
00082 
00083 /*  [action callbacks] */
00084 
00085 /*  [action requests] */
00086 
00087 void DynamicsObjectsLocalizationAlgNode::node_config_update(Config &config, uint32_t level)
00088 {
00089   this->alg_.lock();
00090 
00091   this->alg_.unlock();
00092 }
00093 
00094 void DynamicsObjectsLocalizationAlgNode::addNodeDiagnostics(void)
00095 {
00096 }
00097 
00098 /* main function */
00099 int main(int argc,char *argv[])
00100 {
00101   return algorithm_base::main<DynamicsObjectsLocalizationAlgNode>(argc, argv, "dynamics_objects_localization_alg_node");
00102 }
00103 
00104 /*
00105         Looks for the center of the people and delete points in a square around them.
00106 */
00107 
00108 void DynamicsObjectsLocalizationAlgNode::deleteSegments(void)
00109         {
00110                 unsigned int peopleSize = people_.poses.size();
00111                 unsigned int laserSize = laser_.ranges.size();
00112                 
00113                 unsigned int U = 1.0;
00114 
00115                 for (unsigned int i = 0; i < laserSize; i++)
00116                 {
00117                         float angle = laser_.angle_min + (laser_.angle_increment * i);
00118 
00119                         float x_laser = laser_.ranges[i] * cos(angle);
00120                         float y_laser = laser_.ranges[i] * sin(angle);
00121 
00122                         for(unsigned int j = 0; j < peopleSize; j++)
00123                         {
00124                                 float x_people = people_.poses[j].pose.position.x;
00125                                 float y_people = people_.poses[j].pose.position.y;
00126 
00127                                 if ((x_laser >= x_people - U) && (x_laser <= x_people + U) && 
00128                                         (y_laser >= y_people - U) && (y_laser <= y_people + U))
00129                                 {
00130                                         laser_.ranges[i] = 0.0;
00131                                 }
00132                         }
00133     }
00134                 this->LaserScan_msg_ = laser_;
00135         }


iri_dynamics_objects_localization
Author(s): Ibraim Hernandez
autogenerated on Fri Dec 6 2013 22:24:47