Colony » Colony Scout » Colony Scout OS

#include <ros/ros.h>

#include <encoders/query_encoders.h>

int main(int argc, char **argv)

{

    ros::init(argc, argv, "test_encoders");

    ros::NodeHandle node;

    ros::ServiceClient query_encoders_client =

        node.serviceClient<encoders::query_encoders>("/scout1/query_encoders");

    ros::Rate r(10);

    while (ros::ok())

    {

        encoders::query_encoders srv;

        if (query_encoders_client.call(srv))

        {

            printf("Got encoders: fl:%u, fr:%u, bl:%u, br:%u).\n",

                srv.response.fl_distance,

                srv.response.fr_distance,

                srv.response.bl_distance,

                srv.response.br_distance);

        }

        else

        {

            printf("ERROR: Failed to call service.\n");

        }

        ros::spinOnce();

        r.sleep();

    }

    return 0;

}