Skip to content

Commit c5d19b3

Browse files
pablogs9JanStaschulat
pablogs9
and
JanStaschulat
authored
Update Services examples (#31)
* Update Services examples * Update rclc/addtwoints_client/main.c Co-authored-by: Jan Staschulat <jan.staschulat@de.bosch.com> * Update rclc/addtwoints_client/main.c Co-authored-by: Jan Staschulat <jan.staschulat@de.bosch.com> * Update rclc/addtwoints_client/main.c Co-authored-by: Jan Staschulat <jan.staschulat@de.bosch.com> * Update rclc/addtwoints_server/main.c Co-authored-by: Jan Staschulat <jan.staschulat@de.bosch.com> * Update rclc/addtwoints_server/main.c Co-authored-by: Jan Staschulat <jan.staschulat@de.bosch.com> * Update Co-authored-by: Jan Staschulat <jan.staschulat@de.bosch.com>
1 parent 24df683 commit c5d19b3

4 files changed

Lines changed: 83 additions & 100 deletions

File tree

‎rclc/addtwoints_client/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,12 +4,14 @@ project(addtwoints_client LANGUAGES C)
44

55
find_package(ament_cmake REQUIRED)
66
find_package(rcl REQUIRED)
7+
find_package(rclc REQUIRED)
78
find_package(example_interfaces REQUIRED)
89

910
add_executable(${PROJECT_NAME} main.c)
1011

1112
ament_target_dependencies(${PROJECT_NAME}
1213
rcl
14+
rclc
1315
example_interfaces
1416
)
1517

‎rclc/addtwoints_client/main.c‎

Lines changed: 34 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,8 @@
11
#include <rcl/rcl.h>
22
#include <rcl/error_handling.h>
3+
#include <rclc/rclc.h>
4+
#include <rclc/executor.h>
5+
36
#include "example_interfaces/srv/add_two_ints.h"
47

58
#include <stdio.h>
@@ -8,70 +11,52 @@
811
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
912
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
1013

14+
example_interfaces__srv__AddTwoInts_Request req;
15+
example_interfaces__srv__AddTwoInts_Response res;
16+
17+
void client_callback(const void * msg, rmw_request_id_t * req_id){
18+
example_interfaces__srv__AddTwoInts_Response * msgin = (example_interfaces__srv__AddTwoInts_Response * ) msg;
19+
printf("Received service response %ld + %ld = %ld. Seq %ld\n", req.a, req.b, msgin->sum, req_id->sequence_number);
20+
}
21+
1122
int main(int argc, const char * const * argv)
1223
{
13-
rcl_init_options_t options = rcl_get_zero_initialized_init_options();
14-
RCCHECK(rcl_init_options_init(&options, rcl_get_default_allocator()))
24+
RCLC_UNUSED(argc);
25+
RCLC_UNUSED(argv);
26+
rcl_allocator_t allocator = rcl_get_default_allocator();
27+
rclc_support_t support;
1528

16-
rcl_context_t context = rcl_get_zero_initialized_context();
17-
RCCHECK(rcl_init(argc, argv, &options, &context))
29+
// create init_options
30+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
1831

19-
rcl_node_options_t node_ops = rcl_node_get_default_options();
20-
rcl_node_t node = rcl_get_zero_initialized_node();
21-
RCCHECK(rcl_node_init(&node, "addtowints_client_rcl", "", &context, &node_ops))
32+
// create node
33+
rcl_node_t node = rcl_get_zero_initialized_node();
34+
RCCHECK(rclc_node_init_default(&node, "add_twoints_client_rclc", "", &support));
2235

23-
const char * client_name = "/addtwoints";
24-
25-
rcl_client_options_t client_options = rcl_client_get_default_options();
36+
// create client
2637
rcl_client_t client = rcl_get_zero_initialized_client();
27-
const rosidl_service_type_support_t * client_type_support = ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);
38+
RCCHECK(rclc_client_init_default(&client, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts), "/addtwoints"));
2839

29-
RCCHECK(rcl_client_init(&client, &node, client_type_support, client_name, &client_options))
40+
// create executor
41+
rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
42+
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
3043

31-
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
32-
RCCHECK(rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, 0, &context, rcl_get_default_allocator()))
44+
unsigned int rcl_wait_timeout = 10; // in ms
45+
RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
46+
RCCHECK(rclc_executor_add_client(&executor, &client, &res, client_callback));
3347

34-
sleep(2); // Sleep a while to ensure DDS matching before sending request
35-
36-
// Send request
3748
int64_t seq;
38-
example_interfaces__srv__AddTwoInts_Request req;
3949
example_interfaces__srv__AddTwoInts_Request__init(&req);
4050
req.a = 24;
4151
req.b = 42;
4252

53+
sleep(2); // Sleep a while to ensure DDS matching before sending request
54+
4355
RCCHECK(rcl_send_request(&client, &req, &seq))
4456
printf("Send service request %ld + %ld. Seq %ld\n",req.a, req.b, seq);
57+
58+
rclc_executor_spin(&executor);
4559

46-
// Wait for response
47-
bool done = false;
48-
do {
49-
RCSOFTCHECK(rcl_wait_set_clear(&wait_set))
50-
51-
size_t index;
52-
RCSOFTCHECK(rcl_wait_set_add_client(&wait_set, &client, &index))
53-
54-
RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(1)))
55-
56-
for (size_t i = 0; i < wait_set.size_of_clients; i++) {
57-
if (wait_set.clients[0]) {
58-
rmw_request_id_t req_id;
59-
example_interfaces__srv__AddTwoInts_Response res;
60-
example_interfaces__srv__AddTwoInts_Response__init(&res);
61-
62-
rcl_ret_t rc = rcl_take_response(&client, &req_id, &res);
63-
64-
if (RCL_RET_OK == rc) {
65-
printf("Received service response %ld + %ld = %ld. Seq %ld\n",req.a, req.b, res.sum, req_id.sequence_number);
66-
done = true;
67-
}
68-
}
69-
}
70-
} while ( !done );
71-
72-
RCCHECK(rcl_client_fini(&client,&node))
73-
RCCHECK(rcl_node_fini(&node))
74-
75-
return 0;
60+
RCCHECK(rcl_client_fini(&client, &node));
61+
RCCHECK(rcl_node_fini(&node));
7662
}
77-

