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
00008 this->loop_rate_ = 60;
00009
00010 this->mapReceived=false;
00011 this->peopleReceived=false;
00012 this->neighborRadius = 0.25;
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
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
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
00034
00035
00036
00037
00038
00039
00040 }
00041
00042 LaserPeopleMapFilterAlgNode::~LaserPeopleMapFilterAlgNode(void)
00043 {
00044
00045 }
00046
00047 void LaserPeopleMapFilterAlgNode::mainNodeThread(void)
00048 {
00049 if(this->mapReceived && this->peopleReceived)
00050 {
00051
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
00061 float x0 = this->map.info.origin.position.x;
00062 float y0 = this->map.info.origin.position.y;
00063
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
00071
00072 this->detectionArray_msg_.detection.push_back(this->peopleIn.detection[i]);
00073 this->detectionArray_msg_.header= this->peopleIn.header;
00074 }
00075 else
00076 {
00077
00078 }
00079 }
00080 this->peopleReceived=false;
00081
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
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101 }
00102
00103
00104 void LaserPeopleMapFilterAlgNode::map_callback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
00105 {
00106 ROS_DEBUG("LaserPeopleMapFilterAlgNode::map_callback: New Message Received");
00107
00108
00109 this->alg_.lock();
00110
00111
00112
00113 if(!mapReceived){
00114
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
00123 this->alg_.unlock();
00124
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
00132 this->alg_.lock();
00133
00134
00135
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
00145
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
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
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
00187 this->alg_.unlock();
00188
00189 }
00190
00191
00192
00193
00194
00195
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
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
00223 bool valid = true;
00224 int W = this->map.info.width;
00225 int N = neighborDistance;
00226
00227
00228
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
00235 if(indexi >0 && indexi < signed(map.data.size()))
00236 {
00237
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
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 }