Change which variable we are using.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

rclcpp/CMakeLists.txt

@@ -21,7 +21,7 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wall -Wextra -Wpedantic -rdynamic -g)
 endif()
 
 include_directories(include)

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -24,6 +24,7 @@
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/types.h"
 #include "tracetools/tracetools.h"
+#include "tracetools/utils.hpp"
 
 namespace rclcpp
 {
@@ -98,6 +99,21 @@ public:
     }
     TRACEPOINT(callback_end, (const void *)this);
   }
+
+  void register_callback_for_tracing()
+  {
+    if (shared_ptr_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(shared_ptr_callback_));
+    } else if (shared_ptr_with_request_header_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(shared_ptr_with_request_header_callback_));
+    }
+  }
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/clock.hpp

@@ -16,6 +16,9 @@
 #define RCLCPP__CLOCK_HPP_
 
 #include <functional>
+#include <mutex>
+#include <shared_mutex>  // NOLINT
+#include <unordered_map>
 
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
@@ -138,6 +141,19 @@ private:
   rcl_allocator_t allocator_;
 };
 
+// This code is an ABI-compatible version of the code that went in for
+// https://github.com/ros2/rclcpp/pull/999.  The way that this works is to have
+// a global map of pointers to mutexes, one per clock class that is created.
+// During the rclcpp::Clock constructor, it adds a new one to this map, and
+// during the rclcpp::Clock destructor, it removes it from this map.  When
+// it needs to lock it, it just looks it up in the map.  The map itself has
+// to be protected when adding a new mutex, removing one, or looking it up,
+// hence the g_clock_map_mutex.
+extern std::shared_timed_mutex g_clock_map_mutex;
+extern std::unordered_map<rclcpp::Clock *, std::mutex> g_clock_mutex_map;
+
+std::mutex & get_clock_mutex(rclcpp::Clock * clock);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CLOCK_HPP_

rclcpp/include/rclcpp/function_traits.hpp

@@ -88,8 +88,7 @@ struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
 struct function_traits<
-  std::_Binder<std::_Unforced, ReturnTypeT(__cdecl ClassT::*)(Args ...), FArgs ...>
->
+  std::_Binder<std::_Unforced, ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
 #else
 #error "Unsupported C++ compiler / standard library"
 #endif
@@ -106,8 +105,7 @@ struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...) co
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...) const>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
 struct function_traits<
-  std::_Binder<std::_Unforced, ReturnTypeT(__cdecl ClassT::*)(Args ...) const, FArgs ...>
->
+  std::_Binder<std::_Unforced, ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
 #else
 #error "Unsupported C++ compiler / standard library"
 #endif
@@ -121,7 +119,7 @@ struct function_traits<std::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>
 #elif defined _MSC_VER  // MS Visual Studio
-struct function_traits<std::_Binder<std::_Unforced, ReturnTypeT(__cdecl &)(Args ...), FArgs ...>>
+struct function_traits<std::_Binder<std::_Unforced, ReturnTypeT( &)(Args ...), FArgs ...>>
 #else
 #error "Unsupported C++ compiler / standard library"
 #endif

rclcpp/include/rclcpp/service.hpp

@@ -159,6 +159,7 @@ public:
       rclcpp_service_callback_added,
       (const void *)get_service_handle().get(),
       (const void *)&any_callback_);
+    any_callback_.register_callback_for_tracing();
   }
 
   Service(
@@ -181,6 +182,7 @@ public:
       rclcpp_service_callback_added,
       (const void *)get_service_handle().get(),
       (const void *)&any_callback_);
+    any_callback_.register_callback_for_tracing();
   }
 
   Service(
@@ -205,6 +207,7 @@ public:
       rclcpp_service_callback_added,
       (const void *)get_service_handle().get(),
       (const void *)&any_callback_);
+    any_callback_.register_callback_for_tracing();
   }
 
   Service() = delete;

rclcpp/src/rclcpp/clock.cpp

@@ -12,6 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <mutex>
+#include <shared_mutex>  // NOLINT
+#include <unordered_map>
+
+#include <cstdio>
+#include <cstdlib>
+#include <execinfo.h>
+#include <cxxabi.h>
+
 #include "rclcpp/clock.hpp"
 
 #include "rclcpp/exceptions.hpp"
