From 9f82fffdbe87d8af3a2866b16e70076d1f2de84d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 1 Nov 2018 11:56:38 -0500 Subject: [PATCH 1/6] Adding a new, hybrid JointTrajectory/velocity-forwarding controller. --- .../joint_trajectory_controller_impl.h | 9 +-- traj_or_jog_controller/CMakeLists.txt | 40 ++++++++++ .../export_traj_or_jog_controllers.h | 17 ++++ .../traj_or_jog_controller.h | 78 +++++++++++++++++++ traj_or_jog_controller/package.xml | 23 ++++++ .../src/traj_or_jog_controller.cpp | 69 ++++++++++++++++ .../traj_or_jog_controller_plugins.xml | 11 +++ 7 files changed, 239 insertions(+), 8 deletions(-) create mode 100644 traj_or_jog_controller/CMakeLists.txt create mode 100644 traj_or_jog_controller/include/traj_or_jog_controller/export_traj_or_jog_controllers.h create mode 100644 traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h create mode 100644 traj_or_jog_controller/package.xml create mode 100644 traj_or_jog_controller/src/traj_or_jog_controller.cpp create mode 100644 traj_or_jog_controller/traj_or_jog_controller_plugins.xml diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 5c0e75ede..126760af3 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -229,14 +229,7 @@ bool JointTrajectoryController::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 diff --git a/traj_or_jog_controller/CMakeLists.txt b/traj_or_jog_controller/CMakeLists.txt new file mode 100644 index 000000000..a601392ce --- /dev/null +++ b/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}) \ No newline at end of file diff --git a/traj_or_jog_controller/include/traj_or_jog_controller/export_traj_or_jog_controllers.h b/traj_or_jog_controller/include/traj_or_jog_controller/export_traj_or_jog_controllers.h new file mode 100644 index 000000000..96b659cda --- /dev/null +++ b/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, + hardware_interface::VelocityJointInterface> + TrajOrJogController; +} + +PLUGINLIB_EXPORT_CLASS(traj_or_jog_controllers::TrajOrJogController, controller_interface::ControllerBase) + +#endif // header guard \ No newline at end of file diff --git a/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h b/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h new file mode 100644 index 000000000..387850b99 --- /dev/null +++ b/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 + +// Project +#include +#include +#include +#include +#include + +namespace traj_or_jog_controller +{ +template +class TrajOrJogController : +public joint_trajectory_controller::JointTrajectoryController +{ +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 > commands_buffer_; + unsigned int n_joints_; + +protected: + /** \brief Provide an action server for the execution of trajectories. */ + typedef joint_trajectory_controller::JointTrajectoryController JointTrajectoryController; + typedef actionlib::ActionServer::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 \ No newline at end of file diff --git a/traj_or_jog_controller/package.xml b/traj_or_jog_controller/package.xml new file mode 100644 index 000000000..ecbd8780f --- /dev/null +++ b/traj_or_jog_controller/package.xml @@ -0,0 +1,23 @@ + + + traj_or_jog_controller + 0.0.0 + Controller accepting trajectories or real-time jog commands. + + Andy Zelenak + + BSD + + Andy Zelenak + + catkin + controller_interface + joint_trajectory_controller + realtime_tools + roscpp + std_msgs + + + + + \ No newline at end of file diff --git a/traj_or_jog_controller/src/traj_or_jog_controller.cpp b/traj_or_jog_controller/src/traj_or_jog_controller.cpp new file mode 100644 index 000000000..4be7a7011 --- /dev/null +++ b/traj_or_jog_controller/src/traj_or_jog_controller.cpp @@ -0,0 +1,69 @@ +#include +#include + +namespace traj_or_jog_controller +{ + +/** \brief Override the initializer of the base class. */ +template +bool TrajOrJogController::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(n_joints_, 0.0)); + + return true; +} + +template +void TrajOrJogController::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 & command = *commands_buffer_.readFromRT(); + for(unsigned int i=0; i +void TrajOrJogController:: +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 \ No newline at end of file diff --git a/traj_or_jog_controller/traj_or_jog_controller_plugins.xml b/traj_or_jog_controller/traj_or_jog_controller_plugins.xml new file mode 100644 index 000000000..1cc1986f1 --- /dev/null +++ b/traj_or_jog_controller/traj_or_jog_controller_plugins.xml @@ -0,0 +1,11 @@ + + + + + TODO + + + + From 9b0408124792d0734cbebc96d4da00cd4e29623e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 21 Nov 2018 13:01:50 -0600 Subject: [PATCH 2/6] Addressing minor concerns of ros_controllers PR #383. --- traj_or_jog_controller/CMakeLists.txt | 5 ++--- .../export_traj_or_jog_controllers.h | 17 ----------------- .../traj_or_jog_controller.h | 2 +- traj_or_jog_controller/package.xml | 2 +- .../src/traj_or_jog_controller.cpp | 16 ++++++++++++++-- .../traj_or_jog_controller_plugins.xml | 6 +++--- 6 files changed, 21 insertions(+), 27 deletions(-) delete mode 100644 traj_or_jog_controller/include/traj_or_jog_controller/export_traj_or_jog_controllers.h diff --git a/traj_or_jog_controller/CMakeLists.txt b/traj_or_jog_controller/CMakeLists.txt index a601392ce..036a123fe 100644 --- a/traj_or_jog_controller/CMakeLists.txt +++ b/traj_or_jog_controller/CMakeLists.txt @@ -1,8 +1,6 @@ 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 @@ -37,4 +35,5 @@ install(TARGETS ${PROJECT_NAME} ) install(FILES traj_or_jog_controller_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) \ No newline at end of file + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + \ No newline at end of file diff --git a/traj_or_jog_controller/include/traj_or_jog_controller/export_traj_or_jog_controllers.h b/traj_or_jog_controller/include/traj_or_jog_controller/export_traj_or_jog_controllers.h deleted file mode 100644 index 96b659cda..000000000 --- a/traj_or_jog_controller/include/traj_or_jog_controller/export_traj_or_jog_controllers.h +++ /dev/null @@ -1,17 +0,0 @@ -#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, - hardware_interface::VelocityJointInterface> - TrajOrJogController; -} - -PLUGINLIB_EXPORT_CLASS(traj_or_jog_controllers::TrajOrJogController, controller_interface::ControllerBase) - -#endif // header guard \ No newline at end of file diff --git a/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h b/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h index 387850b99..c38204ee0 100644 --- a/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h +++ b/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h @@ -75,4 +75,4 @@ public joint_trajectory_controller::JointTrajectoryController - \ No newline at end of file + diff --git a/traj_or_jog_controller/src/traj_or_jog_controller.cpp b/traj_or_jog_controller/src/traj_or_jog_controller.cpp index 4be7a7011..a803db2b3 100644 --- a/traj_or_jog_controller/src/traj_or_jog_controller.cpp +++ b/traj_or_jog_controller/src/traj_or_jog_controller.cpp @@ -1,5 +1,4 @@ #include -#include namespace traj_or_jog_controller { @@ -66,4 +65,17 @@ goalCB(GoalHandle gh) JointTrajectoryController::goalCB(gh); } -} // namespace traj_or_jog_controller \ No newline at end of file +} // 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, + hardware_interface::VelocityJointInterface> + TrajOrJogController; +} + +PLUGINLIB_EXPORT_CLASS(velocity_controllers::TrajOrJogController, controller_interface::ControllerBase) \ No newline at end of file diff --git a/traj_or_jog_controller/traj_or_jog_controller_plugins.xml b/traj_or_jog_controller/traj_or_jog_controller_plugins.xml index 1cc1986f1..e611943e7 100644 --- a/traj_or_jog_controller/traj_or_jog_controller_plugins.xml +++ b/traj_or_jog_controller/traj_or_jog_controller_plugins.xml @@ -1,10 +1,10 @@ - - TODO + A controller accepting trajectories or velocity commands. From f340f03e83cc564930772cca7c45fac83122830d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 21 Nov 2018 13:05:58 -0600 Subject: [PATCH 3/6] Incoming real-time commands preempt active trajectory goals. --- .../include/traj_or_jog_controller/traj_or_jog_controller.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h b/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h index c38204ee0..c1772f738 100644 --- a/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h +++ b/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h @@ -58,7 +58,11 @@ public joint_trajectory_controller::JointTrajectoryController data.size()!=n_joints_) { From eb5503804d85783157b99b1cc208880ecd12fd36 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 21 Nov 2018 14:00:37 -0600 Subject: [PATCH 4/6] Placing an explicit stopping() when a trajectory is interrupted by a velocity command. --- .../include/traj_or_jog_controller/traj_or_jog_controller.h | 5 +++-- traj_or_jog_controller/src/traj_or_jog_controller.cpp | 4 +++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h b/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h index c1772f738..76e52b295 100644 --- a/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h +++ b/traj_or_jog_controller/include/traj_or_jog_controller/traj_or_jog_controller.h @@ -49,7 +49,7 @@ public joint_trajectory_controller::JointTrajectoryController ::GoalHandle GoalHandle; ros::Subscriber velocity_command_sub_; - bool allow_trajectory_execution_ = true; ///< Current mode. + bool allow_trajectory_execution_; ///< Current mode. /** * \brief Callback for real-time JointGroupVelocityController commands. @@ -61,7 +61,8 @@ public joint_trajectory_controller::JointTrajectoryController data.size()!=n_joints_) diff --git a/traj_or_jog_controller/src/traj_or_jog_controller.cpp b/traj_or_jog_controller/src/traj_or_jog_controller.cpp index a803db2b3..2e7078593 100644 --- a/traj_or_jog_controller/src/traj_or_jog_controller.cpp +++ b/traj_or_jog_controller/src/traj_or_jog_controller.cpp @@ -20,6 +20,8 @@ bool TrajOrJogController::init(HardwareInterface commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + allow_trajectory_execution_ = false; + return true; } @@ -78,4 +80,4 @@ namespace velocity_controllers TrajOrJogController; } -PLUGINLIB_EXPORT_CLASS(velocity_controllers::TrajOrJogController, controller_interface::ControllerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(velocity_controllers::TrajOrJogController, controller_interface::ControllerBase) From d3f0a977f3f02a83fe8554393651610874f8fb80 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 21 Nov 2018 15:32:12 -0600 Subject: [PATCH 5/6] In the JointTrajectoryController CB, stopping() is not necessary. --- traj_or_jog_controller/src/traj_or_jog_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/traj_or_jog_controller/src/traj_or_jog_controller.cpp b/traj_or_jog_controller/src/traj_or_jog_controller.cpp index 2e7078593..9d5ef9db7 100644 --- a/traj_or_jog_controller/src/traj_or_jog_controller.cpp +++ b/traj_or_jog_controller/src/traj_or_jog_controller.cpp @@ -61,7 +61,6 @@ goalCB(GoalHandle gh) 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); From d399cf7aca26005b0d3449ba8c087d2e4e32c4d9 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 21 Nov 2018 22:34:17 -0600 Subject: [PATCH 6/6] Only restart the trajectory controller if necessary. --- traj_or_jog_controller/src/traj_or_jog_controller.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/traj_or_jog_controller/src/traj_or_jog_controller.cpp b/traj_or_jog_controller/src/traj_or_jog_controller.cpp index 9d5ef9db7..7edc5bd0e 100644 --- a/traj_or_jog_controller/src/traj_or_jog_controller.cpp +++ b/traj_or_jog_controller/src/traj_or_jog_controller.cpp @@ -58,10 +58,13 @@ goalCB(GoalHandle gh) { // Make sure trajectory execution is enabled. // It will be interrupted by any new real-time commands. - allow_trajectory_execution_ = true; + if (!allow_trajectory_execution_) + { + // Reset the JointTrajectoryController to ensure it has current joint angles, etc. + JointTrajectoryController::starting(ros::Time::now()); - // Reset the JointTrajectoryController to ensure it has current joint angles, etc. - JointTrajectoryController::starting(ros::Time::now()); + allow_trajectory_execution_ = true; + } JointTrajectoryController::goalCB(gh); }