WIP: backport message flow tracing to foxy

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -88,7 +88,7 @@ public:
     std::shared_ptr<typename ServiceT::Request> request,
     std::shared_ptr<typename ServiceT::Response> response)
   {
-    TRACEPOINT(callback_start, (const void *)this, false);
+    TRACEPOINT(callback_start, static_cast<const void *>(this), false);
     if (shared_ptr_callback_ != nullptr) {
       (void)request_header;
       shared_ptr_callback_(request, response);
@@ -97,7 +97,7 @@ public:
     } else {
       throw std::runtime_error("unexpected request without any callback set");
     }
-    TRACEPOINT(callback_end, (const void *)this);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   void register_callback_for_tracing()
@@ -106,12 +106,12 @@ public:
     if (shared_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(shared_ptr_callback_));
     } else if (shared_ptr_with_request_header_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(shared_ptr_with_request_header_callback_));
     }
 #endif  // TRACETOOLS_DISABLED

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -158,7 +158,7 @@ public:
   void dispatch(
     std::shared_ptr<MessageT> message, const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, (const void *)this, false);
+    TRACEPOINT(callback_start, static_cast<const void *>(this), false);
     if (shared_ptr_callback_) {
       shared_ptr_callback_(message);
     } else if (shared_ptr_with_info_callback_) {
@@ -178,13 +178,13 @@ public:
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
-    TRACEPOINT(callback_end, (const void *)this);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   void dispatch_intra_process(
     ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, (const void *)this, true);
+    TRACEPOINT(callback_start, static_cast<const void *>(this), false);
     if (const_shared_ptr_callback_) {
       const_shared_ptr_callback_(message);
     } else if (const_shared_ptr_with_info_callback_) {
@@ -201,13 +201,13 @@ public:
         throw std::runtime_error("unexpected message without any callback set");
       }
     }
-    TRACEPOINT(callback_end, (const void *)this);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   void dispatch_intra_process(
     MessageUniquePtr message, const rclcpp::MessageInfo & message_info)
   {
-    TRACEPOINT(callback_start, (const void *)this, true);
+    TRACEPOINT(callback_start, static_cast<const void *>(this), false);
     if (shared_ptr_callback_) {
       typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_callback_(shared_message);
@@ -225,7 +225,7 @@ public:
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
-    TRACEPOINT(callback_end, (const void *)this);
+    TRACEPOINT(callback_end, static_cast<const void *>(this));
   }
 
   bool use_take_shared_method() const
@@ -239,32 +239,32 @@ public:
     if (shared_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(shared_ptr_callback_));
     } else if (shared_ptr_with_info_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(shared_ptr_with_info_callback_));
     } else if (const_shared_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(const_shared_ptr_callback_));
     } else if (const_shared_ptr_with_info_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(const_shared_ptr_with_info_callback_));
     } else if (unique_ptr_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(unique_ptr_callback_));
     } else if (unique_ptr_with_info_callback_) {
       TRACEPOINT(
         rclcpp_callback_register,
-        (const void *)this,
+        static_cast<const void *>(this),
         get_symbol(unique_ptr_with_info_callback_));
     }
 #endif  // TRACETOOLS_DISABLED

rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp

@@ -23,6 +23,7 @@
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
 #include "rclcpp/macros.hpp"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -93,6 +94,11 @@ public:
 
     buffer_ = std::move(buffer_impl);
 
+    TRACEPOINT(
+      rclcpp_buffer_to_ipb,
+      static_cast<const void *>(buffer_.get()),
+      static_cast<const void *>(this));
+
     if (!allocator) {
       message_allocator_ = std::make_shared<MessageAlloc>();
     } else {

rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp

@@ -29,6 +29,7 @@
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -51,6 +52,10 @@ public:
     if (capacity == 0) {
       throw std::invalid_argument("capacity must be a positive, non-zero value");
     }
+    TRACEPOINT(
+      rclcpp_construct_ring_buffer,
+      static_cast<const void *>(this),
+      capacity_);
   }
 
   virtual ~RingBufferImplementation() {}