@@ -21,6 +30,93 @@
 namespace rclcpp
 {
 
+std::shared_timed_mutex g_clock_map_mutex;
+std::unordered_map<rclcpp::Clock *, std::mutex> g_clock_mutex_map;
+
+/** Print a demangled stack backtrace of the caller function to FILE* out. */
+void print_backtrace(FILE *out = stderr, unsigned int max_frames = 63)
+{
+  fprintf(out, "stack trace:\n");
+
+  // storage array for stack trace address data
+  void* addrlist[max_frames+1];
+
+  // retrieve current stack addresses
+  int addrlen = backtrace(addrlist, sizeof(addrlist) / sizeof(void*));
+
+  if (addrlen == 0) {
+    fprintf(out, "  <empty, possibly corrupt>\n");
+    return;
+  }
+
+  // resolve addresses into strings containing "filename(function+address)",
+  // this array must be free()-ed
+  char** symbollist = backtrace_symbols(addrlist, addrlen);
+
+  // allocate string which will be filled with the demangled function name
+  size_t funcnamesize = 256;
+  char* funcname = (char*)malloc(funcnamesize);
+
+  // iterate over the returned symbol lines. skip the first, it is the
+  // address of this function.
+  for (int i = 1; i < addrlen; i++) {
+    char *begin_name = 0, *begin_offset = 0, *end_offset = 0;
+
+    // find parentheses and +address offset surrounding the mangled name:
+    // ./module(function+0x15c) [0x8048a6d]
+    for (char *p = symbollist[i]; *p; ++p) {
+      if (*p == '(') {
+        begin_name = p;
+      } else if (*p == '+') {
+        begin_offset = p;
+      } else if (*p == ')' && begin_offset) {
+        end_offset = p;
+        break;
+      }
+    }
+
+    if (begin_name && begin_offset && end_offset && begin_name < begin_offset) {
+      *begin_name++ = '\0';
+      *begin_offset++ = '\0';
+      *end_offset = '\0';
+
+      // mangled name is now in [begin_name, begin_offset) and caller
+      // offset in [begin_offset, end_offset). now apply
+      // __cxa_demangle():
+
+      int status;
+      char* ret = abi::__cxa_demangle(begin_name, funcname, &funcnamesize, &status);
+      if (status == 0) {
+        funcname = ret; // use possibly realloc()-ed string
+        fprintf(out, "  %s : %s+%s\n", symbollist[i], funcname, begin_offset);
+      }
+      else {
+        // demangling failed. Output function name as a C function with
+        // no arguments.
+        fprintf(out, "  %s : %s()+%s\n", symbollist[i], begin_name, begin_offset);
+      }
+    }
+    else {
+      // couldn't parse the line? print the whole line.
+      fprintf(out, "  %s\n", symbollist[i]);
+    }
+  }
+
+  free(funcname);
+  free(symbollist);
+}
+
+std::mutex & get_clock_mutex(rclcpp::Clock * clock)
+{
+  std::shared_lock<std::shared_timed_mutex> lk(g_clock_map_mutex);
+  try {
+    return g_clock_mutex_map.at(clock);
+  } catch (const std::out_of_range &) {
+    print_backtrace();
+    throw;
+  }
+}
+
 JumpHandler::JumpHandler(
   pre_callback_t pre_callback,
   post_callback_t post_callback,
@@ -37,10 +133,24 @@ Clock::Clock(rcl_clock_type_t clock_type)
   if (ret != RCL_RET_OK) {
     exceptions::throw_from_rcl_error(ret, "could not get current time stamp");
   }
+
+  {
+    std::lock_guard<std::shared_timed_mutex> lg(g_clock_map_mutex);
+    // Add a new default-constructed mutex, keyed off of this pointer.
+    fprintf(stderr, "Adding clock mutex for %p\n", static_cast<void *>(this));
+    g_clock_mutex_map[this];
+  }
 }
 
 Clock::~Clock()
 {
+  {
+    std::lock_guard<std::shared_timed_mutex> lg(g_clock_map_mutex);
+    // Remove the mutex corresponding to this clock object.
+    g_clock_mutex_map.erase(this);
+    fprintf(stderr, "Removed clock mutex for %p\n", static_cast<void *>(this));
+  }
+
   auto ret = rcl_clock_fini(&rcl_clock_);
   if (ret != RCL_RET_OK) {
     RCUTILS_LOG_ERROR("Failed to fini rcl clock.");
@@ -118,18 +228,25 @@ Clock::create_jump_callback(
     throw std::bad_alloc{};
   }
 
-  // Try to add the jump callback to the clock
-  rcl_ret_t ret = rcl_clock_add_jump_callback(
-    &rcl_clock_, threshold, Clock::on_time_jump,
-    handler.get());
-  if (RCL_RET_OK != ret) {
-    exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
+  {
+    fprintf(stderr, "create_jump_callback() getting clock mutex for %p\n", static_cast<void *>(this));
+    std::lock_guard<std::mutex> clock_guard(get_clock_mutex(this));
+    // Try to add the jump callback to the clock
+    rcl_ret_t ret = rcl_clock_add_jump_callback(
+      &rcl_clock_, threshold, Clock::on_time_jump,
+      handler.get());
+    if (RCL_RET_OK != ret) {
+      exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
+    }
   }
 
   // *INDENT-OFF*
   // create shared_ptr that removes the callback automatically when all copies are destructed
   // TODO(dorezyuk) UB, if the clock leaves scope before the JumpHandler
   return JumpHandler::SharedPtr(handler.release(), [this](JumpHandler * handler) noexcept {
+    fprintf(stderr, "create_jump_callback release lambda getting clock mutex for %p\n", static_cast<void *>(this));
+    std::lock_guard<std::mutex> clock_guard(get_clock_mutex(this));
+
     rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, Clock::on_time_jump,
         handler);
     delete handler;

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -321,9 +321,11 @@ rclcpp::Event::SharedPtr
 NodeGraph::get_graph_event()
 {
   auto event = rclcpp::Event::make_shared();
-  std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
-  graph_events_.push_back(event);
-  graph_users_count_++;
+  {
+    std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
+    graph_events_.push_back(event);
+    graph_users_count_++;
+  }
   // on first call, add node to graph_listener_
   if (should_add_to_graph_listener_.exchange(false)) {
     graph_listener_->add_node(this);

rclcpp/src/rclcpp/timer.cpp

@@ -17,7 +17,9 @@
 #include <chrono>
 #include <string>
 #include <memory>
+#include <mutex>
 
+#include "rclcpp/clock.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/exceptions.hpp"
 
@@ -40,11 +42,15 @@ TimerBase::TimerBase(
   timer_handle_ = std::shared_ptr<rcl_timer_t>(
     new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable
     {
-      if (rcl_timer_fini(timer) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str);
-        rcl_reset_error();
+      {
+        fprintf(stderr, "TimerBase handle lambda getting clock mutex for %p\n", static_cast<void *>(clock.get()));
+        std::lock_guard<std::mutex> clock_guard(get_clock_mutex(clock.get()));
+        if (rcl_timer_fini(timer) != RCL_RET_OK) {
+          RCUTILS_LOG_ERROR_NAMED(
+            "rclcpp",
+            "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str);
+          rcl_reset_error();
+        }
       }
       delete timer;
       // Captured shared pointers by copy, reset to make sure timer is finalized before clock
@@ -55,15 +61,19 @@ TimerBase::TimerBase(
   *timer_handle_.get() = rcl_get_zero_initialized_timer();
 
   rcl_clock_t * clock_handle = clock_->get_clock_handle();
-  if (
-    rcl_timer_init(
-      timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr,
-      rcl_get_default_allocator()) != RCL_RET_OK)
   {
-    RCUTILS_LOG_ERROR_NAMED(
-      "rclcpp",
-      "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
-    rcl_reset_error();
+    fprintf(stderr, "TimerBase constructor getting clock mutex for %p\n", static_cast<void *>(this->clock_.get()));
+    std::lock_guard<std::mutex> clock_guard(get_clock_mutex(this->clock_.get()));
+    if (
+      rcl_timer_init(
+        timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr,
+        rcl_get_default_allocator()) != RCL_RET_OK)
+    {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
+      rcl_reset_error();
+    }
   }
 }