Skip to content
This repository was archived by the owner on Jun 21, 2023. It is now read-only.

Commit 983ed01

Browse files
JanStaschulatJanStaschulat
committed
changed in testbench: one timer per publisher
Signed-off-by: Staschulat Jan <jan.staschulat@de.bosch.com>
1 parent 948270a commit 983ed01

2 files changed

Lines changed: 141 additions & 62 deletions

File tree

‎rcl_executor_testbench/src/create_testbench.sh‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#!/bin/bash
22
echo "Creating testbench"
33
BINARY=/home/jst3si/temp/ros2_ws/install/rcl_executor_testbench/lib/rcl_executor_testbench/rcl_executor_testbench
4+
GENERATOR_BIN=/home/jst3si/temp/ros2_ws/src/rcl_executor/rcl_executor_testbench/src/configure_test.py
45
RATE=1
56
MSG_SIZE=2
67
TOPOLOGY=F
@@ -10,5 +11,5 @@ NUM_SUBS=10
1011

1112

1213
echo script $BINARY $NUM_TOPICS $TOPOLOGY
13-
python3 ./configure_test.py $BINARY $RATE $MSG_SIZE $TOPOLOGY $PROCESS_TYPE $NUM_TOPICS $NUM_SUBS
14+
python3 $GENERATOR_BIN $BINARY $RATE $MSG_SIZE $TOPOLOGY $PROCESS_TYPE $NUM_TOPICS $NUM_SUBS
1415

‎rcl_executor_testbench/src/let_exec_bm.c‎

Lines changed: 139 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -18,20 +18,83 @@
1818
#include <std_msgs/msg/string.h>
1919
// #include <unistd.h> // for usleep
2020

21+
/* TODO
22+
- one timer for each publisher
23+
- finish after number of published /subscribed topics (command line argument and logic)
24+
- documentation
25+
26+
27+
Idea:
28+
- call spin_some() while (!testbench_finished() )
29+
- in timer increment # published topics
30+
- in subscription_callback count # received topics
31+
- testbench_finished() {
32+
bool finished_publish = true;
33+
bool finished_subscribed = true;
34+
if (num_pubs > 0) {
35+
for(unsigned int i=0; i< num_nodes && finished_publish;i++) {
36+
for(unsigned int p=0; p<nodes[n].num_pubs;p++){
37+
if (nodes[n].pubs[p].published_topics < num_topics) {
38+
finished_publish = false;
39+
break;
40+
}
41+
}
42+
}
43+
44+
if (num_subs > 0) {
45+
for(unsigned int i=0; i< num_nodes && finished_subscribed;i++) {
46+
for(unsigned int s=0; s<nodes[n].num_subs;s++){
47+
if (nodes[n].subs[s].received_topics < num_topics) {
48+
finished_publish = false;
49+
break;
50+
}
51+
}
52+
}
53+
54+
55+
vielleicht ist es besser, beim senden/empfangen direkt hochzuzählen
56+
wenn num < max => senden, sonst nicht mehr und Flag im Ergebnis-Vektor speichern
57+
nodes[i].subs[]
58+
59+
1 globale variable - wird incrementiert, wenn ein teilnehmer fertig ist (fürs senden)
60+
publisher_finished++; => wenn max number of published topics erreicht wurde
61+
subscriber_finsihed++;
62+
dann ist der check ganz einfach:
63+
64+
// init
65+
unsigned int publisher_finished = 0;
66+
unsigned int subscriber_finished = 0;
67+
68+
// geht auch für den fall conf.num_pubs oder conf.num_subs==0
69+
int work_to_do() {}
70+
bool finished = false;
71+
if ((publisher_finished == conf.num_pubs) &&
72+
(subscriber_finished == conf.num_subs)
73+
)
74+
finished = true;
75+
}
76+
return !finished;
77+
}
78+
79+
while( work_to_do() )
80+
{
81+
executor.spin_some();
82+
}
83+
*/
84+
2185
/* helper data structure for rcl interface
2286
to save the node with its publishers and subscribers
2387
*/
24-
2588
typedef struct {
2689
rcl_node_t * rcl_node; // rcl_node
2790
unsigned int id; // node id
2891
unsigned int num_pubs; // number of publishers
2992
unsigned int num_subs; // number of subscribers
30-
rcl_publisher_t ** pubs; // list of publishers
31-
unsigned int * pub_names; // list of topic ids of the publishers
32-
rcl_subscription_t ** subs; // list of subscribers
93+
rcl_publisher_t ** pubs; // list of publishers (length = num_pubs)
94+
rcl_timer_t ** timers; // list of timers (length = num_pubs)
95+
unsigned int * pub_names; // list of topic ids of the publishers (length = num_pubs)
96+
rcl_subscription_t ** subs; // list of subscribers (length = num_subs)
3397
std_msgs__msg__String ** subs_msg; // list of msgs of the subscribers
34-
rcl_timer_t * timer; // timer for publishers
3598
} rcl_node_wrapper_t;
3699

37100