@@ -61,6 +66,12 @@ public:
 
     write_index_ = next(write_index_);
     ring_buffer_[write_index_] = std::move(request);
+    TRACEPOINT(
+      rclcpp_ring_buffer_enqueue,
+      static_cast<const void *>(this),
+      write_index_,
+      size_ + 1,
+      is_full());
 
     if (is_full()) {
       read_index_ = next(read_index_);
@@ -79,6 +90,11 @@ public:
     }
 
     auto request = std::move(ring_buffer_[read_index_]);
+    TRACEPOINT(
+      rclcpp_ring_buffer_dequeue,
+      static_cast<const void *>(this),
+      read_index_,
+      size_ - 1);
     read_index_ = next(read_index_);
 
     size_--;
@@ -101,7 +117,9 @@ public:
     return size_ == capacity_;
   }
 
-  void clear() {}
+  void clear() {
+    TRACEPOINT(rclcpp_ring_buffer_clear, static_cast<const void *>(this));
+  }
 
 private:
   size_t capacity_;

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -92,8 +92,8 @@ public:
 
     TRACEPOINT(
       rclcpp_subscription_callback_added,
-      (const void *)this,
-      (const void *)&any_callback_);
+      static_cast<const void *>(this),
+      static_cast<const void *>(&any_callback_));
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.

rclcpp/include/rclcpp/publisher.hpp

@@ -40,6 +40,8 @@
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "tracetools/tracetools.h"
+
 namespace rclcpp
 {
 
@@ -279,6 +281,7 @@ protected:
   void
   do_inter_process_publish(const MessageT & msg)
   {
+    TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(&msg));
     auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
@@ -312,6 +315,7 @@ protected:
   void
   do_loaned_message_publish(MessageT * msg)
   {
+    TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(msg));
     auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg, nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
@@ -341,12 +345,17 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
+    TRACEPOINT(
+      rclcpp_intra_publish,
+      static_cast<const void *>(publisher_handle_.get()),
+      msg.get());
+
     ipm->template do_intra_process_publish<MessageT, AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),
       message_allocator_);
   }
-
+  
   std::shared_ptr<const MessageT>
   do_intra_process_publish_and_return_shared(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
@@ -359,6 +368,11 @@ protected:
       throw std::runtime_error("cannot publish msg which is a null pointer");
     }
 
+    TRACEPOINT(
+      rclcpp_intra_publish,
+      static_cast<const void *>(publisher_handle_.get()),
+      msg.get());
+
     return ipm->template do_intra_process_publish_and_return_shared<MessageT, AllocatorT>(
       intra_process_publisher_id_,
       std::move(msg),

rclcpp/include/rclcpp/service.hpp

@@ -214,8 +214,8 @@ public:
     }
     TRACEPOINT(
       rclcpp_service_callback_added,
-      (const void *)get_service_handle().get(),
-      (const void *)&any_callback_);
+      static_cast<const void *>(get_service_handle().get()),
+      static_cast<const void *>(&any_callback_));
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif
@@ -249,8 +249,8 @@ public:
     service_handle_ = service_handle;
     TRACEPOINT(
       rclcpp_service_callback_added,
-      (const void *)get_service_handle().get(),
-      (const void *)&any_callback_);
+      static_cast<const void *>(get_service_handle().get()),
+      static_cast<const void *>(&any_callback_));
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif
@@ -286,8 +286,8 @@ public:
     service_handle_->impl = service_handle->impl;
     TRACEPOINT(
       rclcpp_service_callback_added,
-      (const void *)get_service_handle().get(),
-      (const void *)&any_callback_);
+      static_cast<const void *>(get_service_handle().get()),
+      static_cast<const void *>(&any_callback_));
 #ifndef TRACETOOLS_DISABLED
     any_callback_.register_callback_for_tracing();
 #endif

rclcpp/include/rclcpp/subscription.hpp

