Add timer tracking for executor.

ros2 · Mar 28, 2018 · c94c152 · c94c152
1 parent cc48c11
commit c94c152
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 2 deletions.
diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt
@@ -329,6 +329,7 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_utilities
       "rcl")
     target_link_libraries(test_utilities ${PROJECT_NAME})
+  endif()
 
   ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
     APPEND_LIBRARY_DIRS "${append_library_dirs}")
@@ -337,7 +338,7 @@ if(BUILD_TESTING)
       "rcl")
     target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
   endif()
-endif()
+endif(BUILD_TESTING)
 
 ament_package(
   CONFIG_EXTRAS rclcpp-extras.cmake

diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
@@ -22,6 +22,7 @@
 #include <iostream>
 #include <list>
 #include <memory>
+#include <set>
 #include <string>
 #include <vector>
 
@@ -351,6 +352,7 @@ class Executor
   RCLCPP_DISABLE_COPY(Executor)
 
   std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+  std::set<rclcpp::TimerBase::SharedPtr> taken_timers_;
 };
 
 }  // namespace executor

diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
@@ -251,6 +251,10 @@ Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec)
   }
   if (any_exec->timer) {
     execute_timer(any_exec->timer);
+    auto it = taken_timers_.find(any_exec->timer);
+    if (it != taken_timers_.end()) {
+      taken_timers_.erase(it);
+    }
   }
   if (any_exec->subscription) {
     execute_subscription(any_exec->subscription);
@@ -522,10 +526,12 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
       }
       for (auto & timer_ref : group->get_timer_ptrs()) {
         auto timer = timer_ref.lock();
-        if (timer && timer->is_ready()) {
+
+        if (timer && timer->is_ready() && taken_timers_.count(timer) == 0) {
           any_exec->timer = timer;
           any_exec->callback_group = group;
           node = get_node_by_group(group);
+          taken_timers_.insert(timer);
           return;
         }
       }