Adding a hybrid JointTrajectory/velocity-forwarding controller.

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

@@ -229,14 +229,7 @@ bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInt
 
   // Stop trajectory duration
   stop_trajectory_duration_ = 0.0;
-  if (!controller_nh_.getParam("stop_trajectory_duration", stop_trajectory_duration_))
-  {
-    // TODO: Remove this check/warning in Indigo
-    if (controller_nh_.getParam("hold_trajectory_duration", stop_trajectory_duration_))
-    {
-      ROS_WARN("The 'hold_trajectory_duration' has been deprecated in favor of the 'stop_trajectory_duration' parameter. Please update your controller configuration.");
-    }
-  }
+  controller_nh_.getParam("stop_trajectory_duration", stop_trajectory_duration_);
   ROS_DEBUG_STREAM_NAMED(name_, "Stop trajectory has a duration of " << stop_trajectory_duration_ << "s.");
 
   // Checking if partial trajectories are allowed

traj_or_jog_controller/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(traj_or_jog_controller)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin
+  REQUIRED COMPONENTS
+    realtime_tools
+    roscpp
+    std_msgs
+)
+
+find_package(joint_trajectory_controller REQUIRED)
+
+catkin_package(
+  CATKIN_DEPENDS
+    joint_trajectory_controller
+    realtime_tools
+    roscpp
+    std_msgs
+  INCLUDE_DIRS include
+  LIBRARIES traj_or_jog_controller
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${joint_trajectory_controller_INCLUDE_DIRS} ${Boost_INCLUDE_DIR})
+
+add_library(traj_or_jog_controller src/traj_or_jog_controller.cpp)
+target_link_libraries(traj_or_jog_controller ${catkin_LIBRARIES})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+install(FILES traj_or_jog_controller_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

traj_or_jog_controller/include/traj_or_jog_controller/export_traj_or_jog_controllers.h

@@ -0,0 +1,17 @@
+#ifndef EXPORT_TRAJ_OR_JOG_CONTROLLERS_H
+#define EXPORT_TRAJ_OR_JOG_CONTROLLERS_H
+
+// Set up namespacing of controllers and create their plugins.
+namespace traj_or_jog_controllers
+{
+  /**
+   * \brief A combination of a JointTrajectoryController with a ForwardJointGroupCommand controller.
+   */
+  typedef traj_or_jog_controller::TrajOrJogController<trajectory_interface::QuinticSplineSegment<double>,
+                                                                 hardware_interface::VelocityJointInterface>
+          TrajOrJogController;
+}
+
+PLUGINLIB_EXPORT_CLASS(traj_or_jog_controllers::TrajOrJogController, controller_interface::ControllerBase)
+
+#endif  // header guard

traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h

@@ -0,0 +1,78 @@
+#ifndef TRAJ_OR_JOG_CONTROLLER_H
+#define TRAJ_OR_JOG_CONTROLLER_H
+
+// Pluginlib
+#include <pluginlib/class_list_macros.hpp>
+
+// Project
+#include <actionlib/server/action_server.h>
+#include <joint_trajectory_controller/joint_trajectory_controller.h>
+#include <realtime_tools/realtime_buffer.h>
+#include <trajectory_interface/quintic_spline_segment.h>
+#include <std_msgs/Float64MultiArray.h>
+
+namespace traj_or_jog_controller
+{
+template <class SegmentImpl, class HardwareInterface>
+class TrajOrJogController : 
+public joint_trajectory_controller::JointTrajectoryController <SegmentImpl,HardwareInterface>
+{
+public:
+  /** \name Non Real-Time Safe Functions
+   *\{*/
+
+  /** \brief Override the init function of the base class. */
+  bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+  /*\}*/
+
+  /** \name Real-Time Safe Functions
+  *\{*/
+  void starting(const ros::Time& time)
+  {
+    // Start the base class, JointTrajectoryController
+    JointTrajectoryController::starting(time);
+
+    // Start the real-time velocity controller with 0.0 velocities
+    commands_buffer_.readFromRT()->assign(n_joints_, 0.0); 
+  }
+
+  /** \brief Override updates of the base class. */
+  void update(const ros::Time& time, const ros::Duration& period);
+  /*\}*/
+
+  realtime_tools::RealtimeBuffer<std::vector<double> > commands_buffer_;
+  unsigned int n_joints_;
+
+protected:
+  /** \brief Provide an action server for the execution of trajectories. */
+  typedef joint_trajectory_controller::JointTrajectoryController<SegmentImpl, HardwareInterface>  JointTrajectoryController;
+  typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::GoalHandle  GoalHandle;
+
+  ros::Subscriber  velocity_command_sub_;
+  bool allow_trajectory_execution_ = true; ///< Current mode.
+
+  /**
+   * \brief Callback for real-time JointGroupVelocityController commands.
+   * Incoming commands interrupt trajectory execution.
+   */
+  void velocityCommandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
+  {
+    // Disable trajectory execution since the real-time velocity command takes priority
+    allow_trajectory_execution_ = false;
+
+    if(msg->data.size()!=n_joints_)
+    { 
+      ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
+      return; 
+    }
+    commands_buffer_.writeFromNonRT(msg->data);
+  }
+
+  /**
+   * \brief Override the callback for the JointTrajectoryController action server.
+   */
+  void goalCB(GoalHandle gh);
+};
+}
+
+#endif  // header guard

traj_or_jog_controller/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>traj_or_jog_controller</name>
+  <version>0.0.0</version>
+  <description>Controller accepting trajectories or real-time jog commands.</description>
+
+  <maintainer email="andyz@utexas.edu">Andy Zelenak</maintainer>
+
+  <license>BSD</license>
+
+  <author email="andyz@utexas.edu">Andy Zelenak</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>controller_interface</depend>
+  <depend>joint_trajectory_controller</depend>
+  <depend>realtime_tools</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+
+  <export>
+    <controller_interface plugin="${prefix}/traj_or_jog_controller_plugins.xml"/>
+  </export>
+</package>

traj_or_jog_controller/src/traj_or_jog_controller.cpp

@@ -0,0 +1,69 @@
+#include <traj_or_jog_controller/traj_or_jog_controller.h>
+#include <traj_or_jog_controller/export_traj_or_jog_controllers.h>
+
+namespace traj_or_jog_controller
+{
+
+/** \brief Override the initializer of the base class. */
+template <class SegmentImpl, class HardwareInterface>
+bool TrajOrJogController<SegmentImpl, HardwareInterface>::init(HardwareInterface* hw,
+          ros::NodeHandle&   root_nh,
+          ros::NodeHandle&   controller_nh)
+{
+  // Initialize the base class, JointTrajectoryController
+  JointTrajectoryController::init(hw, root_nh, controller_nh);
+
+  // Add a subscriber for real-time velocity commands
+  velocity_command_sub_ = JointTrajectoryController::
+    controller_nh_.subscribe("velocity_command", 1, &TrajOrJogController::velocityCommandCB, this);
+
+  n_joints_ = JointTrajectoryController::joint_names_.size();
+
+  commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
+
+  return true;
+}
+
+template <class SegmentImpl, class HardwareInterface>
+void TrajOrJogController<SegmentImpl, HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
+{
+  // When updating, the real-time velocity controller takes priority over the trajectory controller
+  // The member variable allow_trajectory_execution_ determines which controller gets updated.
+
+  // If trajectory execution is not active
+  if ( !allow_trajectory_execution_ )
+  {
+    JointTrajectoryController::preemptActiveGoal();
+    std::vector<double> & command = *commands_buffer_.readFromRT();
+    for(unsigned int i=0; i<n_joints_; ++i)
+    {
+      JointTrajectoryController::joints_[i].setCommand(command[i]);
+    }
+  }
+  // If trajectory execution is allowed
+  else
+  {
+    // Update the base class, JointTrajectoryController
+    JointTrajectoryController::update(time, period);
+  }
+}
+
+/**
+ * \brief Override the callback for the JointTrajectoryController action server.
+ */
+template <class SegmentImpl, class HardwareInterface>
+void TrajOrJogController<SegmentImpl, HardwareInterface>::
+goalCB(GoalHandle gh)
+{
+  // Make sure trajectory execution is enabled.
+  // It will be interrupted by any new real-time commands.
+  allow_trajectory_execution_ = true;
+
+  // Reset the JointTrajectoryController to ensure it has current joint angles, etc.
+  JointTrajectoryController::stopping(ros::Time::now());
+  JointTrajectoryController::starting(ros::Time::now());
+
+  JointTrajectoryController::goalCB(gh);
+}
+
+}  // namespace traj_or_jog_controller

traj_or_jog_controller/traj_or_jog_controller_plugins.xml

@@ -0,0 +1,11 @@
+<library path="lib/libtraj_or_jog_controller">
+
+  <class name="traj_or_jog_controllers/TrajOrJogController"
+	type="traj_or_jog_controllers::TrajOrJogController"
+	base_class_type="controller_interface::ControllerBase">
+  <description>
+    TODO
+  </description>
+  </class>
+
+</library>