Re: [ros-users] Ros on Arm Hanging

>>> class LatencyClient {
>>> public:
>>>    LatencyClient() :
>>>        client(node.serviceClient< rpc_latency::ping_pong
>>> >("rpc_latency"))
>>>    {}
>>>    /**
>>>     * Latency benchmarking service request handler. This initiates
>>> the request and waits
>>>     * for the response. Handling is done in main().
>>>     * @return bool - success or failure of the request.
>>>     */
>>>    bool ping_pong() {
>>>        rpc_latency::ping_pong srv;
>>>        TimeStamp send_stamp;
>>>        TimeStamp final_stamp;

>>>        srv.request.ping_time = static_cast<double>(send_stamp.stamp());
>>>        if ( client.call(srv) ) {
>>>            final_stamp.stamp();
>>>            ROS_INFO("Send   Time : %lf", srv.response.pong_time -
>>> srv.request.ping_time);
>>>            ROS_INFO("Return Time : %lf",
>>> static_cast<double>(final_stamp) - srv.response.pong_time);
>>>            ROS_INFO("Round  Trip : %lf",
>>> static_cast<double>(final_stamp - send_stamp) );
>>>            return true;
>>>        } else {
>>>            ROS_ERROR("Failed to call service rpc_latency");
>>>            return false;
>>>        }
>>>    }

>>> private:
>>>    ros::NodeHandle node;
>>>    ros::ServiceClient client;
>>> };

>>> int main(int argc, char **argv) {
>>>    ros::init(argc, argv, "rpc_latency_client");

>>>    LatencyClient latency;
>>>    latency.ping_pong();
>>>    return 0;
>>> }

>>> class LatencyServer {
>>> public:
>>>    LatencyServer() :
>>>        service (node.advertiseService("rpc_latency",
>>> &LatencyServer::ping_pong, this))
>>>    {}

>>>    void spin() {
>>>        ros::spin();
>>>    }
>>>    bool ping_pong(rpc_latency::ping_pong::Request  &request,
>>>                   rpc_latency::ping_pong::Response &response ) {
>>>        response.pong_time = timestamp.stamp();
>>>        ROS_INFO("TimeStamp: %lf", static_cast<double>(timestamp) );
>>>        return true;
>>>    }

>>> private:
>>>    TimeStamp timestamp;
>>>    ros::NodeHandle node;
>>>    ros::ServiceServer service;
>>> };

>>> int main(int argc, char **argv) {
>>>    ros::init(argc, argv, "rpc_latency_server");

>>>    LatencyServer latency_server;
>>>    latency_server.spin();
>>>    return 0;
>>> }