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
00007
00008
00009
00010 this->scan_filtered_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan_filtered", 100);
00011
00012
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
00017
00018
00019
00020
00021
00022
00023
00024 }
00025
00026 DynamicsObjectsLocalizationAlgNode::~DynamicsObjectsLocalizationAlgNode(void)
00027 {
00028
00029 }
00030
00031 void DynamicsObjectsLocalizationAlgNode::mainNodeThread(void)
00032 {
00033
00034
00035 deleteSegments();
00036
00037
00038
00039
00040
00041
00042
00043
00044 this->scan_filtered_publisher_.publish(this->LaserScan_msg_);
00045 }
00046
00047
00048 void DynamicsObjectsLocalizationAlgNode::people_callback(const iri_nav_msgs::PoseWithCovarianceStampedArray::ConstPtr& msg)
00049 {
00050
00051
00052
00053 this->alg_.lock();
00054 this->people_mutex_.enter();
00055
00056 this->people_ = *msg;
00057
00058
00059 this->alg_.unlock();
00060 this->people_mutex_.exit();
00061
00062 }
00063 void DynamicsObjectsLocalizationAlgNode::scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
00064 {
00065
00066
00067
00068 this->alg_.lock();
00069 this->scan_mutex_.enter();
00070
00071 this->laser_ = *msg;
00072
00073
00074
00075
00076 this->alg_.unlock();
00077 this->scan_mutex_.exit();
00078
00079 }
00080
00081
00082
00083
00084
00085
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
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
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 }