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 "cob_people_tracking_filter/people_tracking_node.h"
00038 #include "cob_people_tracking_filter/tracker_particle.h"
00039 #include "cob_people_tracking_filter/tracker_kalman.h"
00040 #include "cob_people_tracking_filter/state_pos_vel.h"
00041 #include "cob_people_tracking_filter/rgb.h"
00042 #include <cob_perception_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<cob_perception_msgs::PositionMeasurement>("people_tracker_filter",10);
00086
00087 people_filter_vis_pub_ = nh_.advertise<sensor_msgs::PointCloud>("people_tracker_filter_visualization",10);
00088 people_tracker_vis_pub_ = nh_.advertise<sensor_msgs::PointCloud>("people_tracker_measurements_visualization",10);
00089
00090
00091 people_meas_sub_ = nh_.subscribe("people_tracker_measurements", 1, &PeopleTrackingNode::callbackRcv, this);
00092
00093 }
00094
00095
00096
00097 PeopleTrackingNode::~PeopleTrackingNode()
00098 {
00099
00100 delete message_sequencer_;
00101
00102
00103 for (list<Tracker*>::iterator it= trackers_.begin(); it!=trackers_.end(); it++)
00104 delete *it;
00105 };
00106
00107
00108
00109
00110
00111 void PeopleTrackingNode::callbackRcv(const cob_perception_msgs::PositionMeasurement::ConstPtr& message)
00112 {
00113 ROS_DEBUG("Tracking node got a people position measurement (%f,%f,%f)",
00114 message->pos.x, message->pos.y, message->pos.z);
00115
00116 Stamped<tf::Vector3> meas_rel, meas;
00117 meas_rel.setData(
00118 tf::Vector3(message->pos.x, message->pos.y, message->pos.z));
00119 meas_rel.stamp_ = message->header.stamp;
00120 meas_rel.frame_id_ = message->header.frame_id;
00121 robot_state_.transformPoint(fixed_frame_, meas_rel, meas);
00122
00123
00124 SymmetricMatrix cov(3);
00125 for (unsigned int i = 0; i < 3; i++)
00126 for (unsigned int j = 0; j < 3; j++)
00127 cov(i + 1, j + 1) = message->covariance[3 * i + j];
00128
00129
00130 boost::mutex::scoped_lock lock(filter_mutex_);
00131
00132
00133 for (list<Tracker*>::iterator it = trackers_.begin(); it != trackers_.end(); it++)
00134 if ((*it)->getName() == message->object_id) {
00135 (*it)->updatePrediction(message->header.stamp.toSec());
00136 (*it)->updateCorrection(meas, cov);
00137 }
00138
00139 if (message->object_id == "" && message->reliability > reliability_threshold_) {
00140 double closest_tracker_dist = start_distance_min_;
00141 StatePosVel est;
00142 for (list<Tracker*>::iterator it = trackers_.begin(); it != trackers_.end(); it++) {
00143 (*it)->getEstimate(est);
00144 double dst = sqrt(pow(est.pos_[0] - meas[0], 2) + pow(est.pos_[1] - meas[1], 2));
00145 if (dst < closest_tracker_dist)
00146 closest_tracker_dist = dst;
00147 }
00148
00149 if (follow_one_person_)
00150 cout << "Following one person" << endl;
00151 if (message->initialization == 1 && ((!follow_one_person_ && (closest_tracker_dist >= start_distance_min_)) || (follow_one_person_ && trackers_.empty()))) {
00152
00153
00154 ROS_INFO("Passed crazy conditional.");
00155 tf::Point pt;
00156 tf::pointMsgToTF(message->pos, pt);
00157 tf::Stamped<tf::Point> loc(pt, message->header.stamp, message->header.frame_id);
00158 robot_state_.transformPoint("base_link", loc, loc);
00159 float cur_dist;
00160 if ((cur_dist = pow(loc[0], 2.0) + pow(loc[1], 2.0)) < tracker_init_dist) {
00161
00162 cout << "starting new tracker" << endl;
00163 stringstream tracker_name;
00164 StatePosVel prior_sigma(tf::Vector3(sqrt(cov(1, 1)), sqrt(cov(
00165 2, 2)), sqrt(cov(3, 3))), tf::Vector3(0.0000001, 0.0000001, 0.0000001));
00166 tracker_name << "person " << tracker_counter_++;
00167 Tracker* new_tracker = new TrackerKalman(tracker_name.str(),
00168 sys_sigma_);
00169
00170 new_tracker->initialize(meas, prior_sigma,
00171 message->header.stamp.toSec());
00172 trackers_.push_back(new_tracker);
00173 ROS_INFO("Initialized new tracker %s", tracker_name.str().c_str());
00174 }
00175 else
00176 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);
00177 }
00178 else
00179 ROS_INFO("Failed crazy conditional.");
00180 }
00181 lock.unlock();
00182
00183
00184
00185
00186 meas_cloud_.points[0].x = meas[0];
00187 meas_cloud_.points[0].y = meas[1];
00188 meas_cloud_.points[0].z = meas[2];
00189 meas_cloud_.header.frame_id = meas.frame_id_;
00190 people_tracker_vis_pub_.publish(meas_cloud_);
00191 }
00192
00193
00194
00195
00196 void PeopleTrackingNode::callbackDrop(const cob_perception_msgs::PositionMeasurement::ConstPtr& message)
00197 {
00198 ROS_INFO("DROPPED PACKAGE for %s from %s with delay %f !!!!!!!!!!!",
00199 message->object_id.c_str(), message->name.c_str(), (ros::Time::now() - message->header.stamp).toSec());
00200
00201 }
00202
00203
00204
00205
00206
00207 void PeopleTrackingNode::spin()
00208 {
00209 ROS_INFO("People tracking manager started.");
00210
00211 while (ros::ok()){
00212
00213 boost::mutex::scoped_lock lock(filter_mutex_);
00214
00215
00216 vector<geometry_msgs::Point32> filter_visualize(trackers_.size());
00217 vector<float> weights(trackers_.size());
00218 sensor_msgs::ChannelFloat32 channel;
00219
00220
00221 unsigned int i=0;
00222 list<Tracker*>::iterator it= trackers_.begin();
00223 while (it!=trackers_.end()){
00224
00225 (*it)->updatePrediction(ros::Time::now().toSec() - sequencer_delay);
00226
00227
00228 cob_perception_msgs::PositionMeasurement est_pos;
00229 (*it)->getEstimate(est_pos);
00230 est_pos.header.frame_id = fixed_frame_;
00231
00232 ROS_DEBUG("Publishing people tracker filter.");
00233 people_filter_pub_.publish(est_pos);
00234
00235
00236 filter_visualize[i].x = est_pos.pos.x;
00237 filter_visualize[i].y = est_pos.pos.y;
00238 filter_visualize[i].z = est_pos.pos.z;
00239 weights[i] = *(float*)&(rgb[min(998, 999-max(1, (int)trunc( (*it)->getQuality()*999.0 )))]);
00240
00241
00242 ROS_INFO("Quality of tracker %s = %f",(*it)->getName().c_str(), (*it)->getQuality());
00243 if ((*it)->getQuality() <= 0){
00244 ROS_INFO("Removing tracker %s",(*it)->getName().c_str());
00245 delete *it;
00246 trackers_.erase(it++);
00247 }
00248 else it++;
00249 i++;
00250 }
00251 lock.unlock();
00252
00253
00254
00255
00256 channel.name = "rgb";
00257 channel.values = weights;
00258 sensor_msgs::PointCloud people_cloud;
00259 people_cloud.channels.push_back(channel);
00260 people_cloud.header.frame_id = fixed_frame_;
00261 people_cloud.points = filter_visualize;
00262 people_filter_vis_pub_.publish(people_cloud);
00263
00264
00265 usleep(1e6/freq_);
00266
00267 ros::spinOnce();
00268 }
00269 };
00270
00271
00272 };
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282 using namespace estimation;
00283 int main(int argc, char **argv)
00284 {
00285
00286 ros::init(argc, argv,"cob_people_tracker");
00287 ros::NodeHandle(nh);
00288
00289
00290 PeopleTrackingNode my_tracking_node(nh);
00291
00292
00293 my_tracking_node.spin();
00294
00295
00296
00297 return 0;
00298 }