ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

can't synchronize Velodyne points with other PointCloud2 data [closed]

asked 2018-10-03 15:14:14 -0500

roskinetic gravatar image

updated 2018-10-03 20:49:45 -0500

jayess gravatar image

Hi, I am new to ROS and I need a help for time synchronization. When I synchronize Velodyne points with ZED Images, they are synchronized without any problem (I attached this working code below). But, when I synchronize Velodyne points with other PointCloud2 data which is published by a node that I created, the callback function in my code never get called. I think both have the same type of time header since they both are PointCloud2 but I don't know why the time synchronizer doesn't work in this case. I tried to use Approximate/Exact but none of them works. Please advise.

Working code.

ros::Publisher data1_pub;

ros::Publisher data2_pub;

void callback(const sensor_msgs::Image::ConstPtr& data1, const sensor_msgs::PointCloud2::ConstPtr& data2)
{
  ROS_INFO("Sync_Callback");

  data1_pub.publish(data1);

 data2_pub.publish(data2);

}

int main(int argc, char** argv)

{

  ros::init(argc, argv, "TimeSynchronizer");

  std::string data1_topic = "/zed/right/image_rect_color";

  std::string data2_topic = "/velodyne_points";

  ros::NodeHandle nh;

  message_filters::Subscriber<sensor_msgs::Image> data1_sub(nh, data1_topic, 1000);

  message_filters::Subscriber<sensor_msgs::PointCloud2> data2_sub(nh, data2_topic, 1000);

  data1_pub = nh.advertise<sensor_msgs::Image>("/synchronized" + data1_topic, 1000);

  data2_pub = nh.advertise<sensor_msgs::PointCloud2>("/synchronized" + data2_topic, 1000);

  TimeSynchronizer<Image, PointCloud2> sync(data1_sub, data2_sub,10);

   sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;

}

Not Working code.

ros::Publisher data1_pub;

ros::Publisher data2_pub;

void callback(const sensor_msgs::PointCloud2::ConstPtr& data1, const sensor_msgs::PointCloud2::ConstPtr& data2)
{

  ROS_INFO("Sync_Callback");

  data1_pub.publish(data1);

  data2_pub.publish(data2);

}

int main(int argc, char** argv)

{

  ros::init(argc, argv, "TimeSynchronizer");

  std::string data1_topic = "/velodyne_points";

  std::string data2_topic = "/chatter";

  ros::NodeHandle nh;

  message_filters::Subscriber<sensor_msgs::PointCloud2> data1_sub(nh, data1_topic, 1000);

  message_filters::Subscriber<sensor_msgs::PointCloud2> data2_sub(nh, data2_topic, 1000);

  data1_pub = nh.advertise<sensor_msgs::PointCloud2>("/synchronized" + data1_topic, 1000);

  data2_pub = nh.advertise<sensor_msgs::PointCloud2>("/synchronized" + data2_topic, 1000);

  TimeSynchronizer<PointCloud2, PointCloud2> sync(data1_sub, data2_sub,10);

   sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

"talker" node which publishes PointCloud2 data to /chatter topic

int main (int argc, char **argv){

    ros::init(argc, argv, "talker");

    ros::NodeHandle n;

        ros::Publisher chatter_pub = n.advertise<sensor_msgs::PointCloud2>("chatter", 1000);

        ros::Rate loop_rate(30); 

        int speed = 100;

    int count = 0;

    while (ros::ok()){

                sensor_msgs::PointCloud2 msg;

                msg.height = speed;

                ROS_INFO("%d",msg.height);

        chatter_pub.publish(msg);

                ros::spinOnce(); 

        loop_rate.sleep();

        ++count;

    }

    return 0;

}
edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by kmhallen
close date 2018-10-04 19:48:03.160113

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-10-03 23:11:52 -0500

kmhallen gravatar image

The time synchronization uses the header timestamp to find pairs of messages. Make sure to set the header timestamp in your talker node.

msg.header.stamp = ros::Time::now();
edit flag offensive delete link more

Comments

Thank you! It works now.

roskinetic gravatar image roskinetic  ( 2018-10-04 13:17:16 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2018-10-03 15:14:14 -0500

Seen: 321 times

Last updated: Oct 03 '18