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,39 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(traj_or_jog_controller)
+
+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/traj_or_jog_controller.h

@@ -0,0 +1,83 @@
+#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_; ///< 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
+ if (allow_trajectory_execution_)
+ {
+ allow_trajectory_execution_ = false;
+ JointTrajectoryController::setHoldPosition(ros::Time::now());
+ JointTrajectoryController::stopping(ros::Time::now());
+ }
+
+ 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="[email protected]">Andy Zelenak</maintainer>
+
+ <license>BSD</license>
+
+ <author email="[email protected]">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,85 @@
+#include <traj_or_jog_controller/traj_or_jog_controller.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));
+
+ allow_trajectory_execution_ = false;
+
+ 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.
+ if (!allow_trajectory_execution_)
+ {
+ // Reset the JointTrajectoryController to ensure it has current joint angles, etc.
+ JointTrajectoryController::starting(ros::Time::now());
+
+ allow_trajectory_execution_ = true;
+ }
+
+ JointTrajectoryController::goalCB(gh);
+}
+
+} // namespace traj_or_jog_controller
+
+// Set up namespacing of controllers and create their plugins.
+namespace velocity_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(velocity_controllers::TrajOrJogController, controller_interface::ControllerBase)

traj_or_jog_controller/traj_or_jog_controller_plugins.xml

@@ -0,0 +1,11 @@
+<library path="lib/libtraj_or_jog_controller">
+
+ <class name="velocity_controllers/TrajOrJogController"
+ type="velocity_controllers::TrajOrJogController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller accepting trajectories or velocity commands.
+ </description>
+ </class>
+
+</library>