root / quad2 / arduino / src / ros_lib / examples / ServiceClient / ServiceClient.pde @ c1426757
History | View | Annotate | Download (707 Bytes)
1 | c1426757 | Tom Mullins | /* |
---|---|---|---|
2 | * rosserial Service Client |
||
3 | */ |
||
4 | |||
5 | #include <ros.h> |
||
6 | #include <std_msgs/String.h> |
||
7 | #include <rosserial_arduino/Test.h> |
||
8 | |||
9 | ros::NodeHandle nh; |
||
10 | using rosserial_arduino::Test; |
||
11 | |||
12 | ros::ServiceClient<Test::Request, Test::Response> client("test_srv"); |
||
13 | |||
14 | std_msgs::String str_msg; |
||
15 | ros::Publisher chatter("chatter", &str_msg); |
||
16 | |||
17 | char hello[13] = "hello world!"; |
||
18 | |||
19 | void setup() |
||
20 | { |
||
21 | nh.initNode(); |
||
22 | nh.serviceClient(client); |
||
23 | nh.advertise(chatter); |
||
24 | while(!nh.connected()) nh.spinOnce(); |
||
25 | nh.loginfo("Startup complete"); |
||
26 | } |
||
27 | |||
28 | void loop() |
||
29 | { |
||
30 | Test::Request req; |
||
31 | Test::Response res; |
||
32 | req.input = hello; |
||
33 | client.call(req, res); |
||
34 | str_msg.data = res.output; |
||
35 | chatter.publish( &str_msg ); |
||
36 | nh.spinOnce(); |
||
37 | delay(100); |
||
38 | } |