Index: leg_detector/src/laser_processor.cpp
===================================================================
--- leg_detector/src/laser_processor.cpp (revision 39342)
+++ leg_detector/src/laser_processor.cpp (working copy)
@@ -42,11 +42,11 @@
Sample* Sample::Extract(int ind, const sensor_msgs::LaserScan& scan)
{
- Sample* s = new Sample;
+ Sample* s = new Sample();
s->index = ind;
s->range = scan.ranges[ind];
- s->intensity = scan.intensities[ind];
+ //s->intensity = scan.intensities[ind];
s->x = cos( scan.angle_min + ind*scan.angle_increment ) * s->range;
s->y = sin( scan.angle_min + ind*scan.angle_increment ) * s->range;
if (s->range > scan.range_min && s->range < scan.range_max)
@@ -89,9 +89,9 @@
if (cloud.channels[0].name == "rgb")
cloud.channels[0].values.push_back(color_val);
-
+/*
if (cloud.channels[0].name == "intensity")
- cloud.channels[0].values.push_back((*sample_iter)->intensity);
+ cloud.channels[0].values.push_back((*sample_iter)->intensity);*/
}
}
Index: leg_detector/launch/leg_detector.launch
===================================================================
--- leg_detector/launch/leg_detector.launch (revision 39342)
+++ leg_detector/launch/leg_detector.launch (working copy)
@@ -1,4 +1,4 @@
-
+
Index: face_detector/launch/face_detector.wide.launch
===================================================================
--- face_detector/launch/face_detector.wide.launch (revision 39342)
+++ face_detector/launch/face_detector.wide.launch (working copy)
@@ -1,5 +1,5 @@
-
+
Index: people_tracking_filter/include/people_tracking_filter/people_tracking_node.h
===================================================================
--- people_tracking_filter/include/people_tracking_filter/people_tracking_node.h (revision 39342)
+++ people_tracking_filter/include/people_tracking_filter/people_tracking_node.h (working copy)
@@ -53,8 +53,9 @@
// messages
#include
#include
-#include
-
+#include
+#include
+
// log files
#include
@@ -72,10 +73,10 @@
virtual ~PeopleTrackingNode();
/// callback for messages
- void callbackRcv(const people::PositionMeasurement::ConstPtr& message);
+ void callbackRcv(const people_msgs::PositionMeasurement::ConstPtr& message);
/// callback for dropped messages
- void callbackDrop(const people::PositionMeasurement::ConstPtr& message);
+ void callbackDrop(const people_msgs::PositionMeasurement::ConstPtr& message);
/// tracker loop
void spin();
@@ -89,8 +90,10 @@
ros::Publisher people_filter_vis_pub_;
ros::Publisher people_tracker_vis_pub_;
+ ros::Subscriber people_meas_sub_;
+
/// message sequencer
- message_sequencing::TimeSequencer* message_sequencer_;
+ message_filters::TimeSequencer* message_sequencer_;
/// trackers
std::list trackers_;
Index: people_tracking_filter/manifest.xml
===================================================================
--- people_tracking_filter/manifest.xml (revision 39342)
+++ people_tracking_filter/manifest.xml (working copy)
@@ -15,7 +15,7 @@
-
+
Index: people_tracking_filter/src/mcpdf_pos_vel.cpp
===================================================================
--- people_tracking_filter/src/mcpdf_pos_vel.cpp (revision 39342)
+++ people_tracking_filter/src/mcpdf_pos_vel.cpp (working copy)
@@ -117,7 +117,7 @@
weights[t] = rgb[999-(int)trunc(max(0.0,min(999.0,hist(r,c)*2*total*total)))];
t++;
}
- cloud.header.frame_id = "odom";
+ cloud.header.frame_id = "odom_combined";
cloud.points = points;
channel.name = "rgb";
channel.values = weights;
Index: people_tracking_filter/src/people_tracking_node.cpp
===================================================================
--- people_tracking_filter/src/people_tracking_node.cpp (revision 39342)
+++ people_tracking_filter/src/people_tracking_node.cpp (working copy)
@@ -45,7 +45,7 @@
using namespace std;
using namespace tf;
using namespace BFL;
-using namespace message_sequencing;
+using namespace message_filters;
static const double sequencer_delay = 0.8; //TODO: this is probably too big, it was 0.8
static const unsigned int sequencer_internal_buffer = 100;
@@ -89,10 +89,19 @@
people_tracker_vis_pub_ = nh_.advertise("people_tracker_measurements_visualization",10);
// register message sequencer
+ people_meas_sub_ = nh_.subscribe("people_tracker_measurements", 1, &PeopleTrackingNode::callbackRcv, this );
+/*
+ message_filters::Subscriber sub(nh, "people_tracker_measurements", 1);
+ message_filters::TimeSequencer seq(sub, ros::Duration().fromSec(sequencer_delay), ros::Duration().fromSec(0.1), 10, nh_);
+ seq.registerCallback( &PeopleTrackingNode::callbackRcv, this );
+*/
+
+/*
message_sequencer_ = new TimeSequencer(this, "people_tracker_measurements", boost::bind(&PeopleTrackingNode::callbackRcv, this, _1),
boost::bind(&PeopleTrackingNode::callbackDrop, this, _1),
ros::Duration().fromSec(sequencer_delay),
sequencer_internal_buffer, sequencer_subscribe_buffer);
+*/
}
@@ -213,7 +222,7 @@
{
ROS_INFO("People tracking manager started.");
- while (nh_.ok()){
+ while (ros::ok()){
// ------ LOCKED ------
boost::mutex::scoped_lock lock(filter_mutex_);
Index: people_tracking_filter/launch/filter.launch
===================================================================
--- people_tracking_filter/launch/filter.launch (revision 39342)
+++ people_tracking_filter/launch/filter.launch (working copy)
@@ -1,5 +1,5 @@
-
+
@@ -22,6 +22,6 @@
-
+
Index: people_tracking_filter/CMakeLists.txt
===================================================================
--- people_tracking_filter/CMakeLists.txt (revision 39342)
+++ people_tracking_filter/CMakeLists.txt (working copy)
@@ -30,7 +30,9 @@
#target_link_libraries(example ${PROJECT_NAME})
-#rosbuild_add_executable(people_tracker
+rosbuild_add_executable(people_tracker
+ src/people_tracking_node.cpp )
+target_link_libraries(people_tracker people_tracking_filter)
rosbuild_add_library(people_tracking_filter
src/uniform_vector.cpp
src/gaussian_vector.cpp
@@ -44,4 +46,4 @@
src/tracker_particle.cpp
src/tracker_kalman.cpp
src/detector_particle.cpp )
-# src/people_tracking_node.cpp )
+