Collect log messages from rcl, and reset. (#2720)

* Collect log messages from rcl, and reset.

Signed-off-by: Tomoya Fujita <[email protected]>

* call rcl_reset_error once the error message is collected.

Signed-off-by: Tomoya Fujita <[email protected]>

* address CI failure, error is already collected and reset.

Signed-off-by: Tomoya Fujita <[email protected]>

---------

Signed-off-by: Tomoya Fujita <[email protected]>

Author: fujitatomoya

rclcpp/include/rclcpp/event_handler.hpp

@@ -278,6 +278,7 @@ class EventHandler : public EventHandlerBase
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
         "Couldn't take event info: %s", rcl_get_error_string().str);
+      rcl_reset_error();
       return nullptr;
     }
     return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -201,6 +201,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
+        rcl_reset_error();
         return false;
       }
     }
@@ -210,6 +211,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add client to wait set: %s", rcl_get_error_string().str);
+        rcl_reset_error();
         return false;
       }
     }
@@ -219,6 +221,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add service to wait set: %s", rcl_get_error_string().str);
+        rcl_reset_error();
         return false;
       }
     }
@@ -228,6 +231,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add timer to wait set: %s", rcl_get_error_string().str);
+        rcl_reset_error();
         return false;
       }
     }

rclcpp/src/rclcpp/exceptions/exceptions.cpp

@@ -93,7 +93,9 @@ throw_from_rcl_error(
 RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
 : ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
   formatted_message(rcl_get_error_string().str)
-{}
+{
+  rcl_reset_error();
+}
 
 RCLError::RCLError(
   rcl_ret_t ret,

rclcpp/src/rclcpp/executor.cpp

@@ -568,6 +568,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
                 "rcl_return_loaned_message_from_subscription() failed for subscription on topic "
                 "'%s': %s",
                 subscription->get_topic_name(), rcl_get_error_string().str);
+              rcl_reset_error();
             }
             loaned_msg = nullptr;
           }

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -132,13 +132,15 @@ NodeBase::NodeBase(
             RCUTILS_LOG_ERROR_NAMED(
               "rclcpp",
               "Error in destruction of rosout publisher: %s", rcl_get_error_string().str);
+            rcl_reset_error();
           }
         }
       }
       if (rcl_node_fini(node) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
+        rcl_reset_error();
       }
       delete node;
     });

rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp

@@ -105,6 +105,7 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl
         RCLCPP_ERROR(
           logger_, "Failed to initialize ~/get_type_description service: %s",
           rcl_get_error_string().str);
+        rcl_reset_error();
         throw std::runtime_error(
                 "Failed to initialize ~/get_type_description service.");
       }

rclcpp/src/rclcpp/serialized_message.cpp

@@ -122,6 +122,7 @@ SerializedMessage::~SerializedMessage()
       RCLCPP_ERROR(
         get_logger("rclcpp"),
         "Failed to destroy serialized message: %s", rcl_get_error_string().str);
+      rcl_reset_error();
     }
   }
 }

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -535,8 +535,8 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) {
     });
   allocator_memory_strategy()->collect_entities(weak_groups_to_nodes);
   EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr));
-  EXPECT_TRUE(rcl_error_is_set());
-  rcl_reset_error();
+  // The error message is collected and already reset.
+  EXPECT_FALSE(rcl_error_is_set());
 }
 
 TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_subscription) {

rclcpp_action/src/server.cpp

@@ -132,7 +132,9 @@ ServerBase::ServerBase(
         if (RCL_RET_OK != ret) {
           RCLCPP_DEBUG(
             rclcpp::get_logger("rclcpp_action"),
-            "failed to fini rcl_action_server_t in deleter");
+            "failed to fini rcl_action_server_t in deleter: %s",
+            rcl_get_error_string().str);
+          rcl_reset_error();
         }
         delete ptr;
       }
@@ -424,7 +426,9 @@ ServerBase::execute_goal_request_received(
           if (RCL_RET_OK != fail_ret) {
             RCLCPP_DEBUG(
               rclcpp::get_logger("rclcpp_action"),
-              "failed to fini rcl_action_goal_handle_t in deleter");
+              "failed to fini rcl_action_goal_handle_t in deleter: %s",
+              rcl_get_error_string().str);
+            rcl_reset_error();
           }
           delete ptr;
         }

rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp

@@ -70,7 +70,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl()
   if (ret != RCL_RET_OK) {
     RCLCPP_FATAL(
       node_logging_interface_->get_logger(),
-      "failed to destroy rcl_state_machine");
+      "failed to destroy rcl_state_machine: %s",
+      rcl_get_error_string().str);
+    rcl_reset_error();
   }
 }
 
@@ -405,6 +407,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
         node_logging_interface_->get_logger(),
         "Unable to change state for state machine for %s: %s",
         node_base_interface_->get_name(), rcl_get_error_string().str);
+      rcl_reset_error();
       return RCL_RET_ERROR;
     }