00001 #include <cstdio> 00002 #include <ros/ros.h> 00003 #include <boost/lexical_cast.hpp> 00004 #include "polled_camera/GetPolledImage.h" 00005 00006 int main(int argc, char** argv) 00007 { 00008 ros::init(argc, argv, "poller", ros::init_options::AnonymousName); 00009 if (argc < 2) { 00010 printf("Usage: %s <Hz> camera:=<namespace> output:=<namespace>\n", argv[0]); 00011 return 0; 00012 } 00013 double hz = boost::lexical_cast<double>(argv[1]); 00014 00015 ros::NodeHandle nh; 00016 std::string service_name = nh.resolveName("camera") + "/request_image"; 00017 ros::ServiceClient client = nh.serviceClient<polled_camera::GetPolledImage>(service_name); 00018 00019 polled_camera::GetPolledImage::Request req; 00020 polled_camera::GetPolledImage::Response rsp; 00021 req.response_namespace = nh.resolveName("output"); 00022 00023 ros::Rate loop_rate(hz); 00024 while (nh.ok()) { 00025 if (client.call(req, rsp)) { 00026 std::cout << "Timestamp: " << rsp.stamp << std::endl; 00027 loop_rate.sleep(); 00028 } 00029 else { 00030 ROS_ERROR("Service call failed"); 00031 client.waitForExistence(); 00032 } 00033 } 00034 }