@@ -179,8 +179,8 @@ public:
         resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback));
       TRACEPOINT(
         rclcpp_subscription_init,
-        (const void *)get_subscription_handle().get(),
-        (const void *)subscription_intra_process.get());
+        static_cast<const void *>(get_subscription_handle().get()),
+        static_cast<const void *>(subscription_intra_process.get()));
 
       // Add it to the intra process manager.
       using rclcpp::experimental::IntraProcessManager;
@@ -195,12 +195,12 @@ public:
 
     TRACEPOINT(
       rclcpp_subscription_init,
-      (const void *)get_subscription_handle().get(),
-      (const void *)this);
+      static_cast<const void *>(get_subscription_handle().get()),
+      static_cast<const void *>(this));
     TRACEPOINT(
       rclcpp_subscription_callback_added,
-      (const void *)this,
-      (const void *)&any_callback_);
+      static_cast<const void *>(this),
+      static_cast<const void *>(&any_callback_));
     // The callback object gets copied, so if registration is done too early/before this point
     // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
     // in subsequent tracepoints.

rclcpp/include/rclcpp/timer.hpp

@@ -176,11 +176,11 @@ public:
   {
     TRACEPOINT(
       rclcpp_timer_callback_added,
-      (const void *)get_timer_handle().get(),
-      (const void *)&callback_);
+      static_cast<const void *>(get_timer_handle().get()),
+      static_cast<const void *>(&callback_));
     TRACEPOINT(
       rclcpp_callback_register,
-      (const void *)&callback_,
+      static_cast<const void *>(&callback_),
       get_symbol(callback_));
   }
 
@@ -205,9 +205,9 @@ public:
     if (ret != RCL_RET_OK) {
       throw std::runtime_error("Failed to notify timer that callback occurred");
     }
-    TRACEPOINT(callback_start, (const void *)&callback_, false);
+    TRACEPOINT(callback_start, static_cast<const void *>(&callback_), false);
     execute_callback_delegate<>();
-    TRACEPOINT(callback_end, (const void *)&callback_);
+    TRACEPOINT(callback_end, static_cast<const void *>(&callback_));
   }
 
   // void specialization

rclcpp/src/rclcpp/executor.cpp

@@ -29,6 +29,8 @@
 
 #include "rcutils/logging_macros.h"
 
+#include "tracetools/tracetools.h"
+
 using rclcpp::exceptions::throw_from_rcl_error;
 using rclcpp::AnyExecutable;
 using rclcpp::Executor;
@@ -301,9 +303,15 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
     return;
   }
   if (any_exec.timer) {
+    TRACEPOINT(
+      rclcpp_executor_execute,
+      static_cast<const void *>(any_exec.timer->get_timer_handle().get()));
     execute_timer(any_exec.timer);
   }
   if (any_exec.subscription) {
+    TRACEPOINT(
+      rclcpp_executor_execute,
+      static_cast<const void *>(any_exec.subscription->get_subscription_handle().get()));
     execute_subscription(any_exec.subscription);
   }
   if (any_exec.service) {
@@ -463,6 +471,7 @@ Executor::execute_client(
 void
 Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
+  TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());
   {
     std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
 
@@ -570,6 +579,7 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
 bool
 Executor::get_next_ready_executable(AnyExecutable & any_executable)
 {
+  TRACEPOINT(rclcpp_executor_get_next_ready);
   bool success = false;
   // Check the timers to see if there are any that are ready
   memory_strategy_->get_next_timer(any_executable, weak_nodes_);

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "rclcpp/node_interfaces/node_timers.hpp"
+#include "tracetools/tracetools.h"
 
 #include <string>
 
@@ -44,4 +45,9 @@ NodeTimers::add_timer(
             std::string("Failed to notify wait set on timer creation: ") +
             rmw_get_error_string().str);
   }
+
+  TRACEPOINT(
+    rclcpp_timer_link_node,
+    static_cast<const void *>(timer->get_timer_handle().get()),
+    static_cast<const void *>(node_base_->get_rcl_node_handle()));
 }

rclcpp/src/rclcpp/subscription_base.cpp

@@ -143,6 +143,7 @@ SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & mes
     &message_info_out.get_rmw_message_info(),
     nullptr  // rmw_subscription_allocation_t is unused here
   );
