From 604d8536d80ecff625ea1855da90de092d7325a9 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Fri, 4 Jun 2021 18:38:11 -0700 Subject: [PATCH 1/5] Added actionlib interface to forward_joint_group_command_controller --- .../forward_joint_group_command_controller.h | 114 ++++--- ...ward_joint_group_command_controller_impl.h | 304 ++++++++++++++++++ 2 files changed, 365 insertions(+), 53 deletions(-) create mode 100644 forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h index a0f9e166a..636e90574 100644 --- a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h +++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h @@ -42,10 +42,20 @@ #include #include +#include + +// actionlib +#include + +// ROS messages +#include +#include +#include + +// ros_controls +#include #include #include -#include -#include namespace forward_command_controller @@ -68,71 +78,69 @@ namespace forward_command_controller * Subscribes to: * - \b command (std_msgs::Float64MultiArray) : The joint commands to apply. */ -template -class ForwardJointGroupCommandController: public controller_interface::Controller +template +class ForwardJointGroupCommandController: public controller_interface::Controller { public: ForwardJointGroupCommandController() {} ~ForwardJointGroupCommandController() {sub_command_.shutdown();} - bool init(T* hw, ros::NodeHandle &n) - { - // List of controlled joints - std::string param_name = "joints"; - if(!n.getParam(param_name, joint_names_)) - { - ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ")."); - 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; igetHandle(joint_names_[i])); - } - catch (const hardware_interface::HardwareInterfaceException& e) - { - ROS_ERROR_STREAM("Exception thrown: " << e.what()); - return false; - } - } - - commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); - - sub_command_ = n.subscribe("command", 1, &ForwardJointGroupCommandController::commandCB, this); - return true; - } + bool init(HardwareInterface* hw, ros::NodeHandle &n); - void starting(const ros::Time& time); - void update(const ros::Time& /*time*/, const ros::Duration& /*period*/) + void starting(const ros::Time& /*time*/); + void stopping(const ros::Time& /*time*/) {preemptActiveGoal();} + void update(const ros::Time& time, const ros::Duration& /*period*/) { std::vector & commands = *commands_buffer_.readFromRT(); for(unsigned int i=0; i joint_names_; - std::vector< hardware_interface::JointHandle > joints_; +protected: + typedef actionlib::ActionServer ActionServer; + typedef std::shared_ptr ActionServerPtr; + typedef ActionServer::GoalHandle GoalHandle; + typedef realtime_tools::RealtimeServerGoalHandle RealtimeGoalHandle; + typedef boost::shared_ptr RealtimeGoalHandlePtr; + realtime_tools::RealtimeBuffer > commands_buffer_; + std::vector default_commands_; // Defaults to 0 on init, can override in starting() unsigned int n_joints_; + std::vector< std::string > joint_names_; ///< Controlled joint names. + std::vector< hardware_interface::JointHandle > joints_; ///< Handle to controlled joint. + std::string name_; ///< Controller name. + RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any. + + // ROS API + ros::NodeHandle controller_nh_; + ros::Subscriber sub_command_; + ActionServerPtr action_server_; + ros::Timer goal_handle_timer_; + ros::Timer goal_duration_timer_; + ros::Duration action_monitor_period_; + + void goalCB(GoalHandle gh); + void cancelCB(GoalHandle gh); + void timeoutCB(const ros::TimerEvent& event); + void preemptActiveGoal(); + void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg); + + // Defaults to no change in default command + void updateDefaultCommand() {} + private: - ros::Subscriber sub_command_; - void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg) - { - 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 Updates the pre-allocated feedback of the current active goal (if any) + * based on the current state values. + * + * @note This function is NOT thread safe but intended to be used in the + * update-function. + */ + void setActionFeedback(const ros::Time& time); }; -} +} // namespace + +#include diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h new file mode 100644 index 000000000..e781c6363 --- /dev/null +++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h @@ -0,0 +1,304 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2021, Personal Robotics Lab +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Personal Robotics Lab nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Ethan Kroll Gordon + +#pragma once + + +namespace forward_command_controller +{ + +namespace internal +{ +// TODO: create a utils file? +/** + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \t2 indices. + * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is + * "{2, 1}". + */ +template +inline std::vector mapping(const T& t1, const T& t2) +{ + typedef unsigned int SizeType; + + // t1 must be a subset of t2 + if (t1.size() > t2.size()) {return std::vector();} + + std::vector mapping_vector(t1.size()); // Return value + for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) + { + typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it); + if (t2.end() == t2_it) {return std::vector();} + else + { + const SizeType t1_dist = std::distance(t1.begin(), t1_it); + const SizeType t2_dist = std::distance(t2.begin(), t2_it); + mapping_vector[t1_dist] = t2_dist; + } + } + return mapping_vector; +} + +std::string getLeafNamespace(const ros::NodeHandle& nh) +{ + const std::string complete_ns = nh.getNamespace(); + std::size_t id = complete_ns.find_last_of("/"); + return complete_ns.substr(id + 1); +} + +} // namespace + +template +bool ForwardJointGroupCommandController::init(HardwareInterface* hw, + ros::NodeHandle& n) +{ + // Cache controller node handle + controller_nh_ = n; + + // Controller name + name_ = internal::getLeafNamespace(controller_nh_); + + // Action status checking update rate + double action_monitor_rate = 20.0; + controller_nh_.getParam("action_monitor_rate", action_monitor_rate); + action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate); + ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz."); + + // Initialize controlled joints + std::string param_name = "joints"; + if(!n.getParam(param_name, joint_names_)) + { + ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ")."); + 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; igetHandle(joint_names_[i])); + } + catch (const hardware_interface::HardwareInterfaceException& e) + { + ROS_ERROR_STREAM("Exception thrown: " << e.what()); + return false; + } + } + + commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + default_commands_.resize(n_joints_); + + // ROS API: Subscribed topics + sub_command_ = n.subscribe("command", 1, &ForwardJointGroupCommandController::commandCB, this); + + // ROS API: Action interface + action_server_.reset(new ActionServer(controller_nh_, "joint_group_command", + boost::bind(&ForwardJointGroupCommandController::goalCB, this, _1), + boost::bind(&ForwardJointGroupCommandController::cancelCB, this, _1), + false)); + action_server_->start(); + return true; +} + +template +void ForwardJointGroupCommandController::preemptActiveGoal() +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + + // Cancel any goal timeout + goal_duration_timer_.stop(); + + // Cancels the currently active goal + if (current_active_goal) + { + // Marks the current goal as canceled + rt_active_goal_.reset(); + current_active_goal->gh_.setCanceled(); + } +} + +template +void ForwardJointGroupCommandController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg) +{ + // Preconditions + if (!this->isRunning()) + { + ROS_ERROR_STREAM_NAMED(name_, "Can't accept new commands. Controller is not running."); + return; + } + + if (!msg) + { + ROS_WARN_STREAM_NAMED(name_, "Received null-pointer message, skipping."); + return; + } + + if(msg->data.size()!=n_joints_) + { + ROS_ERROR_STREAM_NAMED(name_, "Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!"); + return; + } + + commands_buffer_.writeFromNonRT(msg->data); + preemptActiveGoal(); +} + +template +void ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal"); + control_msgs::JointGroupCommandResult result; + + // Preconditions + if (!this->isRunning()) + { + result.error_string = "Can't accept new action goals. Controller is not running."; + ROS_ERROR_STREAM_NAMED(name_, result.error_string); + result.error_code = control_msgs::JointGroupCommandResult::INVALID_GOAL; + gh.setRejected(result); + return; + } + + if (gh.getGoal()->joint_names.size() != gh.getGoal()->command.size()) { + result.error_string = "Size of command must match size of joint_names."; + ROS_ERROR_STREAM_NAMED(name_, result.error_string); + result.error_code = control_msgs::JointGroupCommandResult::INVALID_GOAL; + gh.setRejected(result); + return; + } + + // Goal should specify valid controller joints (they can be ordered differently). Reject if this is not the case + using internal::mapping; + std::vector mapping_vector = mapping(gh.getGoal()->joint_names, joint_names_); + + if (mapping_vector.size() != gh.getGoal()->joint_names.size()) + { + result.error_string = "Joints on incoming goal don't match the controller joints."; + ROS_ERROR_STREAM_NAMED(name_, result.error_string); + result.error_code = control_msgs::JointGroupCommandResult::INVALID_JOINTS; + gh.setRejected(result); + return; + } + + // update new command + RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh)); + std::vector< double > new_commands = default_commands_; + for(int i = 0; i < mapping_vector.size(); i++) { + new_commands[mapping_vector[i]] = gh.getGoal()->command[i]; + } + rt_goal->preallocated_feedback_->joint_names = joint_names_; + commands_buffer_.writeFromNonRT(new_commands); + + // Accept new goal + preemptActiveGoal(); + gh.setAccepted(); + rt_active_goal_ = rt_goal; + + // Setup goal status checking timer + goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_, + &RealtimeGoalHandle::runNonRealtime, + rt_goal); + goal_handle_timer_.start(); + + // Setup goal timeout + if (gh.getGoal()->timeout > ros::Duration()) { + goal_duration_timer_ = controller_nh_.createTimer(gh.getGoal()->timeout, + &ForwardJointGroupCommandController::timeoutCB, + this, + true); + goal_duration_timer_.start(); + } +} + +template +void ForwardJointGroupCommandController::timeoutCB(const ros::TimerEvent& event) +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + + // Check that timeout refers to currently active goal (if any) + if (current_active_goal) { + ROS_DEBUG_NAMED(name_, "Active action goal reached requested timeout."); + + // Give sub-classes option to update default_commands_ + updateDefaultCommand(); + commands_buffer_.writeFromNonRT(default_commands_); + + // Marks the current goal as succeeded + rt_active_goal_.reset(); + current_active_goal->gh_.setSucceeded(); + } +} + +template +void ForwardJointGroupCommandController::cancelCB(GoalHandle gh) +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + + // Check that cancel request refers to currently active goal + if (current_active_goal && current_active_goal->gh_ == gh) + { + ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib."); + + // Give sub-classes option to update default_commands_ + updateDefaultCommand(); + commands_buffer_.writeFromNonRT(default_commands_); + + preemptActiveGoal(); + } +} + +template +void ForwardJointGroupCommandController::setActionFeedback(const ros::Time& time) +{ + RealtimeGoalHandlePtr current_active_goal(rt_active_goal_); + if (!current_active_goal) + { + return; + } + + current_active_goal->preallocated_feedback_->header.stamp = time; + current_active_goal->preallocated_feedback_->desired = *commands_buffer_.readFromRT(); + current_active_goal->preallocated_feedback_->actual.positions.clear(); + current_active_goal->preallocated_feedback_->actual.velocities.clear(); + current_active_goal->preallocated_feedback_->actual.effort.clear(); + for (auto j : joints_) + { + current_active_goal->preallocated_feedback_->actual.positions.push_back(j.getPosition()); + current_active_goal->preallocated_feedback_->actual.velocities.push_back(j.getVelocity()); + current_active_goal->preallocated_feedback_->actual.effort.push_back(j.getEffort()); + } + + current_active_goal->setFeedback( current_active_goal->preallocated_feedback_ ); +} + +} // namespace From ac91897d4c781810d4fd6b13ac4dfca995a8ca03 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Mon, 7 Jun 2021 10:27:17 -0700 Subject: [PATCH 2/5] Add default command update in joint group controller subclasses --- .../src/joint_group_effort_controller.cpp | 11 +++++++++++ .../forward_joint_group_command_controller.h | 2 +- .../src/joint_group_position_controller.cpp | 11 +++++++++++ .../src/joint_group_velocity_controller.cpp | 10 ++++++++++ 4 files changed, 33 insertions(+), 1 deletion(-) diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index 6c495797c..0651b422a 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -45,5 +45,16 @@ void forward_command_controller::ForwardJointGroupCommandController::starting commands_buffer_.readFromRT()->assign(n_joints_, 0.0); } +template +void forward_command_controller::ForwardJointGroupCommandController::updateDefaultCommand() +{ + // Set defaults to 0 + for(unsigned int i=0; i::starting for(unsigned int i=0; i +void forward_command_controller::ForwardJointGroupCommandController::updateDefaultCommand() +{ + // Set default to current position + for(unsigned int i=0; i::starting commands_buffer_.readFromRT()->assign(n_joints_, 0.0); } +template +void forward_command_controller::ForwardJointGroupCommandController::updateDefaultCommand() +{ + // Set default to 0 + for(unsigned int i=0; i Date: Thu, 10 Jun 2021 15:37:15 -0700 Subject: [PATCH 3/5] Add actionlib as build and exec dependency --- forward_command_controller/CMakeLists.txt | 2 ++ forward_command_controller/package.xml | 2 ++ 2 files changed, 4 insertions(+) diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 4b364a959..179b8b759 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -8,6 +8,7 @@ endif() # Load catkin and all dependencies required for this package find_package(catkin REQUIRED COMPONENTS + actionlib controller_interface hardware_interface realtime_tools @@ -20,6 +21,7 @@ catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS + actionlib controller_interface hardware_interface realtime_tools diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 916460241..0d2d72d18 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -22,6 +22,8 @@ catkin + actionlib + controller_interface hardware_interface realtime_tools From 6012a4bfd83ead60179897c4d6e3b12138836aac Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 12 Aug 2021 15:57:44 -0700 Subject: [PATCH 4/5] Handle explicit command type in updated control_msg/JointGroupCommandAction --- .../src/joint_group_effort_controller.cpp | 7 ++++++- .../forward_joint_group_command_controller.h | 4 ++++ .../forward_joint_group_command_controller_impl.h | 14 ++++++++------ .../src/joint_group_position_controller.cpp | 6 ++++++ .../src/joint_group_velocity_controller.cpp | 6 ++++++ 5 files changed, 30 insertions(+), 7 deletions(-) diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index 0651b422a..1d4d5cc8e 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -55,6 +55,11 @@ void forward_command_controller::ForwardJointGroupCommandController::updateDe } } - +template +void forward_command_controller::ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + // Set as position command + setGoal(gh, gh.getGoal()->command.effort); +} PLUGINLIB_EXPORT_CLASS(effort_controllers::JointGroupEffortController,controller_interface::ControllerBase) diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h index 7bafb15d3..c9061e606 100644 --- a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h +++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h @@ -121,7 +121,11 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle ros::Timer goal_duration_timer_; ros::Duration action_monitor_period_; + // Callback to call setGoal. Override with specific command void goalCB(GoalHandle gh); + void setGoal(GoalHandle gh, std::vector command); + + // General callbacks void cancelCB(GoalHandle gh); void timeoutCB(const ros::TimerEvent& event); void preemptActiveGoal(); diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h index e781c6363..afd2b644b 100644 --- a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h +++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h @@ -174,7 +174,8 @@ void ForwardJointGroupCommandController::commandCB(const std_ } template -void ForwardJointGroupCommandController::goalCB(GoalHandle gh) +void ForwardJointGroupCommandController::setGoal(GoalHandle gh, + std::vector command) { ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal"); control_msgs::JointGroupCommandResult result; @@ -189,7 +190,8 @@ void ForwardJointGroupCommandController::goalCB(GoalHandle gh return; } - if (gh.getGoal()->joint_names.size() != gh.getGoal()->command.size()) { + + if (gh.getGoal()->joint_names.size() != command.size()) { result.error_string = "Size of command must match size of joint_names."; ROS_ERROR_STREAM_NAMED(name_, result.error_string); result.error_code = control_msgs::JointGroupCommandResult::INVALID_GOAL; @@ -214,7 +216,7 @@ void ForwardJointGroupCommandController::goalCB(GoalHandle gh RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh)); std::vector< double > new_commands = default_commands_; for(int i = 0; i < mapping_vector.size(); i++) { - new_commands[mapping_vector[i]] = gh.getGoal()->command[i]; + new_commands[mapping_vector[i]] = command[i]; } rt_goal->preallocated_feedback_->joint_names = joint_names_; commands_buffer_.writeFromNonRT(new_commands); @@ -231,8 +233,8 @@ void ForwardJointGroupCommandController::goalCB(GoalHandle gh goal_handle_timer_.start(); // Setup goal timeout - if (gh.getGoal()->timeout > ros::Duration()) { - goal_duration_timer_ = controller_nh_.createTimer(gh.getGoal()->timeout, + if (gh.getGoal()->command.time_from_start > ros::Duration()) { + goal_duration_timer_ = controller_nh_.createTimer(gh.getGoal()->command.time_from_start, &ForwardJointGroupCommandController::timeoutCB, this, true); @@ -287,7 +289,7 @@ void ForwardJointGroupCommandController::setActionFeedback(co } current_active_goal->preallocated_feedback_->header.stamp = time; - current_active_goal->preallocated_feedback_->desired = *commands_buffer_.readFromRT(); + current_active_goal->preallocated_feedback_->desired = current_active_goal->gh_.getGoal()->command; current_active_goal->preallocated_feedback_->actual.positions.clear(); current_active_goal->preallocated_feedback_->actual.velocities.clear(); current_active_goal->preallocated_feedback_->actual.effort.clear(); diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index 382ac1c42..f70a34a7c 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -60,5 +60,11 @@ void forward_command_controller::ForwardJointGroupCommandController::updateDe } } +template +void forward_command_controller::ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + // Set as position command + setGoal(gh, gh.getGoal()->command.positions); +} PLUGINLIB_EXPORT_CLASS(position_controllers::JointGroupPositionController,controller_interface::ControllerBase) diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index 8d0479269..8f7ebaaca 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -55,5 +55,11 @@ void forward_command_controller::ForwardJointGroupCommandController::updateDe } } +template +void forward_command_controller::ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + // Set as position command + setGoal(gh, gh.getGoal()->command.velocities); +} PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointGroupVelocityController,controller_interface::ControllerBase) From 45ad85a4159d7781ad35332906b96e44d1f9a29d Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 12 Aug 2021 16:30:53 -0700 Subject: [PATCH 5/5] Make internal function inline to avoid header defining function --- .../forward_joint_group_command_controller_impl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h index afd2b644b..f32446007 100644 --- a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h +++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h @@ -64,7 +64,7 @@ inline std::vector mapping(const T& t1, const T& t2) return mapping_vector; } -std::string getLeafNamespace(const ros::NodeHandle& nh) +inline std::string getLeafNamespace(const ros::NodeHandle& nh) { const std::string complete_ns = nh.getNamespace(); std::size_t id = complete_ns.find_last_of("/");