messagefillter cannot callback. (multi subscribe /scan and /camera/depth/image_rect)

edit

Hi guys.

I'm try to subscribe 2 topic from /scan (laser) and /camera/depth/image_rect (depth camera) for safe wandering program. So I searched and find timesyschronizer from here for subscribing both topic sametime. however, I try to implement using that sync, it seem to be cannot callback.

before runnig this node, I run first openni2 launch and rp_lidar launch to publish data. additionally, when I subscribe each topic using simple subscriber successfully. now I'm wandering, did I use it correct way or not.

this is piece of my source code.

void callback(const sensor_msgs::LaserScan::ConstPtr& laser_msg, const sensor_msgs::Image::ConstPtr& depth_msg){
    printf("%20s:%5d : void callback(const sensor_msgs::LaserScan::ConstPtr& laser_msg, const sensor_msgs::Image::ConstPtr& depth_msg){\n",__func__,__LINE__);
    uint8_t a[10* depth_msg->width];
    sensor_msgs::Image test_image;
    sensor_msgs::Image test_dis_image;

    int test_j = 0;

    test_image.header = depth_msg->header;
    test_image.height = 10;
    test_image.width = depth_msg->width;
    test_image.encoding = depth_msg->encoding;
    test_image.is_bigendian = depth_msg->is_bigendian;
    test_image.step = depth_msg->width;
    test_image.data.resize(test_image.width * 10);
    printf("%20s:%5d : test_image.data.resize(test_image.width * 10);\n",__func__,__LINE__);

    for ( int j =0; j < depth_msg->step; j++){
          cout << ReadDepthData(0,(int)(j/4),depth_msg) << ", " ;
    }

    image_publisher.publish(test_image);
    printf("%20s:%5d : image_publisher.publish(test_image);\n",__func__,__LINE__);
}


int main(int argc, char **argv){
    ros::init(argc, argv, "lidar_depth_wander");    
    ros::NodeHandle n;                      

    message_filters::Subscriber<LaserScan> laser_data_sub(n, "/scan", 10);
    message_filters::Subscriber<Image> image_sub(n, "/camera/depth/image_rect", 10);
    image_publisher  = n.advertise<sensor_msgs::Image>("/test_image_raw", 10);
    printf("%20s:%5d : image_publisher  = n.advertise<sensor_msgs::Image>\n",__func__,__LINE__);
    velocity_publisher = n.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity", 10); // for kobuki
    printf("%20s:%5d : velocity_publisher = n.advertise<geometry_msgs::Twist>; \n",__func__,__LINE__);

    TimeSynchronizer<LaserScan, Image> sync(laser_data_sub, image_sub, 10);
    printf("%20s:%5d : TimeSynchronizer<LaserScan, Image>\n",__func__,__LINE__);
    sync.registerCallback(boost::bind(&callback, _1, _2));
    printf("%20s:%5d : sync.registerCallback\n",__func__,__LINE__);     

    ros::Rate loop_rate(10);
    ros::spin();
    printf("%20s:%5d : ros::spin();\n",__func__,__LINE__);

    while(ros::ok()){
        loop_rate.sleep();
    }
    printf("%20s:%5d : ending program;\n",__func__,__LINE__);

    return 0;
}

then this is CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(lidar_test_project)
find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation roscpp rospy sensor_msgs std_msgs    message_filters)
catkin_package(
  CATKIN_DEPENDS geometry_msgs message_runtime roscpp rospy sensor_msgs std_msgs message_filters
)
include_directories(
 ${catkin_INCLUDE_DIRS}
)
add_executable(lidar_depth_wander src/lidar_depth_wander.cpp)
target_link_libraries(lidar_depth_wander ${catkin_LIBRARIES})
add_dependencies(lidar_depth_wander ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

last my packge.xml

<package>
 <name>lidar_test_project</name>
 <version>0.0.0</version>
 <description>The lidar_test_project package</description>
 <maintainer email="kang@todo.todo">kang</maintainer>
 <license>TODO</license>

 <buildtool_depend>catkin</buildtool_depend>
 <build_depend>geometry_msgs</build_depend>
 <build_depend>message_generation</build_depend>
 <build_depend>roscpp</build_depend>
 <build_depend>rospy</build_depend>
 <build_depend>sensor_msgs</build_depend>
 <build_depend>std_msgs</build_depend>
 <build_depend>message_filters</build_depend>
 <run_depend>geometry_msgs</run_depend>
 <run_depend>roscpp</run_depend>
 <run_depend>rospy</run_depend>
 <run_depend>sensor_msgs</run_depend>
 <run_depend>std_msgs</run_depend>
 <run_depend>message_runtime</run_depend>
 <run_depend>message_filters</run_depend>
<export>

</export> </package>

and program result was:

 main:  187 : image_publisher  = n.advertise<sensor_msgs::Image>
 main:  189 : velocity_publisher = n.advertise<geometry_msgs::Twist>; 
 main:  193 : TimeSynchronizer<LaserScan, Image>
 main:  195 : sync.registerCallback
 main:  201 : ros::spin();                                       <-- Ctrl+C
 main:  206 : ending program;

thanks guys.

ADD I tried ... (more)