+  TRACEPOINT(rclcpp_take, static_cast<const void *>(message_out));
   if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
     return false;
   } else if (RCL_RET_OK != ret) {
@@ -168,6 +169,9 @@ SubscriptionBase::take_serialized(
     &message_out.get_rcl_serialized_message(),
     &message_info_out.get_rmw_message_info(),
     nullptr);
+  TRACEPOINT(
+    rclcpp_take,
+    static_cast<const void *>(&message_out.get_rcl_serialized_message()));
   if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
     return false;
   } else if (RCL_RET_OK != ret) {

rclcpp_action/CMakeLists.txt

@@ -3,6 +3,7 @@ cmake_minimum_required(VERSION 3.5)
 project(rclcpp_action)
 
 find_package(ament_cmake_ros REQUIRED)
+find_package(tracetools REQUIRED)
 find_package(action_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcl_action REQUIRED)
@@ -33,6 +34,7 @@ target_include_directories(${PROJECT_NAME}
   "$<INSTALL_INTERFACE:include>")
 
 ament_target_dependencies(${PROJECT_NAME}
+  "tracetools"
   "action_msgs"
   "rcl_action"
   "rclcpp"
@@ -62,6 +64,7 @@ ament_export_libraries(${PROJECT_NAME})
 ament_export_targets(${PROJECT_NAME})
 
 ament_export_dependencies(ament_cmake)
+ament_export_dependencies(tracetools)
 ament_export_dependencies(action_msgs)
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rcl_action)

rclcpp_action/package.xml

@@ -11,6 +11,7 @@
 
   <build_export_depend>rosidl_runtime_c</build_export_depend>
 
+  <build_depend>tracetools</build_depend>
   <build_depend>rosidl_runtime_c</build_depend>
 
   <depend>action_msgs</depend>

rclcpp_components/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(class_loader REQUIRED)
 find_package(composition_interfaces REQUIRED)
+find_package(tracetools REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcpputils REQUIRED)
 
@@ -29,6 +30,7 @@ ament_target_dependencies(component_manager
   "ament_index_cpp"
   "class_loader"
   "composition_interfaces"
+  "tracetools"
   "rclcpp"
   "rcpputils"
 )
@@ -41,6 +43,7 @@ add_executable(
 )
 target_link_libraries(component_container component_manager)
 ament_target_dependencies(component_container
+  "tracetools"
   "rclcpp"
 )
 
@@ -55,6 +58,7 @@ add_executable(
 )
 target_link_libraries(component_container_mt component_manager)
 ament_target_dependencies(component_container_mt
+  "tracetools"
   "rclcpp"
 )
 

rclcpp_components/package.xml

@@ -6,6 +6,8 @@
   <description>Package containing tools for dynamically loadable components</description>
   <maintainer email="michael@openrobotics.org">Michael Carroll</maintainer>
   <license>Apache License 2.0</license>
+  
+  <depend>tracetools</depend>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 

rclcpp_lifecycle/CMakeLists.txt

@@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake_ros REQUIRED)
+find_package(tracetools REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcl_lifecycle REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
@@ -29,6 +30,7 @@ target_include_directories(${PROJECT_NAME}
   "$<INSTALL_INTERFACE:include>")
 # specific order: dependents before dependencies
 ament_target_dependencies(rclcpp_lifecycle
+  "tracetools"
   "rclcpp"
   "rcl_lifecycle"
   "lifecycle_msgs"

rclcpp_lifecycle/package.xml

@@ -13,6 +13,7 @@
   <build_depend>rclcpp</build_depend>
   <build_depend>rcl_lifecycle</build_depend>
   <build_depend>rosidl_typesupport_cpp</build_depend>
+  <build_depend>tracetools</build_depend>
 
   <exec_depend>lifecycle_msgs</exec_depend>
   <exec_depend>rclcpp</exec_depend>