run timer callback using asynch single threaded executor

moveit · Jul 8, 2020 · 1961c05 · 1961c05
1 parent 9382ede
commit 1961c05
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 3 deletions.
diff --git a/...ing/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/...ing/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
@@ -461,7 +461,7 @@ class PlanningSceneMonitor : private boost::noncopyable
   // TODO: (anasarrak) callbacks on ROS2?
   // https://answers.ros.org/question/300874/how-do-you-use-callbackgroups-as-a-replacement-for-callbackqueues-in-ros2/
   // ros::CallbackQueue queue_;
-  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> spinner_;
+  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> private_executor_;
 
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
 

diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
@@ -156,6 +156,7 @@ PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node,
   , dt_state_update_(0.0)
   , shape_transform_cache_lookup_wait_time_(0, 0)
   , rm_loader_(rm_loader)
+  , private_executor_(std::make_shared<rclcpp::executors::SingleThreadedExecutor>())
 {
   // use same callback queue as root_nh_
   // root_nh_.setCallbackQueue(&queue_);
@@ -177,7 +178,7 @@ PlanningSceneMonitor::~PlanningSceneMonitor()
   stopWorldGeometryMonitor();
   stopSceneMonitor();
 
-  spinner_.reset();
+  private_executor_.reset();
   // delete reconfigure_impl_;
   current_state_monitor_.reset();
   scene_const_.reset();
@@ -260,9 +261,16 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
   state_update_pending_ = false;
   // Period for 0.1 sec
   using std::chrono::nanoseconds;
+  std::shared_ptr<rclcpp::Node> pnode = std::make_shared<rclcpp::Node>(
+		  std::string(node_->get_fully_qualified_name()) + "_private");
   state_update_timer_ =
-      node_->create_wall_timer(dt_state_update_, std::bind(&PlanningSceneMonitor::stateUpdateTimerCallback, this));
+		  pnode->create_wall_timer(dt_state_update_, std::bind(&PlanningSceneMonitor::stateUpdateTimerCallback, this));
+  private_executor_->add_node(pnode);
 
+  // start executor on a different thread now
+  std::thread([this](){
+	  private_executor_->spin();
+  }).detach();
   // reconfigure_impl_ = new DynamicReconfigureImpl(this);
 }