rosping.cpp
Go to the documentation of this file.
00001 // doc/html/boost_asio/example/icmp/
00002 
00003 #include "ping.cpp"
00004 #include <ros/ros.h>
00005 #include <std_msgs/Float64.h>
00006 
00007 int main(int argc, char* argv[])
00008 {
00009   try
00010       {
00011           ros::init(argc, argv, "ping");
00012           if (argc != 2)
00013               {
00014                   std::cerr << "Usage: " << argv[0] << " <host>" << std::endl;
00015 #if !defined(BOOST_WINDOWS)
00016                   std::cerr << "(You may need to set permission to run this program as root.)\n$ chown root.root rosping; ls -al rosping; chmod 4755 rosping\n" << std::endl;
00017 #endif
00018                   return 1;
00019               }
00020 
00021           boost::asio::io_service io_service;
00022           pinger p(io_service, argv[1]);
00023 
00024           
00025 
00026           ros::NodeHandle n;
00027           ros::NodeHandle pnh("~");
00028           double rate;
00029           if (!pnh.getParam("rate", rate)) {
00030             rate = 10.0;         // 0.1Hz
00031           }
00032           ros::Publisher pub = n.advertise<std_msgs::Float64>("ping/delay", 10);
00033 
00034           std_msgs::Float64 msg;
00035 
00036           ros::Time last_time = ros::Time::now();
00037 
00038           while (ros::ok()) {
00039               ros::Time now = ros::Time::now();
00040               io_service.run_one();              
00041               if ( now - last_time > ros::Duration(rate) ) {
00042 
00043                   msg.data = p.delay;
00044                   pub.publish(msg);
00045                   ros::spinOnce();
00046 
00047                   last_time = now;
00048               }
00049           }
00050 
00051       }
00052   catch (std::exception& e)
00053       {
00054           std::cerr << "Exception: " << e.what() << std::endl;
00055       }
00056 }


rosping
Author(s): Kei Okada
autogenerated on Mon Oct 6 2014 11:00:33