Merge pull request #161 from ipa-mdl/rt_group_controller

thread-safe forward controllers

effort_controllers/src/joint_effort_controller.cpp

@@ -41,7 +41,7 @@ template <class T>
 void forward_command_controller::ForwardCommandController<T>::starting(const ros::Time& time)
 {
   // Start controller with 0.0 effort
-  command_ = 0.0;
+  command_buffer_.writeFromNonRT(0.0);
 }
 
 

effort_controllers/src/joint_group_effort_controller.cpp

@@ -42,7 +42,7 @@ template <class T>
 void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
 {
   // Start controller with 0.0 efforts
-  commands_.resize(n_joints_, 0.0);
+  commands_buffer_.readFromRT()->assign(n_joints_, 0.0);
 }
 
 

forward_command_controller/CMakeLists.txt

@@ -2,11 +2,11 @@ cmake_minimum_required(VERSION 2.8.3)
 project(forward_command_controller)
 
 # Load catkin and all dependencies required for this package
-find_package(catkin REQUIRED COMPONENTS controller_interface hardware_interface std_msgs)
+find_package(catkin REQUIRED COMPONENTS controller_interface hardware_interface std_msgs realtime_tools)
 
 # Declare catkin package
 catkin_package(
-  CATKIN_DEPENDS controller_interface hardware_interface std_msgs
+  CATKIN_DEPENDS controller_interface hardware_interface std_msgs realtime_tools
   INCLUDE_DIRS include
   )
 

forward_command_controller/include/forward_command_controller/forward_command_controller.h

@@ -42,6 +42,7 @@
 #include <hardware_interface/joint_command_interface.h>
 #include <controller_interface/controller.h>
 #include <std_msgs/Float64.h>
+#include <realtime_tools/realtime_buffer.h>
 
 
 namespace forward_command_controller
@@ -68,7 +69,7 @@ template <class T>
 class ForwardCommandController: public controller_interface::Controller<T>
 {
 public:
-  ForwardCommandController() : command_(0) {}
+  ForwardCommandController() {}
   ~ForwardCommandController() {sub_command_.shutdown();}
 
   bool init(T* hw, ros::NodeHandle &n)
@@ -85,14 +86,14 @@ class ForwardCommandController: public controller_interface::Controller<T>
   }
 
   void starting(const ros::Time& time);
-  void update(const ros::Time& time, const ros::Duration& period) {joint_.setCommand(command_);}
+  void update(const ros::Time& time, const ros::Duration& period) {joint_.setCommand(*command_buffer_.readFromRT());}
 
   hardware_interface::JointHandle joint_;
-  double command_;
+  realtime_tools::RealtimeBuffer<double> command_buffer_;
 
 private:
   ros::Subscriber sub_command_;
-  void commandCB(const std_msgs::Float64ConstPtr& msg) {command_ = msg->data;}
+  void commandCB(const std_msgs::Float64ConstPtr& msg) {command_buffer_.writeFromNonRT(msg->data);}
 };
 
 }

forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h

@@ -46,6 +46,7 @@
 #include <hardware_interface/joint_command_interface.h>
 #include <controller_interface/controller.h>
 #include <std_msgs/Float64MultiArray.h>
+#include <realtime_tools/realtime_buffer.h>
 
 
 namespace forward_command_controller
@@ -72,7 +73,7 @@ template <class T>
 class ForwardJointGroupCommandController: public controller_interface::Controller<T>
 {
 public:
-  ForwardJointGroupCommandController() { commands_.clear(); }
+  ForwardJointGroupCommandController() {}
   ~ForwardJointGroupCommandController() {sub_command_.shutdown();}
 
   bool init(T* hw, ros::NodeHandle &n)
@@ -85,7 +86,11 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle
       return false;
     }
     n_joints_ = joint_names_.size();
-
+
+    if(n_joints_ == 0){
+      ROS_ERROR_STREAM("List of joint names is empty.");
+      return false;
+    }
     for(unsigned int i=0; i<n_joints_; i++)
     {
       try
@@ -98,21 +103,24 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle
         return false;
       }
     }
-
+
+    commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
+
     sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &ForwardJointGroupCommandController::commandCB, this);
     return true;
   }
 
   void starting(const ros::Time& time);
   void update(const ros::Time& time, const ros::Duration& period) 
   {
+    std::vector<double> & commands = *commands_buffer_.readFromRT();
     for(unsigned int i=0; i<n_joints_; i++)
-    {  joints_[i].setCommand(commands_[i]);  }
+    {  joints_[i].setCommand(commands[i]);  }
   }
 
   std::vector< std::string > joint_names_;
   std::vector< hardware_interface::JointHandle > joints_;
-  std::vector< double > commands_;
+  realtime_tools::RealtimeBuffer<std::vector<double> > commands_buffer_;
   unsigned int n_joints_;
 
 private:
@@ -124,8 +132,7 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle
       ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
       return; 
     }
-    for(unsigned int i=0; i<n_joints_; i++)
-    {  commands_[i] = msg->data[i];  }
+    commands_buffer_.writeFromNonRT(msg->data);
   }
 };
 

forward_command_controller/package.xml

@@ -16,10 +16,12 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>controller_interface</build_depend> 
   <build_depend>hardware_interface</build_depend> 
-  <build_depend>std_msgs</build_depend> 
+  <build_depend>std_msgs</build_depend>
+  <build_depend>realtime_tools</build_depend>
   <run_depend>controller_interface</run_depend> 
   <run_depend>hardware_interface</run_depend> 
   <run_depend>std_msgs</run_depend> 
+  <run_depend>realtime_tools</run_depend>
 
   <export>
     <cpp cflags="-I${prefix}/include"/>

position_controllers/src/joint_group_position_controller.cpp

@@ -42,10 +42,10 @@ template <class T>
 void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
 {
   // Start controller with current joint positions
-  commands_.resize(n_joints_, 0.0);
+  std::vector<double> & commands = *commands_buffer_.readFromRT();
   for(unsigned int i=0; i<joints_.size(); i++)
   {
-    commands_[i]=joints_[i].getPosition();
+    commands[i]=joints_[i].getPosition();
   }
 }
 

position_controllers/src/joint_position_controller.cpp

@@ -41,7 +41,7 @@ template <class T>
 void forward_command_controller::ForwardCommandController<T>::starting(const ros::Time& time)
 {
   // Start controller with current joint position
-  command_ = joint_.getPosition();
+  command_buffer_.writeFromNonRT(joint_.getPosition());
 }
 
 PLUGINLIB_EXPORT_CLASS(position_controllers::JointPositionController,controller_interface::ControllerBase)

velocity_controllers/src/joint_group_velocity_controller.cpp

@@ -42,7 +42,7 @@ template <class T>
 void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
 {
   // Start controller with 0.0 velocities
-  commands_.resize(n_joints_, 0.0);
+  commands_buffer_.readFromRT()->assign(n_joints_, 0.0);
 }
 
 

velocity_controllers/src/joint_velocity_controller.cpp

@@ -41,7 +41,7 @@ template <class T>
 void forward_command_controller::ForwardCommandController<T>::starting(const ros::Time& time)
 {
   // Start controller with 0.0 velocity
-  command_ = 0.0;
+  command_buffer_.writeFromNonRT(0.0);
 }