[ros-users] people stack on boxturtle

I've been trying to get the people_tracking_filter working on the pr2
with boxturtle. I'm attaching a patch to update most of the launch
files for current locations of packages/nodes and to make leg_detector
not crash. This patch also includes changes to people_tracking_node so
that it will compile. However, despite receiving good measurements
from the face_detector and leg_detector nodes, it is not outputting
any estimates as a point cloud. Does anyone know what I'm doing wrong?

I am attaching the two launch files that I'm using.

-Dave
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 @@
 <launch>
-  <node pkg="people" type="leg_detector" args="scan:=base_scan $(find people)/config/trained_leg_detector.yaml" output="screen"/>
+  <node pkg="leg_detector" type="leg_detector" args="scan:=base_scan $(find leg_detector)/config/trained_leg_detector.yaml" output="screen" name="leg_detector"/>
 </launch>

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 @@
 <launch>
-  <node ns="wide_stereo" pkg="stereo_image_proc" type="stereo_image_proc" args="" output="screen" name="stereoproc"/> 
+  <!--<node ns="wide_stereo" pkg="stereo_image_proc" type="stereo_image_proc" args="" output="screen" name="stereoproc"/> -->

   <node pkg="face_detector" type="face_detector" name="face_detector" args="stereo:=wide_stereo image:=image_rect" output="screen">
        <param name="classifier_name" type="string" value="frontalface" />
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 <sensor_msgs/PointCloud.h>
 #include <people_msgs/PositionMeasurement.h>
-#include <message_sequencing/time_sequencer.h>
- 
+#include <message_filters/time_sequencer.h>
+#include <message_filters/subscriber.h> 
+
 // log files
 #include <fstream>

@@ -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<people::PositionMeasurement>* message_sequencer_;
+ message_filters::TimeSequencer<people_msgs::PositionMeasurement>* message_sequencer_;

   /// trackers
   std::list<Tracker*> 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 @@
   <depend package="tf"/>
   <depend package="bfl"/>
   <depend package="bullet"/>
-  <!--depend package="message_sequencing" /-->
+  <depend package="message_filters" />
   <depend package="people_msgs" />
   <depend package="sensor_msgs" />

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<sensor_msgs::PointCloud>("people_tracker_measurements_visualization",10);

     // register message sequencer
+    people_meas_sub_ = nh_.subscribe("people_tracker_measurements", 1, &PeopleTrackingNode::callbackRcv, this );
+/*
+    message_filters::Subscriber<people_msgs::PositionMeasurement> sub(nh, "people_tracker_measurements", 1);   
+    message_filters::TimeSequencer<people_msgs::PositionMeasurement> seq(sub, ros::Duration().fromSec(sequencer_delay), ros::Duration().fromSec(0.1), 10, nh_);
+    seq.registerCallback( &PeopleTrackingNode::callbackRcv, this );
+*/
+
+/*
     message_sequencer_ = new TimeSequencer<people_msgs::PositionMeasurement>(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 @@
 <launch>
-<param name="people_tracker/fixed_frame" type="string" value="odom"/>
+<param name="people_tracker/fixed_frame" type="string" value="odom_combined"/>
 <param name="people_tracker/freq" value="10.0"/>
 <param name="people_tracker/start_distance_min" value="0.5"/>
 <param name="people_tracker/reliability_threshold" value="0.75"/>
@@ -22,6 +22,6 @@
 <param name="people_tracker/sys_sigma_vel_y" value="0.5"/>
 <param name="people_tracker/sys_sigma_vel_z" value="0.5"/>

-<node pkg="people" type="people_tracker" output="screen"/>
+<node pkg="people_tracking_filter" type="people_tracker" name="people_tracker" output="screen"/>
</launch>

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 )
+