diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index c683345..86d4208 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -88,7 +88,7 @@ public: std::shared_ptr request, std::shared_ptr response) { - TRACEPOINT(callback_start, (const void *)this, false); + TRACEPOINT(callback_start, static_cast(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(this)); } void register_callback_for_tracing() @@ -106,12 +106,12 @@ public: if (shared_ptr_callback_) { TRACEPOINT( rclcpp_callback_register, - (const void *)this, + static_cast(this), get_symbol(shared_ptr_callback_)); } else if (shared_ptr_with_request_header_callback_) { TRACEPOINT( rclcpp_callback_register, - (const void *)this, + static_cast(this), get_symbol(shared_ptr_with_request_header_callback_)); } #endif // TRACETOOLS_DISABLED diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index e1ac86a..114d99f 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -158,7 +158,7 @@ public: void dispatch( std::shared_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, (const void *)this, false); + TRACEPOINT(callback_start, static_cast(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(this)); } void dispatch_intra_process( ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, (const void *)this, true); + TRACEPOINT(callback_start, static_cast(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(this)); } void dispatch_intra_process( MessageUniquePtr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, (const void *)this, true); + TRACEPOINT(callback_start, static_cast(this), false); if (shared_ptr_callback_) { typename std::shared_ptr 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(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(this), get_symbol(shared_ptr_callback_)); } else if (shared_ptr_with_info_callback_) { TRACEPOINT( rclcpp_callback_register, - (const void *)this, + static_cast(this), get_symbol(shared_ptr_with_info_callback_)); } else if (const_shared_ptr_callback_) { TRACEPOINT( rclcpp_callback_register, - (const void *)this, + static_cast(this), get_symbol(const_shared_ptr_callback_)); } else if (const_shared_ptr_with_info_callback_) { TRACEPOINT( rclcpp_callback_register, - (const void *)this, + static_cast(this), get_symbol(const_shared_ptr_with_info_callback_)); } else if (unique_ptr_callback_) { TRACEPOINT( rclcpp_callback_register, - (const void *)this, + static_cast(this), get_symbol(unique_ptr_callback_)); } else if (unique_ptr_with_info_callback_) { TRACEPOINT( rclcpp_callback_register, - (const void *)this, + static_cast(this), get_symbol(unique_ptr_with_info_callback_)); } #endif // TRACETOOLS_DISABLED diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index aef38a0..5233767 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/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(buffer_.get()), + static_cast(this)); + if (!allocator) { message_allocator_ = std::make_shared(); } else { diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index ea37c23..c2835c0 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/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(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(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(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(this)); + } private: size_t capacity_; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 618db3c..ee79241 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/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(this), + static_cast(&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. diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 8518353..60d433b 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/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(&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(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(publisher_handle_.get()), + msg.get()); + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), message_allocator_); } - + std::shared_ptr do_intra_process_publish_and_return_shared(std::unique_ptr msg) { @@ -359,6 +368,11 @@ protected: throw std::runtime_error("cannot publish msg which is a null pointer"); } + TRACEPOINT( + rclcpp_intra_publish, + static_cast(publisher_handle_.get()), + msg.get()); + return ipm->template do_intra_process_publish_and_return_shared( intra_process_publisher_id_, std::move(msg), diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index bec8319..41e6787 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/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(get_service_handle().get()), + static_cast(&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(get_service_handle().get()), + static_cast(&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(get_service_handle().get()), + static_cast(&any_callback_)); #ifndef TRACETOOLS_DISABLED any_callback_.register_callback_for_tracing(); #endif diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 08786c5..08e98c6 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/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(get_subscription_handle().get()), + static_cast(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(get_subscription_handle().get()), + static_cast(this)); TRACEPOINT( rclcpp_subscription_callback_added, - (const void *)this, - (const void *)&any_callback_); + static_cast(this), + static_cast(&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. diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 6f0a0ca..a2662f4 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/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(get_timer_handle().get()), + static_cast(&callback_)); TRACEPOINT( rclcpp_callback_register, - (const void *)&callback_, + static_cast(&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(&callback_), false); execute_callback_delegate<>(); - TRACEPOINT(callback_end, (const void *)&callback_); + TRACEPOINT(callback_end, static_cast(&callback_)); } // void specialization diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 81d03f2..a5b0c5b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/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(any_exec.timer->get_timer_handle().get())); execute_timer(any_exec.timer); } if (any_exec.subscription) { + TRACEPOINT( + rclcpp_executor_execute, + static_cast(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 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_); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index d027a06..b081bc7 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/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 @@ -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(timer->get_timer_handle().get()), + static_cast(node_base_->get_rcl_node_handle())); } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 3772603..6da9ef9 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/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(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(&message_out.get_rcl_serialized_message())); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) { diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 4d4b134..0386c84 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/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} "$") 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) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 77713f1..5b35450 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -11,6 +11,7 @@ rosidl_runtime_c + tracetools rosidl_runtime_c action_msgs diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index bd8ffdd..2d4d228 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/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" ) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index ca06599..af39d7e 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -6,6 +6,8 @@ Package containing tools for dynamically loadable components Michael Carroll Apache License 2.0 + + tracetools ament_cmake_ros diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 0bde94d..7cdeed0 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/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} "$") # specific order: dependents before dependencies ament_target_dependencies(rclcpp_lifecycle + "tracetools" "rclcpp" "rcl_lifecycle" "lifecycle_msgs" diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 48a4748..a65b095 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -13,6 +13,7 @@ rclcpp rcl_lifecycle rosidl_typesupport_cpp + tracetools lifecycle_msgs rclcpp