42 #include <people_msgs/PositionMeasurement.h> 65 meas_cloud_.points = vector<geometry_msgs::Point32>(1);
114 ROS_DEBUG(
"Tracking node got a people position measurement (%f,%f,%f)",
115 message->pos.x, message->pos.y, message->pos.z);
119 tf::Vector3(message->pos.x, message->pos.y, message->pos.z));
120 meas_rel.
stamp_ = message->header.stamp;
121 meas_rel.
frame_id_ = message->header.frame_id;
125 SymmetricMatrix cov(3);
126 for (
unsigned int i = 0; i < 3; i++)
127 for (
unsigned int j = 0; j < 3; j++)
128 cov(i + 1, j + 1) = message->covariance[3 * i + j];
135 if ((*it)->getName() == message->object_id)
137 (*it)->updatePrediction(message->header.stamp.toSec());
138 (*it)->updateCorrection(meas, cov);
147 (*it)->getEstimate(est);
148 double dst = sqrt(pow(est.
pos_[0] - meas[0], 2) + pow(est.
pos_[1] - meas[1], 2));
149 if (dst < closest_tracker_dist)
150 closest_tracker_dist = dst;
154 cout <<
"Following one person" << endl;
159 ROS_INFO(
"Passed crazy conditional.");
168 cout <<
"starting new tracker" << endl;
169 stringstream tracker_name;
171 2, 2)), sqrt(cov(3, 3))),
tf::Vector3(0.0000001, 0.0000001, 0.0000001));
177 message->header.stamp.toSec());
179 ROS_INFO(
"Initialized new tracker %s", tracker_name.str().c_str());
182 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);
185 ROS_INFO(
"Failed crazy conditional.");
204 ROS_INFO(
"DROPPED PACKAGE for %s from %s with delay %f !!!!!!!!!!!",
205 message->object_id.c_str(), message->name.c_str(), (
ros::Time::now() - message->header.stamp).toSec());
215 ROS_INFO(
"People tracking manager started.");
223 vector<geometry_msgs::Point32> filter_visualize(
trackers_.size());
225 sensor_msgs::ChannelFloat32 channel;
229 list<Tracker*>::iterator it =
trackers_.begin();
236 people_msgs::PositionMeasurement est_pos;
237 (*it)->getEstimate(est_pos);
240 ROS_DEBUG(
"Publishing people tracker filter.");
244 filter_visualize[i].x = est_pos.pos.x;
245 filter_visualize[i].y = est_pos.pos.y;
246 filter_visualize[i].z = est_pos.pos.z;
247 weights[i] = *(
float*) & (
rgb[
min(998, 999 - max(1, (
int)trunc((*it)->getQuality() * 999.0)))]);
250 ROS_INFO(
"Quality of tracker %s = %f", (*it)->getName().c_str(), (*it)->getQuality());
251 if ((*it)->getQuality() <= 0)
253 ROS_INFO(
"Removing tracker %s", (*it)->getName().c_str());
265 channel.name =
"rgb";
266 channel.values = weights;
267 sensor_msgs::PointCloud people_cloud;
268 people_cloud.channels.push_back(channel);
270 people_cloud.points = filter_visualize;
292 int main(
int argc,
char **argv)
302 my_tracking_node.
spin();
static const unsigned int num_particles_tracker
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
void publish(const boost::shared_ptr< M > &message) const
void setData(const T &input)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
boost::mutex filter_mutex_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void callbackDrop(const people_msgs::PositionMeasurement::ConstPtr &message)
callback for dropped messages
sensor_msgs::PointCloud meas_cloud_
Class representing state with pos and vel.
tf::TransformListener robot_state_
ros::Publisher people_tracker_vis_pub_
void callbackRcv(const people_msgs::PositionMeasurement::ConstPtr &message)
callback for messages
static const unsigned int sequencer_subscribe_buffer
ros::Publisher people_filter_pub_
std::list< Tracker * > trackers_
trackers
unsigned int tracker_counter_
static const double sequencer_delay
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
double reliability_threshold_
virtual void initialize(const BFL::StatePosVel &mu, const BFL::StatePosVel &sigma, const double time)=0
initialize tracker
virtual ~PeopleTrackingNode()
destructor
static const unsigned int sequencer_internal_buffer
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
double start_distance_min_
message_filters::TimeSequencer< people_msgs::PositionMeasurement > * message_sequencer_
message sequencer
BFL::StatePosVel sys_sigma_
ros::Subscriber people_meas_sub_
static const double tracker_init_dist
ros::Publisher people_filter_vis_pub_
ROSCPP_DECL void spinOnce()
int main(int argc, char **argv)