The registerCallback is not running
Hi, I'm working with some packages and there is a node that works on the last step of processing of some data in this case are images an rois to track, the code below shows that the ObjecTtrackinNode has two methods registerCallback, but when I launch the nodes, this ObjectTrackingNode is the only one that doesn't show the image and the results are not available, and I think that is because the registerCallback are not running. So what could be the problem, and why the registerCallback doesn't work ? Or what are the conditions to just omit the registerCallback ?
I'm using ROS indigo, OpenCV 2.4.9, PCL 1.7 and the Kinect to get the images
Thank's and I hope you could help.
#include "ros/ros.h"
#include <sstream>
//Publish Messages
#include "roi_msgs/RoiRect.h"
#include "roi_msgs/Rois.h"
#include "std_msgs/String.h"
#include "roi_msgs/HumanEntry.h"
#include "roi_msgs/HumanEntries.h"
//Time Synchronizer
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
//Subscribe Messages
#include <sensor_msgs/Image.h>
#include <stereo_msgs/DisparityImage.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
// Image Transport
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
// Used to display OPENCV images
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// Tracker libraries
#include <object_tracking/tracker.h>
#include <boost/shared_ptr.hpp>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/thread.hpp>
using namespace stereo_msgs;
using namespace message_filters::sync_policies;
using namespace roi_msgs;
using namespace sensor_msgs;
using namespace sensor_msgs::image_encodings;
using sensor_msgs::Image;
using cv_bridge::CvImagePtr;
using std::vector;
using std::string;
using cv::Rect;
using cv::Mat;
class ObjectTrackingNode
{
private:
// Define Node
ros::NodeHandle node_;
// Subscribe to Messages
message_filters::Subscriber<DisparityImage> sub_disparity_;
message_filters::Subscriber<Image> sub_image_;
message_filters::Subscriber<Rois> sub_rois_;
message_filters::Subscriber<sensor_msgs::CameraInfo> l_camera_info_;
message_filters::Subscriber<sensor_msgs::CameraInfo> r_camera_info_;
// Define the Synchronizer
typedef ApproximateTime<Image, DisparityImage, Rois> ApproximatePolicy;
typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
boost::shared_ptr<ApproximateSync> approximate_sync_;
typedef ApproximateTime<sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximatePolicy2;
typedef message_filters::Synchronizer<ApproximatePolicy2> ApproximateSync2;
boost::shared_ptr<ApproximateSync2> camera_info_sync_;
// Messages to Publish
ros::Publisher pub_Color_Image_;
ros::Publisher human_tracker_data_;
// Trackers
std::vector<boost::shared_ptr<ParticleTrack> > trackers_;
std::vector<cv::Rect> detections_;
// Parallelization
int thread_count_;
boost::asio::io_service service_;
boost::shared_ptr<boost::asio::io_service::work> work_;
boost::thread_group task_threads_;
boost::condition_variable task_condition_;
boost::mutex task_mutex_;
cv::Mat image, disp;
cv::Mat Q_;
uint64_t uid_counter_;
uint64_t write_count_;
std::string filter_dir_;
std::string csv_filename_;
std::ofstream log_file_;
double object_timeout_;
double angle_;
double dt_;
ros::Time current_;
ros::Time previous_;
bool geometry_initialized_;
bool calculate_covariance_;
bool show_images_;
int max_objects_;
bool write_image_results_;
bool debug_mode_;
public:
explicit ObjectTrackingNode(const ros::NodeHandle& nh):
node_(nh)
{
string nn = ros::this_node::getName();
int qs;
if(!node_.getParam(nn + "/Q_Size",qs)){
qs=3;
}
uid_counter_ = 0;
write_count_ = 0;
if(!node_.getParam(nn + "/Filter_Dir",filter_dir_))
{
//Load matrix 2x2 eigenvectors and eigenvalues 2x1
filter_dir_ = std::string("/home/charl/catkin_ws/src/object_tracking/perception_data");
}
if(!node_.getParam(nn + "/Thread_Count",thread_count_)){
thread_count_ = 5; // number of threads for updating tracker states
}
if(!node_.getParam(nn + "/Max_Objects",max_objects_)){
max_objects_ = 5; // number of threads for updating tracker states
}
if(!node_.getParam(nn + "/Object_Timeout",object_timeout_)){
object_timeout_ = 5.0; // 5.0s default timeout
}
if(!node_.getParam(nn + "/Calculate_Covariance", calculate_covariance_)){
calculate_covariance_ = false; // don't calculate by default
}
if(!node_.getParam(nn + "/Show_Images", show_images_)){
show_images_ = true; // show tracker image by default
}
if(!node_.getParam(nn + "/Write_image_results", write_image_results_)){
write_image_results_ = false; // don't publish by default
}
if(!node_.getParam(nn + "/Debug_mode", debug_mode_)){
debug_mode_ = true; // debug_mode active by default
}
// Published Messages
human_tracker_data_ = node_.advertise<roi_msgs::HumanEntries> ("/human_tracker_data", 10);
// Subscribe to Messages
sub_image_.subscribe(node_,"Color_Image",qs);
sub_disparity_.subscribe(node_, "Disparity_Image",qs);
sub_rois_.subscribe(node_,"input_rois",qs);
l_camera_info_.subscribe(node_,"l_camera_info",qs);
r_camera_info_.subscribe(node_,"r_camera_info",qs);
// Sync the Synchronizer
approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(qs),
sub_image_, sub_disparity_, sub_rois_));
approximate_sync_->registerCallback(boost::bind(&ObjectTrackingNode::imageCb,
this,_1,_2,_3));
camera_info_sync_.reset(new ApproximateSync2(ApproximatePolicy2(qs),
l_camera_info_, r_camera_info_));
camera_info_sync_->registerCallback(boost::bind(&ObjectTrackingNode::infoCallback,
this,_1,_2));
// Parallelization
work_.reset(new boost::asio::io_service::work(service_));
for (int i = 0; i < thread_count_; i++)
{
task_threads_.create_thread(
boost::bind(&boost::asio::io_service::run, &service_));
}
// Visualization
if(show_images_)
{
cv::namedWindow("Trackers", 0 ); // non-autosized
}
if(debug_mode_)
{
cv::namedWindow("Test",0);
}
if (show_images_ || debug_mode_)
{
cv::startWindowThread();
}
// Geometry
angle_ = -180.0;
geometry_initialized_ = false;
Q_ = cv::Mat::zeros(4,4,CV_64F); // initialize with default kinect parameters
Q_.at<double>(3,2) = 1.0 / (0.075); // 1/baseline
Q_.at<double>(0,3) = -319.5; // -cx
Q_.at<double>(1,3) = -239.5; // -cy
double fixed_focal_length;
if(!node_.getParam(nn + "/FixedFocalLength", fixed_focal_length)){
fixed_focal_length = 525; // don't calculate by default
}
Q_.at<double>(2,3) = fixed_focal_length; // fx
ROS_ERROR("UUsing focal length of %lf",fixed_focal_length);
previous_ = ros::Time::now();
//previous_ = ros::Time(0);
}
void infoCallback(const sensor_msgs::CameraInfoConstPtr& l_cam_info,
const sensor_msgs::CameraInfoConstPtr& r_cam_info)
{
// Get camera info
if(!geometry_initialized_)
{
sensor_msgs::CameraInfo l_info = *l_cam_info;
sensor_msgs::CameraInfo r_info = *r_cam_info;
//extract baseline from camera info
double baseline;
if(l_info.P[3] < 0.0)
{
ROS_ERROR("Left and right camera info are switched."
"Please check launch file");
baseline = -l_info.P[3] / l_info.P[0];
}
else if(r_info.P[3] < 0.0)
{
baseline = -r_info.P[3]/r_info.P[0];
}
else
{
ROS_ERROR("No baseline detected. Using default baseline"
"of 0.075m");
baseline = 0.075;
}
ROS_ERROR("using baseline=%lf",baseline);
double divisor= l_info.binning_x;
if (divisor<1) divisor = 1.0;// avoid div by zero (kinect)
Q_.at<double>(3,2) = 1.0 / baseline; // 1/baseline
Q_.at<double>(0,3) = -l_info.K[2]/divisor; // -cx
Q_.at<double>(1,3) = -l_info.K[5]/divisor; // -cy
string nn = ros::this_node::getName();
double fixed_focal_length;
if(!node_.getParam(nn + "/FixedFocalLength", fixed_focal_length)){
fixed_focal_length = l_info.K[0]/divisor; // fx
}
Q_.at<double>(2,3) = fixed_focal_length; // fx
ROS_ERROR(" - Using focal length of %lf",fixed_focal_length);
std::cout << "Reprojection Matrix:" << std::endl << Q_ << std::endl;
geometry_initialized_ = true;
}
}
void imageCb(const ImageConstPtr& image_msg,
const DisparityImageConstPtr& disparity_msg,
const RoisConstPtr& rois_msg){
bool label_all;
vector<Rect> R_out;
vector<int> L_out;
string param_name;
string nn = ros::this_node::getName();
string cfnm;
int numSamples;
// Make sure geometry has been initialized
if(!geometry_initialized_)
return;
// Get timestamp
current_ = image_msg->header.stamp;
dt_ = current_.toSec() - previous_.toSec();
//Use CV Bridge to convert image
CvImagePtr cv_gray = cv_bridge::toCvCopy(image_msg, image_encodings::MONO8);
CvImagePtr cv_color = cv_bridge::toCvCopy(image_msg, image_encodings::BGR8);
image = cv_color->image;
// check encoding and create an intensity image from disparity image
assert(disparity_msg->image.encoding == image_encodings::TYPE_32FC1);
cv::Mat disp1(disparity_msg->image.height,
disparity_msg->image.width,
CV_32F,
(float*) &disparity_msg->image.data[0],
disparity_msg->image.step);
disp = disp1;
// Estimate angle initially to make sure it is within tolerance
// Angle is used to correct for rotation when making 2D xz ground map
if(angle_ < -179.0){
angle_ = calculate_angle(disp);
ROS_ERROR("Ground Plane Angle = %lf",angle_);
}
if(std::abs(angle_) > 15.0)
{
angle_ = -180.0;
return;
}
// take the region of interest message and create vectors of ROIs and labels
detections_.clear();
for(unsigned int i = 0;i < rois_msg->rois.size(); i++){
int x = rois_msg->rois[i].x;
int y = rois_msg->rois[i].y;
int w = rois_msg->rois[i].width;
int h = rois_msg->rois[i].height;
int l = rois_msg->rois[i].label;
Rect R(x,y,w,h);
detections_.push_back(R);
}
// Update all objects
int threads = 0;
int finished = 0;
for(int i = 0; i < trackers_.size(); i++)
{
service_.post(boost::bind(&ObjectTrackingNode::update_task, this, i, 0.05, &finished)); // post task
threads++;
}
// wait for all jobs to finish
boost::unique_lock<boost::mutex> lock(task_mutex_);
while (finished < threads && ros::ok())
{
task_condition_.wait(lock);
}
// Register detections to existing objects
match_detections();
// Add unresolved detections to world model
add_new();
// Draw all of the boxes
ROS_ERROR ("Drawing ROIs existing in image");
for(int i = 0; i < trackers_.size(); i++)
{
trackers_[i]->draw_box(image,cv::Scalar(255));
}
if(show_images_)
cv::imshow("Trackers", image);
// Kill old objects (objects not matched to a detection recently)
kill_old();
// Publish object states
publish_object_states();
previous_ = current_;
}
void match_detections()
{
//Match process
}
void add_new()
{
// Create a new tracker object
.........
}
void kill_old()
{
Some process stuff...
}
void update_task(int idx, double dt, int* finished)
{
trackers_[idx]->update_state(image, disp, dt_);
.......
}
void publish_object_states()
{
// accumulate data
....
}
double calculate_angle(cv::Mat& disparity_image)
{
//Some stuff here
}
~ObjectTrackingNode()
{
}
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "ObjectTracking");
ros::NodeHandle n;
ObjectTrackingNode HN(n);
ros::spin();
return 0;
}
Asked by soichirutk on 2016-04-29 17:26:51 UTC
Comments