Wip: Implement some separate spinning to avoid freezes

This does not yet work. Needs further debugging.

joint_to_cartesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h

@@ -45,6 +45,7 @@
 
 // ROS
 #include <geometry_msgs/PoseStamped.h>
+#include <ros/callback_queue.h>
 
 // ros_controls
 #include <controller_interface/controller.h>
@@ -55,6 +56,8 @@
 #include <kdl/chain.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
 
+#include <thread>
+
 namespace joint_to_cartesian_controller
 {
 
@@ -99,6 +102,8 @@ class JointToCartesianController
     ros::Publisher             m_pose_publisher;
 
     JointControllerAdapter     m_controller_adapter;
+    std::thread                m_adapter_thread;
+    ros::CallbackQueue         m_adapter_queue{true};
 
     KDL::Chain m_robot_chain;
     std::vector<
@@ -110,6 +115,7 @@ class JointToCartesianController
     std::shared_ptr<
       controller_manager::ControllerManager>  m_controller_manager;
 
+
 };
 
 }

joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp

@@ -38,11 +38,13 @@
 //-----------------------------------------------------------------------------
 
 // Pluginlib
+#include "ros/forwards.h"
 #include <pluginlib/class_list_macros.h>
 
 // Project
 #include <joint_to_cartesian_controller/joint_to_cartesian_controller.h>
 #include <cartesian_controller_base/Utility.h>
+#include <ros/callback_queue.h>
 
 // KDL
 #include <kdl/tree.hpp>
@@ -159,10 +161,26 @@ bool JointToCartesianController::init(hardware_interface::JointStateInterface* h
   m_positions.data = ctrl::VectorND::Zero(m_joint_handles.size());
   m_velocities.data = ctrl::VectorND::Zero(m_joint_handles.size());
 
+  // Put all of the service callbacks that the controller manager and the
+  // adapter will register at this controller's node handle into a separate
+  // callback queue.
+  nh.setCallbackQueue(&m_adapter_queue);
+  ROS_WARN_STREAM("Callback queue before empty: " << m_adapter_queue.empty());
+
   // Initialize controller adapter and according manager
   m_controller_adapter.init(m_joint_handles,nh);
   m_controller_manager.reset(new controller_manager::ControllerManager(&m_controller_adapter, nh));
 
+  ROS_WARN_STREAM("Callback queue after empty: " << m_adapter_queue.empty());
+
+  // Process m_controller_adapter's callbacks even when we are not running.
+  m_adapter_thread = std::thread([this] {
+    ros::AsyncSpinner spinner(2, &m_adapter_queue);
+    spinner.start();
+    ros::waitForShutdown();
+  });
+  m_adapter_thread.detach();  // gracefully die when our node (driver) shuts down.
+
   // Initialize forward kinematics solver
   m_fk_solver.reset(new KDL::ChainFkSolverPos_recursive(m_robot_chain));
 
@@ -188,6 +206,9 @@ void JointToCartesianController::update(const ros::Time& time, const ros::Durati
   // the joint state handles of this joint_to_cartesian_controller. So,
   // there's no need for a read() function.
 
+  // TODO:
+  // - We need to process the manager's callbacks idependently
+
   // Update connected joint controller
   m_controller_manager->update(time,period);