Merge pull request #1 from hasegawa/feat_caret_support

support CARET.

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")

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<class>
@@ -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<path_info_msg::msg::TopicInfo>(
       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<path_info_msg::msg::TopicInfo>();
           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;
   }
 

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<rclcpp::Clock> clock,
-    std::shared_ptr<rclcpp::Clock> steady_clock);
+    std::shared_ptr<rclcpp::Clock> 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<rclcpp::Clock> clock_;
   std::shared_ptr<rclcpp::Clock> steady_clock_;
 
+  const std::string node_fqn_;
+
+  std::map<std::string, std::string> sub_topics_;
+
 private:
   // parent node subscription topic vs InputInfo
   std::map<std::string, std::shared_ptr<const InputInfo>> input_infos_;
@@ -176,8 +185,10 @@ class TimingAdvertisePublisher : public TimingAdvertisePublisherBase
     std::shared_ptr<rclcpp::Clock> clock,
     std::shared_ptr<rclcpp::Clock> 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));
   }
 };

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 <lttng/tracepoint.h>
+
+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 <lttng/tracepoint-event.h>
+
+#endif  // PATHNODE_TP_H_

src/pathnode/src/timing_advertise_publisher.cpp

@@ -29,8 +29,9 @@ rclcpp::Time pathnode::get_timestamp(rclcpp::Time t, ...)
 
 TimingAdvertisePublisherBase::TimingAdvertisePublisherBase(
   std::shared_ptr<rclcpp::Clock> clock,
-  std::shared_ptr<rclcpp::Clock> steady_clock)
-: clock_(clock), steady_clock_(steady_clock),
+  std::shared_ptr<rclcpp::Clock> 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<const InputInfo> 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)

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"

src/pathnode/test/test_timing_advertise_publisher.cpp

@@ -47,7 +47,7 @@ TEST_F(TestTimingAdvertisePublisher, set_input_info) {
   auto clock = std::make_shared<rclcpp::Clock>();
   auto steady_clock = std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME);
 
-  TimingAdvertisePublisherBase pub(clock, steady_clock);
+  TimingAdvertisePublisherBase pub(clock, steady_clock, "node_name");
   auto info = std::make_shared<pathnode::InputInfo>();
   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<rclcpp::Clock>();
   auto steady_clock = std::make_shared<rclcpp::Clock>(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<rclcpp::Clock>();
   auto steady_clock = std::make_shared<rclcpp::Clock>(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<rclcpp::Clock>();
   auto steady_clock = std::make_shared<rclcpp::Clock>(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";