diff --git a/rclc/src/rclc/node.c b/rclc/src/rclc/node.c index 283777ef..80c7587d 100644 --- a/rclc/src/rclc/node.c +++ b/rclc/src/rclc/node.c @@ -17,6 +17,8 @@ #include "rclc/node.h" #include +#include +#include #include rcl_ret_t @@ -79,6 +81,24 @@ rclc_node_init_with_options( node_ops); if (rc != RCL_RET_OK) { PRINT_RCLC_WARN(rclc_node_init_with_options, rcl_node_init); + return rc; } + + // The initialization for the rosout publisher + if (rcl_logging_rosout_enabled() && node_ops->enable_rosout) { + rc = rcl_logging_rosout_init_publisher_for_node(node); + if (rc != RCL_RET_OK) { + PRINT_RCLC_WARN( + rclc_node_init_with_options, + rcl_logging_rosout_init_publisher_for_node); + if (rcl_logging_rosout_fini_publisher_for_node(node) != RCL_RET_OK) { + PRINT_RCLC_WARN( + rclc_node_init_with_options, + rcl_logging_rosout_fini_publisher_for_node); + } + return rc; + } + } + return rc; } diff --git a/rclc/test/rclc/test_executor.cpp b/rclc/test/rclc/test_executor.cpp index 86346f49..961a6cca 100644 --- a/rclc/test/rclc/test_executor.cpp +++ b/rclc/test/rclc/test_executor.cpp @@ -21,6 +21,8 @@ #include #include +#include "rcl/logging.h" +#include "rcl/logging_rosout.h" #include "rclc/executor.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcutils/time.h" @@ -545,6 +547,10 @@ class TestDefaultExecutor : public ::testing::Test rcl_node_options_t node_options = rcl_node_get_default_options(); ret = rcl_node_init(&this->node, name, "", &this->context, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + if (rcl_logging_rosout_enabled() && node_options.enable_rosout) { + ret = rcl_logging_rosout_init_publisher_for_node(&this->node); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } const rcl_node_options_t * node_ops = rcl_node_get_options(&this->node); this->allocator_ptr = &node_ops->allocator; diff --git a/rclc_examples/src/example_executor_only_rcl.c b/rclc_examples/src/example_executor_only_rcl.c index 3d1c7cf3..0bd78ff7 100644 --- a/rclc_examples/src/example_executor_only_rcl.c +++ b/rclc_examples/src/example_executor_only_rcl.c @@ -15,6 +15,8 @@ #include +#include +#include #include #include @@ -83,6 +85,13 @@ int main(int argc, const char * argv[]) printf("Error in rcl_node_init\n"); return -1; } + if (rcl_logging_rosout_enabled() && node_ops.enable_rosout) { + rc = rcl_logging_rosout_init_publisher_for_node(&my_node); + if (rc != RCL_RET_OK) { + printf("Error in rcl_logging_rosout_init_publisher_for_node\n"); + return -1; + } + } // create a publisher to publish topic 'topic_0' with type std_msg::msg::String // my_pub is global, so that the timer callback can access this publisher. diff --git a/rclc_lifecycle/test/test_lifecycle.cpp b/rclc_lifecycle/test/test_lifecycle.cpp index 2e006acd..b2d06826 100644 --- a/rclc_lifecycle/test/test_lifecycle.cpp +++ b/rclc_lifecycle/test/test_lifecycle.cpp @@ -23,6 +23,9 @@ extern "C" #include #include +#include +#include + #include "rclc_lifecycle/rclc_lifecycle.h" } @@ -63,6 +66,9 @@ TEST(TestRclcLifecycle, lifecycle_node) { rcl_node_t my_node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); + if (rcl_logging_rosout_enabled() && node_ops.enable_rosout) { + res += rcl_logging_rosout_init_publisher_for_node(&my_node); + } rclc_lifecycle_node_t lifecycle_node; rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); @@ -100,6 +106,9 @@ TEST(TestRclcLifecycle, lifecycle_node_transitions) { rcl_node_t my_node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); + if (rcl_logging_rosout_enabled() && node_ops.enable_rosout) { + res += rcl_logging_rosout_init_publisher_for_node(&my_node); + } rclc_lifecycle_node_t lifecycle_node; rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); @@ -170,6 +179,9 @@ TEST(TestRclcLifecycle, lifecycle_node_callbacks) { rcl_node_t my_node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); + if (rcl_logging_rosout_enabled() && node_ops.enable_rosout) { + res += rcl_logging_rosout_init_publisher_for_node(&my_node); + } rclc_lifecycle_node_t lifecycle_node; rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine(); @@ -235,6 +247,9 @@ TEST(TestRclcLifecycle, lifecycle_node_servers) { rcl_node_t my_node = rcl_get_zero_initialized_node(); rcl_node_options_t node_ops = rcl_node_get_default_options(); res += rcl_node_init(&my_node, "lifecycle_node", "rclc", &context, &node_ops); + if (rcl_logging_rosout_enabled() && node_ops.enable_rosout) { + res += rcl_logging_rosout_init_publisher_for_node(&my_node); + } rclc_lifecycle_node_t lifecycle_node; rcl_lifecycle_state_machine_t state_machine = rcl_lifecycle_get_zero_initialized_state_machine();