‎rclc/addtwoints_server/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,12 +4,14 @@ project(addtwoints_server LANGUAGES C)
44

55
find_package(ament_cmake REQUIRED)
66
find_package(rcl REQUIRED)
7+
find_package(rclc REQUIRED)
78
find_package(example_interfaces REQUIRED)
89

910
add_executable(${PROJECT_NAME} main.c)
1011

1112
ament_target_dependencies(${PROJECT_NAME}
1213
rcl
14+
rclc
1315
example_interfaces
1416
)
1517

‎rclc/addtwoints_server/main.c‎

Lines changed: 45 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,8 @@
11
#include <rcl/rcl.h>
22
#include <rcl/error_handling.h>
3+
#include <rclc/rclc.h>
4+
#include <rclc/executor.h>
5+
36
#include "example_interfaces/srv/add_two_ints.h"
47

58
#include <stdio.h>
@@ -8,57 +11,48 @@
811
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
912
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
1013

14+
example_interfaces__srv__AddTwoInts_Request req;
15+
example_interfaces__srv__AddTwoInts_Response res;
16+
17+
void service_callback(const void * req, rmw_request_id_t * req_id, void * res){
18+
example_interfaces__srv__AddTwoInts_Request * req_in = (example_interfaces__srv__AddTwoInts_Request *) req;
19+
example_interfaces__srv__AddTwoInts_Response * res_in = (example_interfaces__srv__AddTwoInts_Response *) res;
20+
21+
printf("Service request value: %d + %d. Seq %d\n", (int) req_in->a, (int) req_in->b, (int) req_id->sequence_number);
22+
23+
res_in->sum = req_in->a + req_in->b;
24+
}
25+
26+
1127
int main(int argc, const char * const * argv)
1228
{
13-
rcl_init_options_t options = rcl_get_zero_initialized_init_options();
14-
RCCHECK(rcl_init_options_init(&options, rcl_get_default_allocator()))
15-
16-
rcl_context_t context = rcl_get_zero_initialized_context();
17-
RCCHECK(rcl_init(argc, argv, &options, &context))
18-
19-
rcl_node_options_t node_ops = rcl_node_get_default_options();
20-
rcl_node_t node = rcl_get_zero_initialized_node();
21-
RCCHECK(rcl_node_init(&node, "addtowints_server_rcl", "", &context, &node_ops))
22-
23-
const char * service_name = "addtwoints";
24-
rcl_service_options_t service_op = rcl_service_get_default_options();
25-
rcl_service_t serv = rcl_get_zero_initialized_service();
26-
const rosidl_service_type_support_t * service_type_support = ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts);
27-
28-
RCCHECK(rcl_service_init(&serv,&node, service_type_support, service_name, &service_op))
29-
30-
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
31-
RCCHECK(rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, &context, rcl_get_default_allocator()))
32-
33-
do {
34-
RCSOFTCHECK(rcl_wait_set_clear(&wait_set))
35-
36-
size_t index;
37-
RCSOFTCHECK(rcl_wait_set_add_service(&wait_set, &serv, &index))
38-
39-
rcl_wait(&wait_set, RCL_MS_TO_NS(1));
40-
for (size_t i = 0; i < wait_set.size_of_services; i++) {
41-
if (wait_set.services[i]) {
42-
rmw_request_id_t req_id;
43-
example_interfaces__srv__AddTwoInts_Request req;
44-
example_interfaces__srv__AddTwoInts_Request__init(&req);
45-
RCSOFTCHECK(rcl_take_request(&serv,&req_id,&req))
46-
47-
printf("Service request value: %d + %d. Seq %d\n", (int)req.a, (int)req.b, (int) req_id.sequence_number);
48-
49-
example_interfaces__srv__AddTwoInts_Response res;
50-
example_interfaces__srv__AddTwoInts_Response__init(&res);
51-
52-
res.sum = req.a + req.b;
53-
54-
RCSOFTCHECK(rcl_send_response(&serv,&req_id,&res))
55-
}
56-
}
57-
} while ( true );
58-
59-
RCCHECK(rcl_service_fini(&serv,&node))
60-
RCCHECK(rcl_node_fini(&node))
61-
62-
return 0;
63-
}
29+
RCLC_UNUSED(argc);
30+
RCLC_UNUSED(argv);
31+
rcl_allocator_t allocator = rcl_get_default_allocator();
32+
rclc_support_t support;
6433

34+
// create init_options
35+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
36+
37+
// create node
38+
rcl_node_t node = rcl_get_zero_initialized_node();
39+
RCCHECK(rclc_node_init_default(&node, "add_twoints_client_rclc", "", &support));
40+
41+
// create service
42+
rcl_service_t service = rcl_get_zero_initialized_service();
43+
RCCHECK(rclc_service_init_default(&service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts), "/addtwoints"));
44+
45+
// create executor
46+
rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
47+
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
48+
49+
unsigned int rcl_wait_timeout = 10; // in ms
50+
RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
51+
52+
RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, service_callback));
53+
54+
rclc_executor_spin(&executor);
55+
56+
RCCHECK(rcl_service_fini(&service, &node));
57+
RCCHECK(rcl_node_fini(&node));
58+
}

0 commit comments

Comments
 (0)