00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "people_tracking_filter/people_tracking_node.h"
00038 #include "people_tracking_filter/tracker_particle.h"
00039 #include "people_tracking_filter/tracker_kalman.h"
00040 #include "people_tracking_filter/state_pos_vel.h"
00041 #include "people_tracking_filter/rgb.h"
00042 #include <people_msgs/PositionMeasurement.h>
00043
00044
00045 using namespace std;
00046 using namespace tf;
00047 using namespace BFL;
00048 using namespace message_filters;
00049
00050 static const double sequencer_delay = 0.8;
00051 static const unsigned int sequencer_internal_buffer = 100;
00052 static const unsigned int sequencer_subscribe_buffer = 10;
00053 static const unsigned int num_particles_tracker = 1000;
00054 static const double tracker_init_dist = 4.0;
00055
00056 namespace estimation
00057 {
00058
00059 PeopleTrackingNode::PeopleTrackingNode(ros::NodeHandle nh)
00060 : nh_(nh),
00061 robot_state_(),
00062 tracker_counter_(0)
00063 {
00064
00065 meas_cloud_.points = vector<geometry_msgs::Point32>(1);
00066 meas_cloud_.points[0].x = 0;
00067 meas_cloud_.points[0].y = 0;
00068 meas_cloud_.points[0].z = 0;
00069
00070
00071 ros::NodeHandle local_nh("~");
00072 local_nh.param("fixed_frame", fixed_frame_, string("default"));
00073 local_nh.param("freq", freq_, 1.0);
00074 local_nh.param("start_distance_min", start_distance_min_, 0.0);
00075 local_nh.param("reliability_threshold", reliability_threshold_, 1.0);
00076 local_nh.param("sys_sigma_pos_x", sys_sigma_.pos_[0], 0.0);
00077 local_nh.param("sys_sigma_pos_y", sys_sigma_.pos_[1], 0.0);
00078 local_nh.param("sys_sigma_pos_z", sys_sigma_.pos_[2], 0.0);
00079 local_nh.param("sys_sigma_vel_x", sys_sigma_.vel_[0], 0.0);
00080 local_nh.param("sys_sigma_vel_y", sys_sigma_.vel_[1], 0.0);
00081 local_nh.param("sys_sigma_vel_z", sys_sigma_.vel_[2], 0.0);
00082 local_nh.param("follow_one_person", follow_one_person_, false);
00083
00084
00085 people_filter_pub_ = nh_.advertise<people_msgs::PositionMeasurement>("people_tracker_filter", 10);
00086
00087
00088 people_filter_vis_pub_ = nh_.advertise<sensor_msgs::PointCloud>("people_tracker_filter_visualization", 10);
00089 people_tracker_vis_pub_ = nh_.advertise<sensor_msgs::PointCloud>("people_tracker_measurements_visualization", 10);
00090
00091
00092 people_meas_sub_ = nh_.subscribe("people_tracker_measurements", 1, &PeopleTrackingNode::callbackRcv, this);
00093
00094 }
00095
00096
00097
00098 PeopleTrackingNode::~PeopleTrackingNode()
00099 {
00100
00101 delete message_sequencer_;
00102
00103
00104 for (list<Tracker*>::iterator it = trackers_.begin(); it != trackers_.end(); it++)
00105 delete *it;
00106 };
00107
00108
00109
00110
00111
00112 void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurement::ConstPtr& message)
00113 {
00114 ROS_DEBUG("Tracking node got a people position measurement (%f,%f,%f)",
00115 message->pos.x, message->pos.y, message->pos.z);
00116
00117 Stamped<tf::Vector3> meas_rel, meas;
00118 meas_rel.setData(
00119 tf::Vector3(message->pos.x, message->pos.y, message->pos.z));
00120 meas_rel.stamp_ = message->header.stamp;
00121 meas_rel.frame_id_ = message->header.frame_id;
00122 robot_state_.transformPoint(fixed_frame_, meas_rel, meas);
00123
00124
00125 SymmetricMatrix cov(3);
00126 for (unsigned int i = 0; i < 3; i++)
00127 for (unsigned int j = 0; j < 3; j++)
00128 cov(i + 1, j + 1) = message->covariance[3 * i + j];
00129
00130
00131 boost::mutex::scoped_lock lock(filter_mutex_);
00132
00133
00134 for (list<Tracker*>::iterator it = trackers_.begin(); it != trackers_.end(); it++)
00135 if ((*it)->getName() == message->object_id)
00136 {
00137 (*it)->updatePrediction(message->header.stamp.toSec());
00138 (*it)->updateCorrection(meas, cov);
00139 }
00140
00141 if (message->object_id == "" && message->reliability > reliability_threshold_)
00142 {
00143 double closest_tracker_dist = start_distance_min_;
00144 StatePosVel est;
00145 for (list<Tracker*>::iterator it = trackers_.begin(); it != trackers_.end(); it++)
00146 {
00147 (*it)->getEstimate(est);
00148 double dst = sqrt(pow(est.pos_[0] - meas[0], 2) + pow(est.pos_[1] - meas[1], 2));
00149 if (dst < closest_tracker_dist)
00150 closest_tracker_dist = dst;
00151 }
00152
00153 if (follow_one_person_)
00154 cout << "Following one person" << endl;
00155 if (message->initialization == 1 && ((!follow_one_person_ && (closest_tracker_dist >= start_distance_min_)) || (follow_one_person_ && trackers_.empty())))
00156 {
00157
00158
00159 ROS_INFO("Passed crazy conditional.");
00160 tf::Point pt;
00161 tf::pointMsgToTF(message->pos, pt);
00162 tf::Stamped<tf::Point> loc(pt, message->header.stamp, message->header.frame_id);
00163 robot_state_.transformPoint("base_link", loc, loc);
00164 float cur_dist;
00165 if ((cur_dist = pow(loc[0], 2.0) + pow(loc[1], 2.0)) < tracker_init_dist)
00166 {
00167
00168 cout << "starting new tracker" << endl;
00169 stringstream tracker_name;
00170 StatePosVel prior_sigma(tf::Vector3(sqrt(cov(1, 1)), sqrt(cov(
00171 2, 2)), sqrt(cov(3, 3))), tf::Vector3(0.0000001, 0.0000001, 0.0000001));
00172 tracker_name << "person " << tracker_counter_++;
00173 Tracker* new_tracker = new TrackerKalman(tracker_name.str(),
00174 sys_sigma_);
00175
00176 new_tracker->initialize(meas, prior_sigma,
00177 message->header.stamp.toSec());
00178 trackers_.push_back(new_tracker);
00179 ROS_INFO("Initialized new tracker %s", tracker_name.str().c_str());
00180 }
00181 else
00182 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);
00183 }
00184 else
00185 ROS_INFO("Failed crazy conditional.");
00186 }
00187 lock.unlock();
00188
00189
00190
00191
00192 meas_cloud_.points[0].x = meas[0];
00193 meas_cloud_.points[0].y = meas[1];
00194 meas_cloud_.points[0].z = meas[2];
00195 meas_cloud_.header.frame_id = meas.frame_id_;
00196 people_tracker_vis_pub_.publish(meas_cloud_);
00197 }
00198
00199
00200
00201
00202 void PeopleTrackingNode::callbackDrop(const people_msgs::PositionMeasurement::ConstPtr& message)
00203 {
00204 ROS_INFO("DROPPED PACKAGE for %s from %s with delay %f !!!!!!!!!!!",
00205 message->object_id.c_str(), message->name.c_str(), (ros::Time::now() - message->header.stamp).toSec());
00206
00207 }
00208
00209
00210
00211
00212
00213 void PeopleTrackingNode::spin()
00214 {
00215 ROS_INFO("People tracking manager started.");
00216
00217 while (ros::ok())
00218 {
00219
00220 boost::mutex::scoped_lock lock(filter_mutex_);
00221
00222
00223 vector<geometry_msgs::Point32> filter_visualize(trackers_.size());
00224 vector<float> weights(trackers_.size());
00225 sensor_msgs::ChannelFloat32 channel;
00226
00227
00228 unsigned int i = 0;
00229 list<Tracker*>::iterator it = trackers_.begin();
00230 while (it != trackers_.end())
00231 {
00232
00233 (*it)->updatePrediction(ros::Time::now().toSec() - sequencer_delay);
00234
00235
00236 people_msgs::PositionMeasurement est_pos;
00237 (*it)->getEstimate(est_pos);
00238 est_pos.header.frame_id = fixed_frame_;
00239
00240 ROS_DEBUG("Publishing people tracker filter.");
00241 people_filter_pub_.publish(est_pos);
00242
00243
00244 filter_visualize[i].x = est_pos.pos.x;
00245 filter_visualize[i].y = est_pos.pos.y;
00246 filter_visualize[i].z = est_pos.pos.z;
00247 weights[i] = *(float*) & (rgb[min(998, 999 - max(1, (int)trunc((*it)->getQuality() * 999.0)))]);
00248
00249
00250 ROS_INFO("Quality of tracker %s = %f", (*it)->getName().c_str(), (*it)->getQuality());
00251 if ((*it)->getQuality() <= 0)
00252 {
00253 ROS_INFO("Removing tracker %s", (*it)->getName().c_str());
00254 delete *it;
00255 trackers_.erase(it++);
00256 }
00257 else it++;
00258 i++;
00259 }
00260 lock.unlock();
00261
00262
00263
00264
00265 channel.name = "rgb";
00266 channel.values = weights;
00267 sensor_msgs::PointCloud people_cloud;
00268 people_cloud.channels.push_back(channel);
00269 people_cloud.header.frame_id = fixed_frame_;
00270 people_cloud.points = filter_visualize;
00271 people_filter_vis_pub_.publish(people_cloud);
00272
00273
00274 usleep(1e6 / freq_);
00275
00276 ros::spinOnce();
00277 }
00278 };
00279
00280
00281 };
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291 using namespace estimation;
00292 int main(int argc, char **argv)
00293 {
00294
00295 ros::init(argc, argv, "people_tracker");
00296 ros::NodeHandle(nh);
00297
00298
00299 PeopleTrackingNode my_tracking_node(nh);
00300
00301
00302 my_tracking_node.spin();
00303
00304
00305
00306 return 0;
00307 }