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