diff --git a/src/pathnode/CMakeLists.txt b/src/pathnode/CMakeLists.txt index 22b0559456..6e87467810 100644 --- a/src/pathnode/CMakeLists.txt +++ b/src/pathnode/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(path_info_msg REQUIRED) find_package(rmw REQUIRED) +find_package(LTTngUST REQUIRED) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -61,10 +62,14 @@ add_library(${PROJECT_NAME} SHARED "src/path_node.cpp" "src/sub_timing_advertise_node.cpp" "src/timing_advertise_publisher.cpp" + "src/tp.c" ) +target_link_libraries(${PROJECT_NAME} ${LTTNGUST_LIBRARIES}) + ament_target_dependencies(${PROJECT_NAME} "rclcpp" "path_info_msg" + LTTngUST ) target_compile_definitions(${PROJECT_NAME} PRIVATE "PATHNODE_BUILDING_LIBRARY") diff --git a/src/pathnode/include/pathnode/sub_timing_advertise_node.hpp b/src/pathnode/include/pathnode/sub_timing_advertise_node.hpp index 2afdf1f75b..5583427f0f 100644 --- a/src/pathnode/include/pathnode/sub_timing_advertise_node.hpp +++ b/src/pathnode/include/pathnode/sub_timing_advertise_node.hpp @@ -33,6 +33,8 @@ #include "path_info_msg/msg/pub_info.hpp" #include "timing_advertise_publisher.hpp" +#include "pathnode/tp.h" + namespace pathnode { template @@ -97,14 +99,24 @@ class SubTimingAdvertiseNode : public rclcpp::Node auto topic_info_name = resolved_topic_name + "/info/sub"; + std::cerr << &callback << std::endl; auto topic_info_pub = create_publisher( topic_info_name, rclcpp::QoS(1)); topic_info_pubs_[topic_info_name] = topic_info_pub; seqs_[topic_info_name] = 0; + auto callback_addr = &callback; + + tracepoint( + TRACEPOINT_PROVIDER, + tilde_subscription_init, + callback_addr, + get_fully_qualified_name(), + resolved_topic_name.c_str()); + auto main_topic_callback = - [this, resolved_topic_name, topic_info_name, callback]( + [this, resolved_topic_name, topic_info_name, callback, callback_addr]( CallbackArgT msg, const rclcpp::MessageInfo & info) -> void { @@ -115,6 +127,12 @@ class SubTimingAdvertiseNode : public rclcpp::Node // publish subscription timing auto minfo = info.get_rmw_message_info(); + tracepoint( + TRACEPOINT_PROVIDER, + tilde_subscribe, + callback_addr, + subtime_steady.nanoseconds()); + auto m = std::make_unique(); auto & seq = seqs_[topic_info_name]; m->seq = seq; @@ -212,6 +230,15 @@ class SubTimingAdvertiseNode : public rclcpp::Node steady_clock_, this->enable_tilde); timing_advertise_pubs_[info_topic] = ta_pub; + + + tracepoint( + TRACEPOINT_PROVIDER, + tilde_publisher_init, + ta_pub.get(), + get_fully_qualified_name(), + pub->get_topic_name()); + return ta_pub; } diff --git a/src/pathnode/include/pathnode/timing_advertise_publisher.hpp b/src/pathnode/include/pathnode/timing_advertise_publisher.hpp index 3cbfe0635f..0a90b792bc 100644 --- a/src/pathnode/include/pathnode/timing_advertise_publisher.hpp +++ b/src/pathnode/include/pathnode/timing_advertise_publisher.hpp @@ -28,6 +28,10 @@ #include "path_info_msg/msg/pub_info.hpp" +#include "pathnode/tp.h" + +#define TILDE_S_TO_NS(seconds) ((seconds) * (1000LL * 1000LL * 1000LL)) + namespace pathnode { @@ -111,7 +115,8 @@ class TimingAdvertisePublisherBase explicit TimingAdvertisePublisherBase( std::shared_ptr clock, - std::shared_ptr steady_clock); + std::shared_ptr steady_clock, + const std::string & node_fqn); void set_input_info( const std::string & sub_topic, @@ -139,6 +144,10 @@ class TimingAdvertisePublisherBase std::shared_ptr clock_; std::shared_ptr steady_clock_; + const std::string node_fqn_; + + std::map sub_topics_; + private: // parent node subscription topic vs InputInfo std::map> input_infos_; @@ -176,8 +185,10 @@ class TimingAdvertisePublisher : public TimingAdvertisePublisherBase std::shared_ptr clock, std::shared_ptr steady_clock, bool enable) - : TimingAdvertisePublisherBase(clock, steady_clock), info_pub_(info_pub), pub_(pub), node_fqn_( - node_fqn), enable_(enable) + : TimingAdvertisePublisherBase(clock, steady_clock, node_fqn), + info_pub_(info_pub), + pub_(pub), + enable_(enable) { } @@ -266,6 +277,22 @@ class TimingAdvertisePublisher : public TimingAdvertisePublisherBase set_input_info(*msg); + for (auto & input_info : msg->input_infos) { + auto pubtime = TILDE_S_TO_NS(msg->output_info.pub_time.sec) + + msg->output_info.pub_time.nanosec; + auto subtime_steady = TILDE_S_TO_NS(input_info.sub_time_steady.sec) + + input_info.sub_time_steady.nanosec; + auto sub = &sub_topics_[input_info.topic_name]; + tracepoint( + TRACEPOINT_PROVIDER, + tilde_publish, + this, + pubtime, + sub, + subtime_steady + ); + } + info_pub_->publish(std::move(msg)); } }; diff --git a/src/pathnode/include/pathnode/tp.h b/src/pathnode/include/pathnode/tp.h new file mode 100644 index 0000000000..5bbbef855a --- /dev/null +++ b/src/pathnode/include/pathnode/tp.h @@ -0,0 +1,109 @@ +// Copyright 2021 Research Institute of Systems Planning, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Provide fake header guard for cpplint +#undef PATHNODE_TP_H_ +#ifndef PATHNODE_TP_H_ +#define PATHNODE_TP_H_ + +#undef TRACEPOINT_PROVIDER +#define TRACEPOINT_PROVIDER ros2_caret + +#undef TRACEPOINT_INCLUDE +#define TRACEPOINT_INCLUDE "pathnode/tp.h" + +#if !defined(_TP_H) || defined(TRACEPOINT_HEADER_MULTI_READ) +#define _TP_H + +#include + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, + tilde_subscription_init, + TP_ARGS( + const void *, subscription_arg, + const char *, node_name_arg, + const char *, topic_name_arg + ), + TP_FIELDS( + ctf_integer_hex(const void *, subscription, subscription_arg) + ctf_string(node_name, node_name_arg) + ctf_string(topic_name, topic_name_arg) + ) +) + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, + tilde_subscribe, + TP_ARGS( + const void *, subscription_arg, + const uint64_t, tilde_message_id_arg + ), + TP_FIELDS( + ctf_integer_hex(const void *, subscription, subscription_arg) + ctf_integer(const uint64_t, tilde_message_id, tilde_message_id_arg) + ) +) + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, + tilde_publisher_init, + TP_ARGS( + const void *, publisher_arg, + const char *, node_name_arg, + const char *, topic_name_arg + ), + TP_FIELDS( + ctf_integer_hex(const void *, publisher, publisher_arg) + ctf_string(node_name, node_name_arg) + ctf_string(topic_name, topic_name_arg) + ) +) + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, + tilde_subscribe_added, + TP_ARGS( + const void *, subscription_id_arg, + const char *, node_name_arg, + const char *, topic_name_arg + ), + TP_FIELDS( + ctf_integer_hex(const void *, subscription_id, subscription_id_arg) + ctf_string(node_name, node_name_arg) + ctf_string(topic_name, topic_name_arg) + ) +) + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, + tilde_publish, + TP_ARGS( + const void *, publisher_arg, + const uint64_t, tilde_publish_timestamp_arg, + const void *, subscription_id_arg, + const uint64_t, tilde_message_id_arg + ), + TP_FIELDS( + ctf_integer_hex(const void *, publisher, publisher_arg) + ctf_integer(const uint64_t, tilde_publish_timestamp, tilde_publish_timestamp_arg) + ctf_integer_hex(const void *, subscription_id, subscription_id_arg) + ctf_integer(const uint64_t, tilde_message_id, tilde_message_id_arg) + ) +) +#endif /* _TP_H */ + +#include + +#endif // PATHNODE_TP_H_ diff --git a/src/pathnode/src/timing_advertise_publisher.cpp b/src/pathnode/src/timing_advertise_publisher.cpp index f541d39b46..a82f0a59db 100644 --- a/src/pathnode/src/timing_advertise_publisher.cpp +++ b/src/pathnode/src/timing_advertise_publisher.cpp @@ -29,8 +29,9 @@ rclcpp::Time pathnode::get_timestamp(rclcpp::Time t, ...) TimingAdvertisePublisherBase::TimingAdvertisePublisherBase( std::shared_ptr clock, - std::shared_ptr steady_clock) -: clock_(clock), steady_clock_(steady_clock), + std::shared_ptr steady_clock, + const std::string & node_fqn) +: clock_(clock), steady_clock_(steady_clock), node_fqn_(node_fqn), MAX_SUB_CALLBACK_INFOS_SEC_(2) { } @@ -40,6 +41,16 @@ void TimingAdvertisePublisherBase::set_input_info( const std::shared_ptr p) { input_infos_[sub_topic] = p; + if (sub_topics_.count(sub_topic) == 0) { + sub_topics_[sub_topic] = sub_topic; + tracepoint( + TRACEPOINT_PROVIDER, + tilde_subscribe_added, + &sub_topics_[sub_topic], + node_fqn_.c_str(), + sub_topic.c_str() + ); + } } void TimingAdvertisePublisherBase::add_explicit_input_info( @@ -60,6 +71,17 @@ void TimingAdvertisePublisherBase::add_explicit_input_info( info.has_header_stamp = true; info.header_stamp = stamp; explicit_input_infos_[sub_topic].push_back(info); + + if (sub_topics_.count(sub_topic) == 0) { + sub_topics_[sub_topic] = sub_topic; + tracepoint( + TRACEPOINT_PROVIDER, + tilde_subscribe_added, + &sub_topics_[sub_topic], + node_fqn_.c_str(), + sub_topic.c_str() + ); + } } void TimingAdvertisePublisherBase::set_input_info(path_info_msg::msg::PubInfo & info_msg) diff --git a/src/pathnode/src/tp.c b/src/pathnode/src/tp.c new file mode 100644 index 0000000000..530c5c67f5 --- /dev/null +++ b/src/pathnode/src/tp.c @@ -0,0 +1,19 @@ +// Copyright 2021 Research Institute of Systems Planning, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#define TRACEPOINT_CREATE_PROBES + +#define TRACEPOINT_DEFINE + +#include "pathnode/tp.h" diff --git a/src/pathnode/test/test_timing_advertise_publisher.cpp b/src/pathnode/test/test_timing_advertise_publisher.cpp index becae6c5df..28e582066e 100644 --- a/src/pathnode/test/test_timing_advertise_publisher.cpp +++ b/src/pathnode/test/test_timing_advertise_publisher.cpp @@ -47,7 +47,7 @@ TEST_F(TestTimingAdvertisePublisher, set_input_info) { auto clock = std::make_shared(); auto steady_clock = std::make_shared(RCL_STEADY_TIME); - TimingAdvertisePublisherBase pub(clock, steady_clock); + TimingAdvertisePublisherBase pub(clock, steady_clock, "node_name"); auto info = std::make_shared(); info->sub_time = rclcpp::Time(1, 0); info->sub_time_steady = steady_clock->now(); @@ -77,7 +77,7 @@ TEST_F(TestTimingAdvertisePublisher, add_explicit_input_info_subtime_not_found) auto clock = std::make_shared(); auto steady_clock = std::make_shared(RCL_STEADY_TIME); - TimingAdvertisePublisherBase pub(clock, steady_clock); + TimingAdvertisePublisherBase pub(clock, steady_clock, "node_name"); auto now = clock->now(); auto info_stamp = now - rclcpp::Duration(1, 0); auto search_stamp = now - rclcpp::Duration(2, 0); @@ -111,7 +111,7 @@ TEST_F(TestTimingAdvertisePublisher, set_explicit_subtime_sucess_then_purged) { // set input_info & explicit_input_info auto clock = std::make_shared(); auto steady_clock = std::make_shared(RCL_STEADY_TIME); - TimingAdvertisePublisherBase pub(clock, steady_clock); + TimingAdvertisePublisherBase pub(clock, steady_clock, "node_name"); const std::string TOPIC = "sample_topic"; auto now = clock->now(); @@ -188,7 +188,7 @@ TEST_F(TestTimingAdvertisePublisher, set_explicit_subtime_sucess_then_purged) { TEST_F(TestTimingAdvertisePublisher, set_multiple_topic) { auto clock = std::make_shared(); auto steady_clock = std::make_shared(RCL_STEADY_TIME); - TimingAdvertisePublisherBase pub(clock, steady_clock); + TimingAdvertisePublisherBase pub(clock, steady_clock, "node_name"); const std::string TOPIC1 = "sample_topic1"; const std::string TOPIC2 = "sample_topic2";