Approximate time synchronizer problem

asked 2015-06-21 11:31:38 -0600

Aggelos Triadafillidis gravatar image

Hello, i have two sensors that both publish to individual topics. The first one publishes PointCloud2 message type and the second one a custom message with header (filled stamp = ros::Time::now()).

The problem is that when i launch the "listener node" the callback works fine for a few seconds and then stops. Both incoming messages keep comming as i can see with rostopic hz.

I have tried many queue size combinations but i get the same result.

Here is the "listener.h" code:

namespace pandora_vision
{
  typedef message_filters::Subscriber<sensor_msgs::PointCloud2> pcSubscriber;
  typedef message_filters::Subscriber<distrib_msgs::flirLeptonMsg> thermalSubscriber;
  typedef message_filters::sync_policies::ApproximateTime
  <sensor_msgs::PointCloud2, distrib_msgs::flirLeptonMsg> mySyncPolicy;

   /**
  @class RgbDTSynchronizer
  @brief Responsible for the synchronization of PointCloud2 messages from
  kinect and flirLeptonMsg from flir-Lepton camera.
  **/
  class RgbDTSynchronizer
  {
    private:

    // The ROS node handle
    ros::NodeHandle nodeHandle_;

    // The message_filters subscriber to kinect that aquires the PointCloud2 message
    pcSubscriber *inputPointCloudSubscriber_;

    // The name of the topic from where the PointCloud2 message is acquired
    std::string inputPointCloudTopic_;

    // The message_filters subscriber to flir-lepton camera that aquires the flirLeptonMsg message
    thermalSubscriber *inputThermalCameraSubscriber_;

    message_filters::Synchronizer<mySyncPolicy> *synchronizer_;

    // The name of the topic from where the flirLeptonMsg message is acquired
    std::string inputThermalCameraTopic_;

    // Subscriber to kinect's pointcloud if thermal mode is off
    ros::Subscriber pointCloudSubscriber_;

    // The publisher which will advertise the synchronized message
    ros::Publisher synchronizedMsgPublisher_;

    // The queue size of the approximate time synch method
    int queue_;
  }

And the "listener.cpp" code:

namespace pandora_vision
{
  RgbDTSynchronizer::RgbDTSynchronizer(void)
  {
    getTopicNames();
    queue_ = 5;

    inputPointCloudSubscriber_ = new pcSubscriber(nodeHandle_,
      inputPointCloudTopic_, queue_);

    inputThermalCameraSubscriber_ = new thermalSubscriber(nodeHandle_,
      inputThermalCameraTopic_, queue_);

    synchronizer_ = new message_filters::Synchronizer<mySyncPolicy>
      (mySyncPolicy(queue_), *inputPointCloudSubscriber_,
      *inputThermalCameraSubscriber_);

    synchronizer_->registerCallback(
      boost::bind(&RgbDTSynchronizer::synchronizedCallback,
      this, _1, _2));

    synchronizedMsgPublisher_ = nodeHandle_.advertise
      <pandora_vision_msgs::SynchronizedMsg>
      (synchronizedMsgTopic_, 1000);

    ROS_INFO("[RGBDT Synchronizer node] Initiated");
}

void RgbDTSynchronizer::synchronizedCallback(
const sensor_msgs::PointCloud2::ConstPtr& pcMsg,
const distrib_msgs::flirLeptonMsg::ConstPtr& thermalMsg)
{
  // The synchronized message that is sent to synchronizer node
  pandora_vision_msgs::SynchronizedMsg msg;
  msg.pc = *pcMsg;

  // The synchronized message header stamp
  msg.header.stamp = ros::Time::now();

  // The thermal Image
  msg.thermalInfo.header = thermalMsg->thermalImage.header;
  msg.thermalInfo = thermalMsg->thermalImage;
  msg.thermalInfo.encoding = "mono8";
  msg.thermalInfo.height = 60;
  msg.thermalInfo.width = 80;
  msg.thermalInfo.step = 80 * sizeof(uint8_t);

  // The temperatures multiarray
  msg.temperatures = thermalMsg->temperatures;

  synchronizedMsgPublisher_.publish(msg);
}

I would understand if callback never been called, but its weird to start the process and then stop.

Thank you for your time.

edit retag flag offensive close merge delete

Comments

1

Presumably the messages are not matching your policy. I suggest that you try displaying the headers of the incoming messages and then you can see if they're matching your synchronization policy. Without more information like your syncronization policy and a way to reproduce, it's hard to help more.

tfoote gravatar image tfoote  ( 2015-06-21 23:00:22 -0600 )edit

Sorry but i forgot to mention that i am running this code with rosbag. It seems that only then it stops responding. How is this possible?

I've set the appropriate arguments (i think) in launch file: args="-- clock -l $(arg path)" and run the code with /use_sim_time=true

Aggelos Triadafillidis gravatar image Aggelos Triadafillidis  ( 2015-06-22 13:01:55 -0600 )edit