From e63b644f37bcfb1378afa3b44372b814d936298d Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Tue, 7 Feb 2023 18:09:22 +0100 Subject: [PATCH] updated documentation (#332) Signed-off-by: Jan Staschulat --- rclc_examples/CMakeLists.txt | 6 +- rclc_examples/README.md | 130 +++++++++--------- rclc_examples/src/example_executor.c | 91 +++++------- ...venience.c => example_executor_only_rcl.c} | 89 +++++++----- 4 files changed, 154 insertions(+), 162 deletions(-) rename rclc_examples/src/{example_executor_convenience.c => example_executor_only_rcl.c} (70%) diff --git a/rclc_examples/CMakeLists.txt b/rclc_examples/CMakeLists.txt index 32815fa5..c45fc9bd 100644 --- a/rclc_examples/CMakeLists.txt +++ b/rclc_examples/CMakeLists.txt @@ -33,8 +33,8 @@ include_directories(include) add_executable(example_executor src/example_executor.c) ament_target_dependencies(example_executor rcl rclc std_msgs) -add_executable(example_executor_convenience src/example_executor_convenience.c) -ament_target_dependencies(example_executor_convenience rcl rclc std_msgs) +add_executable(example_executor_only_rcl src/example_executor_only_rcl.c) +ament_target_dependencies(example_executor_only_rcl rcl rclc std_msgs) add_executable(example_executor_trigger src/example_executor_trigger.c) ament_target_dependencies(example_executor_trigger rcl rclc std_msgs) @@ -69,7 +69,7 @@ ament_target_dependencies(example_short_timer_long_subscription rcl rclc std_msg install(TARGETS example_executor - example_executor_convenience + example_executor_only_rcl example_executor_trigger example_lifecycle_node example_service_node diff --git a/rclc_examples/README.md b/rclc_examples/README.md index 146f0dd4..9df83471 100644 --- a/rclc_examples/README.md +++ b/rclc_examples/README.md @@ -2,13 +2,15 @@ General information about this repository, including legal information, build in # The rclc_examples package -The rclc_examples package provides examples for using the RCLC-Exector and convenience functions. -- [example_executor.c](src/example_executor.c) provides the example for the RCLC-Executor. It creates one publisher and one subscriber and configures the RCLC-Executor accordingly. Then the spin_some() function is demonstrated. -- [example_executor_convenience.c](src/example_executor_convenience.c) provides the example for the RCLC-Executor with the convenience functions from rclc. It creates one publisher and one subscriber and configures the RCLC-Executor accordingly. Then the spin_some() function is demonstrated. +The rclc_examples package provides examples for using the RCLC-Exector and convenience functions for creating RCL objects like subscriptions and timers. + +- [example_executor.c](src/example_executor.c) provides the example for the RCLC-Executor with the convenience functions from rclc. It creates one publisher and one subscriber and configures the RCLC-Executor accordingly. - [example_executor_trigger.c](src/example_executor_trigger.c) demonstrates the trigger condition of the RCLC-Executor. - [example_service_node.c](src/example_service_node.c) implements a service node with the RCLC-Executor. - [example_client_node.c](src/example_client_node.c) implements a client node with RCLC-Executor. -- [example_short_timer_long_subscription.c](src/example_client_node.c) demo with high frequency timer and subscription with long processing time with one executor. +- [example_executor_only_rcl.c](src/example_executor_only_rcl.c) provides the example for the RCLC-Executor. It creates one publisher and one subscriber and configures the RCLC-Executor using only the RCL API. +- [example_short_timer_long_subscription.c](src/example_short_timer_long_subscription.c) demo with high frequency timer and subscription with long processing time with one executor. + The reduction of code lines for configuring the necessary RCL objects for RCLC-Executor directly with RCL objects compared to using the convenience functions is about 24%: - example_executor.c: 92 LoC (lines 56-148) @@ -16,85 +18,30 @@ The reduction of code lines for configuring the necessary RCL objects for RCLC-E counting only the lines of code in which the RCL objects are defined). -## Example RCLC-Executor using RCL objects directly +## Example RCLC-Executor **Step 1** Setup ROS 2 Workspace Open a terminal with ROS 2 workspace. Assuming that the ROS 2 installation resides in `/opt/ros/ROSDISTRO`, setup the ROS2 environment by: ```C -~$ source /opt/ros/$ROSDISTRO/setup.bash +~$ source /opt/ros/ROSDISTRO/setup.bash ``` **Step 2** Build the package -Download and build the the packages `rclc` and `rclc_examples` in a workspace (for example `ros2_ws`). Then source the workspace: +Download and build the rclc repository in a workspace (for example `ros2_ws`). Then source the workspace: ```C ~/ros2_ws/$ colcon build --packages-up-to rclc_examples ~/ros2_ws/$ source ./install/local_setup.bash ``` -It should build these packages: -- rcl_yaml_param_parser -- rcl -- rclc -- rclc_examples - -**Step 3** Run the example executor. +**Step 3** Run the example executor demo. The binary of the example is `example_executor`. ```C -~/ros2_ws/$ ros2 run rclc_examples example_executor -``` -The publisher publishes the message `Hello World!`in `topic_0` at a rate of 1Hz and the subscriber prints out in the callback `Callback: I heard: Hello World!`. - -You should see the following output: - -```C -Created timer with timeout 1000 ms. -Created subscriber topic_0: -Debug: number of DDS handles: 2 -Published message Hello World! -Callback: I heard: Hello World! -Published message Hello World! -Callback: I heard: Hello World! -Published message Hello World! -Callback: I heard: Hello World! -Published message Hello World! -Callback: I heard: Hello World! -Published message Hello World! -Callback: I heard: Hello World! -``` - - - -## Example RCLC-Executor with convenience functions -**Step 1** Setup ROS 2 Workspace - -Open a terminal with ROS 2 workspace. Assuming that the ROS 2 installation resides in `/opt/ros/eloquent`, setup -the ROS2 environment by: -```C -~$ source /opt/ros/eloquent/setup.bash +~/ros2_ws/$ ros2 run rclc_examples example_executor ``` -**Step 2** Build the package -Download and build the the packages `rclc` and `rclc_examples` in a workspace (for example `ros2_ws`). Then source the workspace: -```C -~/ros2_ws/$ colcon build --packages-up-to rclc_examples -~/ros2_ws/$ source ./install/local_setup.bash -``` -It should build these packages: -- rcl_yaml_param_parser -- rcl -- rclc -- rclc_examples - -**Step 3** Run the example executor with the convenience functions from the package rclc. - -The binary of the example is `example_executor_convenience`. - -```C -~/ros2_ws/$ ros2 run rclc_examples example_executor_convenience -``` -The same setup as in the example_executor, just using the RCLC convenience functions. You should see the exact same output: +Example output: ```C Created timer with timeout 1000 ms. @@ -196,18 +143,65 @@ $ ros2 run rclc_examples example_service_node INFO: rcl_wait timeout 10 ms Service request value: 24 + 42. Seq 1 Received service response 24 + 42 = 66. Seq 1 -```C +``` window 2: start client node ```C ~$ ros2 run rclc_examples example_client_node Send service request 24 + 42. Seq 1 INFO: rcl_wait timeout 10 ms -```C +``` A request message is sent from the client node to the service node and answered. +## Example RCLC-Executor using RCL objects directly +**Step 1** Setup ROS 2 Workspace + +Open a terminal with ROS 2 workspace. Assuming that the ROS 2 installation resides in `/opt/ros/ROSDISTRO`, setup the ROS2 environment by: +```C +~$ source /opt/ros/$ROSDISTRO/setup.bash +``` + +**Step 2** Build the package +Download the rclc repository in a workspace (for example `ros2_ws`). Then source the workspace: +```C +~/ros2_ws/$ colcon build --packages-up-to rclc_examples +~/ros2_ws/$ source ./install/local_setup.bash +``` +It should build these packages: +- rcl_yaml_param_parser +- rcl +- rclc +- rclc_examples + + +**Step 3** Run the example executor. + +The binary of the example is `example_executor_only_rcl`. + +```C +~/ros2_ws/$ ros2 run rclc_examples example_executor_only_rcl +``` +The publisher publishes the message `Hello World!`in `topic_0` at a rate of 1Hz and the subscriber prints out in the callback `Callback: I heard: Hello World!`. + +You should see the following output: + +```C +Created timer with timeout 1000 ms. +Created subscriber topic_0: +Debug: number of DDS handles: 2 +Published message Hello World! +Callback: I heard: Hello World! +Published message Hello World! +Callback: I heard: Hello World! +Published message Hello World! +Callback: I heard: Hello World! +Published message Hello World! +Callback: I heard: Hello World! +Published message Hello World! +Callback: I heard: Hello World! +``` ## Example real-time concurrency slow timer and long subscription This example demonstrates what happens, if a high frequency timer (every 100ms) and a subscription with a long processing time is managed by one executor. This demo shows, -that the timer events are dropped during the long processing time of the subscription and are also not caught-up when there would be sufficient time. +that the timer events are dropped during the long processing time of the subscription and are also not caught-up when there would be sufficient time. \ No newline at end of file diff --git a/rclc_examples/src/example_executor.c b/rclc_examples/src/example_executor.c index b966fdb7..5e3bd636 100644 --- a/rclc_examples/src/example_executor.c +++ b/rclc_examples/src/example_executor.c @@ -14,9 +14,9 @@ // limitations under the License. #include - -#include #include +#include +#include // these data structures for the publisher and subscriber are global, so that // they can be configured in main() and can be used in the corresponding callback. @@ -56,31 +56,22 @@ void my_timer_callback(rcl_timer_t * timer, int64_t last_call_time) /******************** MAIN PROGRAM ****************************************/ int main(int argc, const char * argv[]) { - rcl_context_t context = rcl_get_zero_initialized_context(); - rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); rcl_allocator_t allocator = rcl_get_default_allocator(); + rclc_support_t support; rcl_ret_t rc; // create init_options - rc = rcl_init_options_init(&init_options, allocator); + rc = rclc_support_init(&support, argc, argv, &allocator); if (rc != RCL_RET_OK) { - printf("Error rcl_init_options_init.\n"); - return -1; - } - - // create context - rc = rcl_init(argc, argv, &init_options, &context); - if (rc != RCL_RET_OK) { - printf("Error in rcl_init.\n"); + printf("Error rclc_support_init.\n"); return -1; } // create rcl_node rcl_node_t my_node = rcl_get_zero_initialized_node(); - rcl_node_options_t node_ops = rcl_node_get_default_options(); - rc = rcl_node_init(&my_node, "node_0", "executor_examples", &context, &node_ops); + rc = rclc_node_init_default(&my_node, "node_0", "executor_examples", &support); if (rc != RCL_RET_OK) { - printf("Error in rcl_node_init\n"); + printf("Error in rclc_node_init_default\n"); return -1; } @@ -89,36 +80,27 @@ int main(int argc, const char * argv[]) const char * topic_name = "topic_0"; const rosidl_message_type_support_t * my_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String); - rcl_publisher_options_t pub_options = rcl_publisher_get_default_options(); - rc = rcl_publisher_init( + + rc = rclc_publisher_init_default( &my_pub, &my_node, my_type_support, - topic_name, - &pub_options); + topic_name); if (RCL_RET_OK != rc) { - printf("Error in rcl_publisher_init %s.\n", topic_name); + printf("Error in rclc_publisher_init_default %s.\n", topic_name); return -1; } - // create a timer, which will call the publisher every 'period' ms in the 'my_timer_callback' - rcl_clock_t clock; - rc = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); - if (rc != RCL_RET_OK) { - printf("Error in rcl_clock_init.\n"); - return -1; - } + // create a timer, which will call the publisher with period=`timer_timeout` ms in the 'my_timer_callback' rcl_timer_t my_timer = rcl_get_zero_initialized_timer(); const unsigned int timer_timeout = 1000; - rc = rcl_timer_init( + rc = rclc_timer_init_default( &my_timer, - &clock, - &context, + &support, RCL_MS_TO_NS(timer_timeout), - my_timer_callback, - allocator); + my_timer_callback); if (rc != RCL_RET_OK) { - printf("Error in rcl_timer_init.\n"); + printf("Error in rcl_timer_init_default.\n"); return -1; } else { printf("Created timer with timeout %d ms.\n", timer_timeout); @@ -134,16 +116,11 @@ int main(int argc, const char * argv[]) // create subscription rcl_subscription_t my_sub = rcl_get_zero_initialized_subscription(); - rcl_subscription_options_t my_subscription_options = rcl_subscription_get_default_options(); - - - rc = rcl_subscription_init( + rc = rclc_subscription_init_default( &my_sub, &my_node, my_type_support, - topic_name, - &my_subscription_options); - + topic_name); if (rc != RCL_RET_OK) { printf("Failed to create subscriber %s.\n", topic_name); return -1; @@ -158,18 +135,17 @@ int main(int argc, const char * argv[]) // Configuration of RCL Executor //////////////////////////////////////////////////////////////////////////// rclc_executor_t executor; - - // compute total number of subsribers and timers + executor = rclc_executor_get_zero_initialized_executor(); + // total number of handles = #subscriptions + #timers + // + // Note: + // If you need more than the default number of publisher/subscribers, etc., you + // need to configure the micro-ROS middleware also! + // See documentation in the executor.h at the function rclc_executor_init() + // for more details. unsigned int num_handles = 1 + 1; printf("Debug: number of DDS handles: %u\n", num_handles); - rclc_executor_init(&executor, &context, num_handles, &allocator); - - // set timeout for rcl_wait() - unsigned int rcl_wait_timeout = 1000; // in ms - rc = rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)); - if (rc != RCL_RET_OK) { - printf("Error in rclc_executor_set_timeout."); - } + rclc_executor_init(&executor, &support.context, num_handles, &allocator); // add subscription to executor rc = rclc_executor_add_subscription( @@ -184,21 +160,16 @@ int main(int argc, const char * argv[]) printf("Error in rclc_executor_add_timer.\n"); } - // Optional prepare for avoiding allocations during spin - rclc_executor_prepare(&executor); + rclc_executor_spin(&executor); - for (unsigned int i = 0; i < 10; i++) { - // timeout specified in ns (here 1s) - rclc_executor_spin_some(&executor, 1000 * (1000 * 1000)); - } - - // clean up + // clean up (never called in this example) rc = rclc_executor_fini(&executor); rc += rcl_publisher_fini(&my_pub, &my_node); rc += rcl_timer_fini(&my_timer); rc += rcl_subscription_fini(&my_sub, &my_node); rc += rcl_node_fini(&my_node); - rc += rcl_init_options_fini(&init_options); + rc += rclc_support_fini(&support); + std_msgs__msg__String__fini(&pub_msg); std_msgs__msg__String__fini(&sub_msg); diff --git a/rclc_examples/src/example_executor_convenience.c b/rclc_examples/src/example_executor_only_rcl.c similarity index 70% rename from rclc_examples/src/example_executor_convenience.c rename to rclc_examples/src/example_executor_only_rcl.c index 51f887f6..d37213b7 100644 --- a/rclc_examples/src/example_executor_convenience.c +++ b/rclc_examples/src/example_executor_only_rcl.c @@ -14,9 +14,9 @@ // limitations under the License. #include -#include + #include -#include +#include // these data structures for the publisher and subscriber are global, so that // they can be configured in main() and can be used in the corresponding callback. @@ -56,22 +56,31 @@ void my_timer_callback(rcl_timer_t * timer, int64_t last_call_time) /******************** MAIN PROGRAM ****************************************/ int main(int argc, const char * argv[]) { + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); rcl_allocator_t allocator = rcl_get_default_allocator(); - rclc_support_t support; rcl_ret_t rc; // create init_options - rc = rclc_support_init(&support, argc, argv, &allocator); + rc = rcl_init_options_init(&init_options, allocator); if (rc != RCL_RET_OK) { - printf("Error rclc_support_init.\n"); + printf("Error rcl_init_options_init.\n"); + return -1; + } + + // create context + rc = rcl_init(argc, argv, &init_options, &context); + if (rc != RCL_RET_OK) { + printf("Error in rcl_init.\n"); return -1; } // create rcl_node rcl_node_t my_node = rcl_get_zero_initialized_node(); - rc = rclc_node_init_default(&my_node, "node_0", "executor_examples", &support); + rcl_node_options_t node_ops = rcl_node_get_default_options(); + rc = rcl_node_init(&my_node, "node_0", "executor_examples", &context, &node_ops); if (rc != RCL_RET_OK) { - printf("Error in rclc_node_init_default\n"); + printf("Error in rcl_node_init\n"); return -1; } @@ -80,27 +89,36 @@ int main(int argc, const char * argv[]) const char * topic_name = "topic_0"; const rosidl_message_type_support_t * my_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String); - - rc = rclc_publisher_init_default( + rcl_publisher_options_t pub_options = rcl_publisher_get_default_options(); + rc = rcl_publisher_init( &my_pub, &my_node, my_type_support, - topic_name); + topic_name, + &pub_options); if (RCL_RET_OK != rc) { - printf("Error in rclc_publisher_init_default %s.\n", topic_name); + printf("Error in rcl_publisher_init %s.\n", topic_name); return -1; } - // create a timer, which will call the publisher with period=`timer_timeout` ms in the 'my_timer_callback' + // create a timer, which will call the publisher every 'period' ms in the 'my_timer_callback' + rcl_clock_t clock; + rc = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + if (rc != RCL_RET_OK) { + printf("Error in rcl_clock_init.\n"); + return -1; + } rcl_timer_t my_timer = rcl_get_zero_initialized_timer(); const unsigned int timer_timeout = 1000; - rc = rclc_timer_init_default( + rc = rcl_timer_init( &my_timer, - &support, + &clock, + &context, RCL_MS_TO_NS(timer_timeout), - my_timer_callback); + my_timer_callback, + allocator); if (rc != RCL_RET_OK) { - printf("Error in rcl_timer_init_default.\n"); + printf("Error in rcl_timer_init.\n"); return -1; } else { printf("Created timer with timeout %d ms.\n", timer_timeout); @@ -116,11 +134,16 @@ int main(int argc, const char * argv[]) // create subscription rcl_subscription_t my_sub = rcl_get_zero_initialized_subscription(); - rc = rclc_subscription_init_default( + rcl_subscription_options_t my_subscription_options = rcl_subscription_get_default_options(); + + + rc = rcl_subscription_init( &my_sub, &my_node, my_type_support, - topic_name); + topic_name, + &my_subscription_options); + if (rc != RCL_RET_OK) { printf("Failed to create subscriber %s.\n", topic_name); return -1; @@ -135,11 +158,22 @@ int main(int argc, const char * argv[]) // Configuration of RCL Executor //////////////////////////////////////////////////////////////////////////// rclc_executor_t executor; - executor = rclc_executor_get_zero_initialized_executor(); - // total number of handles = #subscriptions + #timers + + // Note: + // If you need more than the default number of publisher/subscribers, etc., you + // need to configure the micro-ROS middleware also! + // See documentation in the executor.h at the function rclc_executor_init() + // for more details. unsigned int num_handles = 1 + 1; printf("Debug: number of DDS handles: %u\n", num_handles); - rclc_executor_init(&executor, &support.context, num_handles, &allocator); + rclc_executor_init(&executor, &context, num_handles, &allocator); + + // set timeout for rcl_wait() + unsigned int rcl_wait_timeout = 1000; // in ms + rc = rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)); + if (rc != RCL_RET_OK) { + printf("Error in rclc_executor_set_timeout."); + } // add subscription to executor rc = rclc_executor_add_subscription( @@ -154,22 +188,15 @@ int main(int argc, const char * argv[]) printf("Error in rclc_executor_add_timer.\n"); } - // Optional prepare for avoiding allocations during spin - rclc_executor_prepare(&executor); + rclc_executor_spin(&executor); - for (unsigned int i = 0; i < 10; i++) { - // timeout specified in nanoseconds (here 1s) - rclc_executor_spin_some(&executor, 1000 * (1000 * 1000)); - } - - // clean up + // clean up (never reached) rc = rclc_executor_fini(&executor); rc += rcl_publisher_fini(&my_pub, &my_node); rc += rcl_timer_fini(&my_timer); rc += rcl_subscription_fini(&my_sub, &my_node); rc += rcl_node_fini(&my_node); - rc += rclc_support_fini(&support); - + rc += rcl_init_options_fini(&init_options); std_msgs__msg__String__fini(&pub_msg); std_msgs__msg__String__fini(&sub_msg);