Hi all,
Right now I have a node RGBDImageProc that does some processing on the incoming data from an RGBD camera. It's a slightly modified version of the one here: https://github.com/ccny-ros-pkg/ccny_rgbd_tools/tree/master/ccny_rgbd
Now I want to use this node with multiple cameras. What I do is to launch 2 instances of this node, each one inside a different namespace and connected to a different camera. The launching works fine and each camera publishes all the information into independent topics. For instance: /cam1/camera/rgb/..., /cam2/camera/rgb/... I can view the images of both cameras simultaneously using image_viewer so they are publishing properly.
My problem comes at the processing node. Right now it's using an ApproximateTime Synchronizer to process the information from the RGB and Depth information in a single Callback, like this:
RGBDImageProc::RGBDImageProc(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private): nh_(nh), nh_private_(nh_private), rgb_image_transport_(nh_), depth_image_transport_(nh_), config_server_(nh_private_), size_in_(0,0){
//....
// subscribers
sub_rgb_.subscribe (rgb_image_transport_,
"/"+cam_name_+"/camera/rgb/image", queue_size_);
sub_depth_.subscribe(depth_image_transport_,
"/"+cam_name_+"/camera/depth/image_raw", queue_size_);
sub_rgb_info_.subscribe (nh_,
"/"+cam_name_+"/camera/rgb/camera_info", queue_size_);
sub_depth_info_.subscribe(nh_,
"/"+cam_name_+"/camera/depth/camera_info", queue_size_);
sync_.reset(new RGBDSynchronizer4(RGBDSyncPolicy4(queue_size_), sub_rgb_, sub_depth_, sub_rgb_info_, sub_depth_info_));
sync_->registerCallback(boost::bind(&RGBDImageProc::RGBDCallback, this, _1, _2, _3, _4));
}
The callback is:
void RGBDImageProc::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& rgb_info_msg,
const CameraInfoMsg::ConstPtr& depth_info_msg)
{
std::cout
The class header contains the following:
private:
ros::NodeHandle nh_; ///< the public nodehandle
ros::NodeHandle nh_private_; ///< the private nodehandle
boost::shared_ptr<RGBDSynchronizer4> sync_; ///< ROS 4-topic synchronizer
ImageTransport rgb_image_transport_; ///< ROS image transport for rgb message
ImageTransport depth_image_transport_; ///< ROS image transport for depth message
ImageSubFilter sub_rgb_; ///< ROS subscriber for rgb message
ImageSubFilter sub_depth_; ///< ROS subscriber for depth message
CameraInfoSubFilter sub_rgb_info_; ///< ROS subscriber for rgb camera info
CameraInfoSubFilter sub_depth_info_; ///< ROS subscriber for depth camera info
and
typedef image_transport::ImageTransport ImageTransport;
typedef image_transport::SubscriberFilter ImageSubFilter;
typedef message_filters::Subscriber CameraInfoSubFilter;
typedef message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, CameraInfoMsg> RGBDSyncPolicy3;
typedef message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, CameraInfoMsg, CameraInfoMsg> RGBDSyncPolicy4;
typedef message_filters::Synchronizer RGBDSynchronizer3;
typedef message_filters::Synchronizer RGBDSynchronizer4;
If I first run one camera, under the namespace /cam1/..., everything works perfectly, and the RGBDCallback is executed. However, when I start the second camera (with the first one still running), under the namespace /cam2/..., the RGBDCallback is not executed anymore, either for cam1 or for cam2 (in this case, it never runs a single call).
Tracking the error, I found out that it hangs after doing sync_->registerCallback(...)
in cam2.
Are the two Synchronizers colliding somehow? How could I solve this problem?
Thank you very much.
P.S.: sorry for the bad code formatting, I don't know how to fix it.
Originally posted by Tinrik on ROS Answers with karma: 51 on 2014-04-27
Post score: 0