@@ -202,7 +265,6 @@ static rcl_node_wrapper_t * nodes; // list of nodes
202265
static unsigned int num_nodes; // necessary in timer_callback
203266
std_msgs__msg__String pub_msg; // one message string for all publishers with a configured message length
204267

205-
206268
/***************************** CALLBACKS ***********************************/
207269

208270

@@ -226,36 +288,40 @@ timer_callback(rcl_timer_t * timer, int64_t last_call_time)
226288
rcl_ret_t rc;
227289
UNUSED(last_call_time);
228290
if (timer != NULL) {
229-
//printf("Timer: time since last call %d\n", (int) last_call_time);
230-
231-
// find corresponding node and publish its messages
232-
for(unsigned int n = 0; n<num_nodes; n++) {
233-
//printf("Timer callback: searching timer in node %u\n", n);
234-
if (nodes[n].timer == timer) {
235-
for(unsigned int p=0; p<nodes[n].num_pubs;p++) {
236-
rc = rcl_publish(nodes[n].pubs[p], &pub_msg, NULL);
291+
// printf("Timer: time since last call %d\n", (int) last_call_time);
292+
293+
// find corresponding timer and publish its messages
294+
// in each node n the list of publishers is mirrored by the list of timers:
295+
// the timer nodes[n].timers[i] corresponds to the publisher nodes[n].pubs[i]
296+
bool found = false;
297+
for(unsigned int n = 0; n<num_nodes && found == false; n++) {
298+
// printf("Timer callback: searching timer in node %u\n", n);
299+
for(unsigned int i=0; i<nodes[n].num_pubs && found == false; i++) {
300+
if (nodes[n].timers[i] == timer) {
301+
found = true;
302+
rc = rcl_publish(nodes[n].pubs[i], &pub_msg, NULL);
303+
304+
// debug output
237305
if (rc != RCL_RET_OK) {
238-
printf("Error publishing message node[%u].pub[%u]\n", n, p);
306+
printf("Error publishing message node[%u].pub[%u]\n", n, i);
239307
} else {
240-
printf("node %u: published topic_%u\n", nodes[n].id, nodes[n].pub_names[p]);
308+
printf("node %u: published topic_%u\n", nodes[n].id, nodes[n].pub_names[i]);
241309
}
242310
}
243-
break;
244311
}
245312
}
246313
} else {
247314
printf("Error: timer_callback is NULL\n");
248315
}
249316
}
317+
250318
/******************** MAIN PROGRAM *****************************************/
251-
int main(int argc, const char * argv[])
252-
{
319+
int main(int argc, const char * argv[]) {
253320
rcl_ret_t rc;
254321
conf_t conf;
255322

256-
257323
////////////////////////////////////////////////////////////////////////////
258-
// Read configuration
324+
// Parse configuration
259325
////////////////////////////////////////////////////////////////////////////
260326
if( parse_args( argc, argv, &conf) != 0) {
261327
printf("Error while parsing arguments.\n");
@@ -277,7 +343,7 @@ int main(int argc, const char * argv[])
277343
}
278344

279345
////////////////////////////////////////////////////////////////////////////
280-
// Configure nodes
346+
// Configure rcl handles (nodes, publishers, timers, subscriptions)
281347
////////////////////////////////////////////////////////////////////////////
282348

283349
// create fixed strings for node_name and topic_name
@@ -307,26 +373,22 @@ int main(int argc, const char * argv[])
307373
nodes[n].num_pubs = conf.nodes[n]->num_pubs;
308374
nodes[n].pubs = calloc ( conf.nodes[n]->num_pubs, sizeof(rcl_publisher_t*));
309375
nodes[n].pub_names = calloc ( conf.nodes[n]->num_pubs, sizeof(unsigned int));
376+
nodes[n].timers = calloc ( conf.nodes[n]->num_pubs, sizeof(rcl_timer_t*));
310377

311378
for(unsigned int p = 0; p < conf.nodes[n]->num_pubs; p++) {
312379
snprintf(topic_name, TOPIC_NAME_SIZE, "topic_%u", conf.nodes[n]->pub_names[p]);
313380
printf(" ... publishes %s\n", topic_name);
314-
381+
nodes[n].pub_names[p] = conf.nodes[n]->pub_names[p];
315382
nodes[n].pubs[p] = rcl_create_publisher_wrapper(nodes[n].rcl_node,
316383
init_obj.allocator, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
317384
topic_name);
318-
nodes[n].pub_names[p] = conf.nodes[n]->pub_names[p];
319385
if (nodes[n].pubs[p] == NULL) {
320386
printf("Error: Could not create publisher %s.\n", topic_name);
321387
return -1;
322-
}
388+
}
389+
nodes[n].timers[p] = rcl_create_timer_wrapper(&init_obj,
390+
RCL_MS_TO_NS(conf.period), timer_callback);
323391
}
324-
325-
// add one timer (to publish all messages of this node)
326-
nodes[n].timer = rcl_create_timer_wrapper(&init_obj,
327-
RCL_MS_TO_NS(conf.period), timer_callback);
328-
} else {
329-
nodes[n].timer = NULL; // properly initialized, is expected in executor (see below)
330392
}
331393

332394
// add subscriptions
@@ -387,50 +449,66 @@ int main(int argc, const char * argv[])
387449
printf("Error in rcle_let_executor_set_timeout.");
388450
}
389451

390-
// add subscriptions and timers
391452
for(unsigned int n = 0; n<conf.num_nodes; n++){
392-
for(unsigned int s=0; s<nodes[n].num_subs;s++) {
393-
rc = rcle_let_executor_add_subscription(&exe, nodes[n].subs[s], nodes[n].subs_msg[s],
394-
&subscriber_callback, ON_NEW_DATA);
453+
// add one timer per publisher
454+
for(unsigned int p=0; p<nodes[n].num_pubs;p++) {
455+
rc = rcle_let_executor_add_timer(&exe, nodes[n].timers[p]);
395456
if (rc != RCL_RET_OK) {
396-
printf("Error in rcle_let_executor_add_subscription node[%u] sub %u \n", n, s);
457+
printf("Error rcle_let_executor_add_timer: Could not add timer to executor: nodes[%u].timers[%u]\n",n, p);
397458
}
398459
}
399460

400-
// when node has publishers then add the timer
401-
if (nodes[n].timer != NULL) {
402-
rc = rcle_let_executor_add_timer(&exe, nodes[n].timer);
461+
// add subscription
462+
for(unsigned int s=0; s<nodes[n].num_subs;s++) {
463+
rc = rcle_let_executor_add_subscription(&exe, nodes[n].subs[s], nodes[n].subs_msg[s],
464+
&subscriber_callback, ON_NEW_DATA);
403465
if (rc != RCL_RET_OK) {
404-
printf("Error rcle_let_executor_add_timer: Could not add timer to executor\n");
466+
printf("Error in rcle_let_executor_add_subscription node[%u] sub %u \n", n, s);
405467
}
406468
}
407469
}
408-
409-
// spin forever
470+
////////////////////////////////////////////////////////////////////////////
471+
// Run RCL Executor
472+
////////////////////////////////////////////////////////////////////////////
410473
rcle_let_executor_spin(&exe);
411474

412-
// clean up
475+
////////////////////////////////////////////////////////////////////////////
476+
// Clean up - free dynamically allocated memory
477+
////////////////////////////////////////////////////////////////////////////
478+
/*
479+
// executor
413480
rc = rcle_let_executor_fini(&exe);
414-
415-
/* TODO clean up
416-
for (unsigned int i = 0 ; i < NUM_PUBLISHER; i++) {
417-
rc = rcl_publisher_fini_wrapper(&init_obj, pubs[i], node0);
418-
}
419481
420-
for (unsigned int i = 0 ; i < NUM_SUBSCRIBER; i++) {
421-
rc = rcl_subscription_fini_wrapper(&init_obj, subs[i], node0);
422-
}
482+
// rcl handles
483+
for (unsigned int n = 0; n < conf.num_nodes; n++) {
484+
// delete memory of publishers and timers
485+
if ( conf.nodes[n]->num_pubs > 0) {
486+
for(unsigned int i = 0; i < conf.nodes[n]->num_pubs; i++) {
487+
rc = rcl_publisher_fini_wrapper(&init_obj, nodes[n].pubs[i], nodes[n].rcl_node);
488+
rc = rcl_timer_fini_wrapper(&init_obj, nodes[n].timers[i]);
489+
}
490+
free (nodes[n].pubs);
491+
free (nodes[n].pub_names);
492+
free (nodes[n].timers);
493+
}
423494
424-
for (unsigned int i = 0 ; i < NUM_SUBSCRIBER; i++) {
425-
init_obj.allocator->deallocate( subs_msg[i], init_obj.allocator->state);
495+
// delete memory of subscriptions
496+
if ( conf.nodes[n]->num_subs > 0) {
497+
for(unsigned int i = 0; i < conf.nodes[n]->num_subs; i++) {
498+
rc = rcl_subscription_fini_wrapper(&init_obj, nodes[n].subs[i], nodes[n].rcl_node);
499+
init_obj.allocator->deallocate(nodes[n].subs_msg[i], init_obj.allocator->state);
500+
}
501+
free(nodes[n].subs);
502+
free(nodes[n].subs_msg);
503+
}
504+
// delete memory of node
505+
rc = rcl_node_fini_wrapper(&init_obj, nodes[n].rcl_node);
426506
}
427-
428-
rc = rcl_timer_fini_wrapper(&init_obj, timer1);
429-
rc = rcl_node_fini_wrapper(&init_obj, node0);
430-
rc = rcl_init_options_fini(&init_obj);
431-
*/
507+
free (pub_string);
508+
rc = rcl_init_fini_wrapper(&init_obj);
509+
free (nodes);
510+
// configuration object (command line arguments)
432511
configuration_fini( &conf);
433-
512+
*/
434513
return 0;
435-
436-
}
514+
}

0 commit comments

Comments
 (0)