$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Wim Meeussen */ 00036 /* Modified by Alex Noyvirt for SRS */ 00037 00038 #include "srs_people_tracking_filter/people_tracking_node.h" 00039 #include "srs_people_tracking_filter/tracker_particle.h" 00040 #include "srs_people_tracking_filter/tracker_kalman.h" 00041 #include "srs_people_tracking_filter/state_pos_vel.h" 00042 #include "srs_people_tracking_filter/rgb.h" 00043 #include <srs_msgs/PositionMeasurement.h> 00044 00045 00046 using namespace std; 00047 using namespace tf; 00048 using namespace BFL; 00049 using namespace message_filters; 00050 00051 static const double sequencer_delay = 0.8; //TODO: this is probably too big, it was 0.8 00052 static const unsigned int sequencer_internal_buffer = 100; 00053 static const unsigned int sequencer_subscribe_buffer = 10; 00054 static const unsigned int num_particles_tracker = 1000; 00055 static const double tracker_init_dist = 4.0; 00056 00057 namespace estimation 00058 { 00059 // constructor 00060 PeopleTrackingNode::PeopleTrackingNode(ros::NodeHandle nh) 00061 : nh_(nh), 00062 robot_state_(), 00063 tracker_counter_(0) 00064 { 00065 // initialize 00066 meas_cloud_.points = vector<geometry_msgs::Point32>(1); 00067 meas_cloud_.points[0].x = 0; 00068 meas_cloud_.points[0].y = 0; 00069 meas_cloud_.points[0].z = 0; 00070 00071 // get parameters 00072 ros::NodeHandle local_nh("~"); 00073 local_nh.param("fixed_frame", fixed_frame_, string("default")); 00074 local_nh.param("freq", freq_, 1.0); 00075 local_nh.param("start_distance_min", start_distance_min_, 0.0); 00076 local_nh.param("reliability_threshold", reliability_threshold_, 1.0); 00077 local_nh.param("sys_sigma_pos_x", sys_sigma_.pos_[0], 0.0); 00078 local_nh.param("sys_sigma_pos_y", sys_sigma_.pos_[1], 0.0); 00079 local_nh.param("sys_sigma_pos_z", sys_sigma_.pos_[2], 0.0); 00080 local_nh.param("sys_sigma_vel_x", sys_sigma_.vel_[0], 0.0); 00081 local_nh.param("sys_sigma_vel_y", sys_sigma_.vel_[1], 0.0); 00082 local_nh.param("sys_sigma_vel_z", sys_sigma_.vel_[2], 0.0); 00083 local_nh.param("follow_one_person", follow_one_person_, false); 00084 00085 // advertise filter output 00086 people_filter_pub_ = nh_.advertise<srs_msgs::PositionMeasurement>("people_tracker_filter",10); 00087 00088 // advertise visualization 00089 people_filter_vis_pub_ = nh_.advertise<sensor_msgs::PointCloud>("people_tracker_filter_visualization",10); 00090 people_tracker_vis_pub_ = nh_.advertise<sensor_msgs::PointCloud>("people_tracker_measurements_visualization",10); 00091 00092 // register message sequencer 00093 people_meas_sub_ = nh_.subscribe("people_tracker_measurements", 1, &PeopleTrackingNode::callbackRcv, this); 00094 00095 } 00096 00097 00098 // destructor 00099 PeopleTrackingNode::~PeopleTrackingNode() 00100 { 00101 // delete sequencer 00102 delete message_sequencer_; 00103 00104 // delete all trackers 00105 for (list<Tracker*>::iterator it= trackers_.begin(); it!=trackers_.end(); it++) 00106 delete *it; 00107 }; 00108 00109 00110 00111 00112 // callback for messages 00113 void PeopleTrackingNode::callbackRcv(const srs_msgs::PositionMeasurement::ConstPtr& message) 00114 { 00115 ROS_DEBUG("Tracking node got a people position measurement (%f,%f,%f)", 00116 message->pos.x, message->pos.y, message->pos.z); 00117 // get measurement in fixed frame 00118 Stamped<tf::Vector3> meas_rel, meas; 00119 meas_rel.setData( 00120 tf::Vector3(message->pos.x, message->pos.y, message->pos.z)); 00121 meas_rel.stamp_ = message->header.stamp; 00122 meas_rel.frame_id_ = message->header.frame_id; 00123 robot_state_.transformPoint(fixed_frame_, meas_rel, meas); 00124 00125 // get measurement covariance 00126 SymmetricMatrix cov(3); 00127 for (unsigned int i = 0; i < 3; i++) 00128 for (unsigned int j = 0; j < 3; j++) 00129 cov(i + 1, j + 1) = message->covariance[3 * i + j]; 00130 00131 // ----- LOCKED ------ 00132 boost::mutex::scoped_lock lock(filter_mutex_); 00133 00134 // update tracker if matching tracker found 00135 for (list<Tracker*>::iterator it = trackers_.begin(); it != trackers_.end(); it++) 00136 if ((*it)->getName() == message->object_id) { 00137 (*it)->updatePrediction(message->header.stamp.toSec()); 00138 (*it)->updateCorrection(meas, cov); 00139 } 00140 // check if reliable message with no name should be a new tracker 00141 if (message->object_id == "" && message->reliability > reliability_threshold_) { 00142 double closest_tracker_dist = start_distance_min_; 00143 StatePosVel est; 00144 for (list<Tracker*>::iterator it = trackers_.begin(); it != trackers_.end(); it++) { 00145 (*it)->getEstimate(est); 00146 double dst = sqrt(pow(est.pos_[0] - meas[0], 2) + pow(est.pos_[1] - meas[1], 2)); 00147 if (dst < closest_tracker_dist) 00148 closest_tracker_dist = dst; 00149 } 00150 // initialize a new tracker 00151 if (follow_one_person_) 00152 cout << "Following one person" << endl; 00153 if (message->initialization == 1 && ((!follow_one_person_ && (closest_tracker_dist >= start_distance_min_)) || (follow_one_person_ && trackers_.empty()))) { 00154 //if (closest_tracker_dist >= start_distance_min_ || message->initialization == 1){ 00155 //if (message->initialization == 1 && trackers_.empty()){ 00156 ROS_INFO("Passed crazy conditional."); 00157 tf::Point pt; 00158 tf::pointMsgToTF(message->pos, pt); 00159 tf::Stamped<tf::Point> loc(pt, message->header.stamp, message->header.frame_id); 00160 robot_state_.transformPoint("base_link", loc, loc); 00161 float cur_dist; 00162 if ((cur_dist = pow(loc[0], 2.0) + pow(loc[1], 2.0)) < tracker_init_dist) { 00163 00164 cout << "starting new tracker" << endl; 00165 stringstream tracker_name; 00166 StatePosVel prior_sigma(tf::Vector3(sqrt(cov(1, 1)), sqrt(cov( 00167 2, 2)), sqrt(cov(3, 3))), tf::Vector3(0.0000001, 0.0000001, 0.0000001)); 00168 tracker_name << "person " << tracker_counter_++; 00169 Tracker* new_tracker = new TrackerKalman(tracker_name.str(), 00170 sys_sigma_); 00171 //Tracker* new_tracker = new TrackerParticle(tracker_name.str(), num_particles_tracker, sys_sigma_); 00172 new_tracker->initialize(meas, prior_sigma, 00173 message->header.stamp.toSec()); 00174 trackers_.push_back(new_tracker); 00175 ROS_INFO("Initialized new tracker %s", tracker_name.str().c_str()); 00176 } 00177 else 00178 ROS_INFO("Found a person, but he/she is not close enough to start following. Person is %f away, and must be less than %f away.", cur_dist , tracker_init_dist); 00179 } 00180 else 00181 ROS_INFO("Failed crazy conditional."); 00182 } 00183 lock.unlock(); 00184 // ------ LOCKED ------ 00185 00186 00187 // visualize measurement 00188 meas_cloud_.points[0].x = meas[0]; 00189 meas_cloud_.points[0].y = meas[1]; 00190 meas_cloud_.points[0].z = meas[2]; 00191 meas_cloud_.header.frame_id = meas.frame_id_; 00192 people_tracker_vis_pub_.publish(meas_cloud_); 00193 } 00194 00195 00196 00197 // callback for dropped messages 00198 void PeopleTrackingNode::callbackDrop(const srs_msgs::PositionMeasurement::ConstPtr& message) 00199 { 00200 ROS_INFO("DROPPED PACKAGE for %s from %s with delay %f !!!!!!!!!!!", 00201 message->object_id.c_str(), message->name.c_str(), (ros::Time::now() - message->header.stamp).toSec()); 00202 00203 } 00204 00205 00206 00207 00208 // filter loop 00209 void PeopleTrackingNode::spin() 00210 { 00211 ROS_INFO("People tracking manager started."); 00212 00213 while (ros::ok()){ 00214 // ------ LOCKED ------ 00215 boost::mutex::scoped_lock lock(filter_mutex_); 00216 00217 // visualization variables 00218 vector<geometry_msgs::Point32> filter_visualize(trackers_.size()); 00219 vector<float> weights(trackers_.size()); 00220 sensor_msgs::ChannelFloat32 channel; 00221 00222 // loop over trackers 00223 unsigned int i=0; 00224 list<Tracker*>::iterator it= trackers_.begin(); 00225 while (it!=trackers_.end()){ 00226 // update prediction up to delayed time 00227 (*it)->updatePrediction(ros::Time::now().toSec() - sequencer_delay); 00228 00229 // publish filter result 00230 srs_msgs::PositionMeasurement est_pos; 00231 (*it)->getEstimate(est_pos); 00232 est_pos.header.frame_id = fixed_frame_; 00233 00234 ROS_DEBUG("Publishing people tracker filter."); 00235 people_filter_pub_.publish(est_pos); 00236 00237 // visualize filter result 00238 filter_visualize[i].x = est_pos.pos.x; 00239 filter_visualize[i].y = est_pos.pos.y; 00240 filter_visualize[i].z = est_pos.pos.z; 00241 weights[i] = *(float*)&(rgb[min(998, 999-max(1, (int)trunc( (*it)->getQuality()*999.0 )))]); 00242 00243 // remove trackers that have zero quality 00244 ROS_INFO("Quality of tracker %s = %f",(*it)->getName().c_str(), (*it)->getQuality()); 00245 if ((*it)->getQuality() <= 0){ 00246 ROS_INFO("Removing tracker %s",(*it)->getName().c_str()); 00247 delete *it; 00248 trackers_.erase(it++); 00249 } 00250 else it++; 00251 i++; 00252 } 00253 lock.unlock(); 00254 // ------ LOCKED ------ 00255 00256 00257 // visualize all trackers 00258 channel.name = "rgb"; 00259 channel.values = weights; 00260 sensor_msgs::PointCloud people_cloud; 00261 people_cloud.channels.push_back(channel); 00262 people_cloud.header.frame_id = fixed_frame_; 00263 people_cloud.points = filter_visualize; 00264 people_filter_vis_pub_.publish(people_cloud); 00265 00266 // sleep 00267 usleep(1e6/freq_); 00268 00269 ros::spinOnce(); 00270 } 00271 }; 00272 00273 00274 }; // namespace 00275 00276 00277 00278 00279 00280 00281 // ---------- 00282 // -- MAIN -- 00283 // ---------- 00284 using namespace estimation; 00285 int main(int argc, char **argv) 00286 { 00287 // Initialize ROS 00288 ros::init(argc, argv,"people_tracker"); 00289 ros::NodeHandle(nh); 00290 00291 // create tracker node 00292 PeopleTrackingNode my_tracking_node(nh); 00293 00294 // wait for filter to finish 00295 my_tracking_node.spin(); 00296 00297 // Clean up 00298 00299 return 0; 00300 }