Go to the documentation of this file.00001 #include "laser_people_area_filter_alg_node.h"
00002
00003 LaserPeopleAreaFilterAlgNode::LaserPeopleAreaFilterAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<LaserPeopleAreaFilterAlgorithm>(),
00005 tf_listener_(ros::Duration(10.f))
00006 {
00007
00008
00009
00010 this->markerWidth=0.5;
00011 this->markerHeight=0.2;
00012 this->markerR=1.0;
00013 this->markerG=1.0;
00014 this->markerB=0.0;
00015 this->markerA=0.75;
00016 this->frame="/map";
00017 this->xmin=0;
00018 this->xmax=10;
00019 this->ymin=-5;
00020 this->ymax=5;
00021
00022
00023 this->peopleMarkers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("peopleMarkers", 100);
00024 this->peopleFiltered_publisher_ = this->public_node_handle_.advertise<iri_perception_msgs::detectionArray>("peopleFiltered", 100);
00025
00026
00027 this->people_subscriber_ = this->public_node_handle_.subscribe("people", 100, &LaserPeopleAreaFilterAlgNode::people_callback, this);
00028
00029
00030
00031
00032
00033
00034
00035
00036 }
00037
00038 LaserPeopleAreaFilterAlgNode::~LaserPeopleAreaFilterAlgNode(void)
00039 {
00040
00041 }
00042
00043 void LaserPeopleAreaFilterAlgNode::mainNodeThread(void)
00044 {
00045 alg_.lock();
00046 if(this->peopleReceived)
00047 {
00048 iri_perception_msgs::detectionArray empty;
00049 this->detectionArray_msg_ = empty;
00050 for(unsigned int i=0; i<this->people.detection.size() ; i++)
00051 {
00052 geometry_msgs::PoseStamped pose;
00053 pose.header=this->people.header;
00054 pose.pose = this->people.detection[i].pose.pose;
00055 ROS_DEBUG("LaserPeopleAreaFilterAlgNode::mainNodeThread: validating detection x,y,frame: %f, %f, %s",pose.pose.position.x, pose.pose.position.y, pose.header.frame_id.c_str());
00056 if(!this->validateByArea(pose))
00057 {
00058 ROS_DEBUG("LaserPeopleAreaFilterAlgNode::mainNodeThread: discarded detection x,y,frame: %f, %f, %s",pose.pose.position.x, pose.pose.position.y, pose.header.frame_id.c_str());
00059 continue;
00060 }
00061 this->detectionArray_msg_.detection.push_back(this->people.detection[i]);
00062 this->detectionArray_msg_.header= this->people.header;
00063 }
00064 this->peopleReceived=false;
00065
00066
00067 if(this->detectionArray_msg_.detection.size()!=0)
00068 {
00069 this->peopleFiltered_publisher_.publish(this->detectionArray_msg_);
00070 this->getPeopleMarkers(detectionArray_msg_, this->MarkerArray_msg_);
00071 this->peopleMarkers_publisher_.publish(this->MarkerArray_msg_);
00072 }
00073 }
00074 alg_.unlock();
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087 }
00088
00089
00090 void LaserPeopleAreaFilterAlgNode::people_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg)
00091 {
00092
00093
00094
00095 this->alg_.lock();
00096
00097
00098
00099 this->people.header = msg->header;
00100 this->people.detection = msg->detection;
00101 this->peopleReceived = true;
00102
00103
00104 this->alg_.unlock();
00105
00106 }
00107
00108
00109
00110
00111
00112
00113
00114 void LaserPeopleAreaFilterAlgNode::node_config_update(Config &config, uint32_t level)
00115 {
00116 this->alg_.lock();
00117 this->xmax = config.xmax;
00118 this->xmin = config.xmin;
00119 this->ymax = config.ymax;
00120 this->ymin = config.ymin;
00121
00122 this->markerWidth = config.markerWidth;
00123 this->markerHeight = config.markerHeight;
00124 this->markerR = config.markerR;
00125 this->markerG = config.markerG;
00126 this->markerB = config.markerB;
00127 this->markerA = config.markerA;
00128 this->frame = config.frame;
00129 this->showArea = config.showArea;
00130
00131 this->alg_.unlock();
00132 }
00133
00134 void LaserPeopleAreaFilterAlgNode::addNodeDiagnostics(void)
00135 {
00136 }
00137
00138
00139 int main(int argc,char *argv[])
00140 {
00141 return algorithm_base::main<LaserPeopleAreaFilterAlgNode>(argc, argv, "laser_people_area_filter_alg_node");
00142 }
00143
00144 bool LaserPeopleAreaFilterAlgNode::validateByArea(geometry_msgs::PoseStamped pose)
00145 {
00146
00147 if(pose.header.frame_id != this->frame)
00148 {
00149 try
00150 {
00151 std::string source_frame = pose.header.frame_id;
00152 ros::Time target_time = pose.header.stamp;
00153
00154
00155
00156
00157
00158 bool tf_exists = tf_listener_.waitForTransform( this->frame, source_frame, target_time, ros::Duration(5), ros::Duration(0.01));
00159
00160 if(tf_exists)
00161 {
00162 geometry_msgs::PoseStamped poseOut;
00163
00164 tf_listener_.transformPose( this->frame, pose, poseOut);
00165 pose = poseOut;
00166 }
00167 else
00168 {
00169 ROS_INFO("LaserPeopleAreaFilterAlgNode::No transform: %s-->%s", source_frame.c_str(), this->frame.c_str());
00170 }
00171 }
00172 catch (tf::TransformException &ex)
00173 {
00174 ROS_INFO("LaserPeopleAreaFilterAlgNode:: %s",ex.what());
00175 }
00176 }
00177 bool valid;
00178 if(pose.header.frame_id == this->frame)
00179 {
00180 valid = pose.pose.position.x>this->xmin && pose.pose.position.x<this->xmax && pose.pose.position.y>this->ymin && pose.pose.position.y<this->ymax;
00181 }
00182 else
00183 {
00184 ROS_INFO("LaserPeopleAreaFilterAlgNode:: Unable to validate people position with area, different frames: %s, %s", this->frame.c_str(), pose.header.frame_id.c_str());
00185 valid = false;
00186 }
00187
00188 return valid;
00189
00190 }
00191
00192 void LaserPeopleAreaFilterAlgNode::getPeopleMarkers(iri_perception_msgs::detectionArray & peopleList, visualization_msgs::MarkerArray & people)
00193 {
00194 static unsigned int lastSize=0;
00195 people.markers.resize(std::max(uint(peopleList.detection.size()), lastSize));
00196 lastSize = peopleList.detection.size();
00197
00198 for(unsigned int i=0; i<peopleList.detection.size(); i++)
00199 {
00200 people.markers[i].scale.x = this->markerWidth;
00201 people.markers[i].scale.y = this->markerWidth;
00202 people.markers[i].scale.z = this->markerHeight + peopleList.detection[i].probability;
00203
00204 people.markers[i].pose.position.x = peopleList.detection[i].pose.pose.position.x;
00205 people.markers[i].pose.position.y = peopleList.detection[i].pose.pose.position.y;
00206 people.markers[i].pose.position.z = people.markers[i].scale.z/2.0;
00207
00208 people.markers[i].header.frame_id = peopleList.header.frame_id;
00209 people.markers[i].header.stamp = peopleList.header.stamp;
00210
00211 people.markers[i].id = i+1;
00212 people.markers[i].type = visualization_msgs::Marker::CYLINDER;
00213 people.markers[i].action = visualization_msgs::Marker::ADD;
00214
00215 people.markers[i].lifetime = ros::Duration(0.5f);
00216
00217 people.markers[i].pose.orientation.x = 0.0;
00218 people.markers[i].pose.orientation.y = 0.0;
00219 people.markers[i].pose.orientation.z = 0.0;
00220 people.markers[i].pose.orientation.w = 1.0;
00221
00222 people.markers[i].color.r = this->markerR;
00223 people.markers[i].color.g = this->markerG;
00224 people.markers[i].color.b = this->markerB;
00225 people.markers[i].color.a = this->markerA;
00226
00227 }
00228
00229
00230 for(unsigned int i=peopleList.detection.size(); i<people.markers.size(); i++)
00231 {
00232 people.markers[i].action = visualization_msgs::Marker::DELETE;
00233 }
00234
00235 if(this->showArea)
00236 {
00237 visualization_msgs::Marker area;
00238
00239 area.scale.x = this->xmax-this->xmin;
00240 area.scale.y = this->ymax-this->ymin;
00241 area.scale.z = 0.1;
00242
00243 area.pose.position.x = area.scale.x/2.0 + xmin;
00244 area.pose.position.y = area.scale.y/2.0 + ymin;
00245 area.pose.position.z = area.scale.z/2.0;
00246
00247 area.header.frame_id = this->frame;
00248 area.header.stamp = peopleList.header.stamp;
00249
00250 area.id = 0;
00251 area.type = visualization_msgs::Marker::CUBE;
00252 area.action = visualization_msgs::Marker::ADD;
00253
00254 area.lifetime = ros::Duration(0.5f);
00255
00256 area.pose.orientation.x = 0.0;
00257 area.pose.orientation.y = 0.0;
00258 area.pose.orientation.z = 0.0;
00259 area.pose.orientation.w = 1.0;
00260
00261 area.color.r = this->markerR;
00262 area.color.g = this->markerG;
00263 area.color.b = this->markerB;
00264 area.color.a = this->markerA/4.0;
00265
00266 people.markers.push_back(area);
00267
00268 }
00269
00270
00271
00272
00273 }