Robotics StackExchange | Archived questions

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

Answers