diff --git a/pr_ros_controllers/CMakeLists.txt b/pr_ros_controllers/CMakeLists.txt index a600f06..3e3bd4e 100644 --- a/pr_ros_controllers/CMakeLists.txt +++ b/pr_ros_controllers/CMakeLists.txt @@ -6,6 +6,9 @@ find_package(catkin REQUIRED COMPONENTS controller_interface rosconsole roscpp + pr_control_msgs + actionlib + actionlib_msgs ) find_package(Boost REQUIRED COMPONENTS system) @@ -16,14 +19,16 @@ link_directories(${Boost_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS}) catkin_package( CATKIN_DEPENDS controller_interface + actionlib + actionlib_msgs INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} ) add_library(${PROJECT_NAME} - src/pr_joint_position_controller.cpp - src/pr_joint_velocity_controller.cpp - src/joint_mode_controller.cpp + src/joint_group_position_controller.cpp + src/joint_group_velocity_controller.cpp + src/joint_group_effort_controller.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} diff --git a/pr_ros_controllers/include/pr_ros_controllers/detail/forward_joint_group_command_controller_impl.h b/pr_ros_controllers/include/pr_ros_controllers/detail/forward_joint_group_command_controller_impl.h new file mode 100644 index 0000000..90d46ca --- /dev/null +++ b/pr_ros_controllers/include/pr_ros_controllers/detail/forward_joint_group_command_controller_impl.h @@ -0,0 +1,320 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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; +} + +inline 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(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& n) +{ + // Get base hardware interface + HardwareInterface* hw = robot_hw->get(); + + // 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; + } + + // Clear joints_ first in case this is called twice + joints_.clear(); + 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(); + + // Add Joint Mode switching if handle provided + hardware_interface::JointModeInterface* jmt = robot_hw->get(); + std::string handle_name; + if(jmt && n.getParam("mode_handle", handle_name)) { + ROS_DEBUG_STREAM_NAMED(name_, "Enabling joint mode handling."); + mode_handle_.reset(new hardware_interface::JointModeHandle(jmt->getHandle(handle_name))); + } + 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::setGoal(GoalHandle gh, + std::vector command) +{ + ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal"); + pr_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 = pr_control_msgs::JointGroupCommandResult::INVALID_GOAL; + gh.setRejected(result); + return; + } + + + 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 = pr_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 = pr_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]] = 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()->command.time_from_start > ros::Duration()) { + goal_duration_timer_ = controller_nh_.createTimer(gh.getGoal()->command.time_from_start, + &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 = 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(); + 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 diff --git a/pr_ros_controllers/include/pr_ros_controllers/forward_joint_group_command_controller.h b/pr_ros_controllers/include/pr_ros_controllers/forward_joint_group_command_controller.h new file mode 100644 index 0000000..feb02e6 --- /dev/null +++ b/pr_ros_controllers/include/pr_ros_controllers/forward_joint_group_command_controller.h @@ -0,0 +1,154 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#pragma once + + +#include +#include + +#include +#include + +// actionlib +#include + +// ROS messages +#include +#include +#include + +// ros_controls +#include +#include +#include +#include + + +namespace forward_command_controller +{ + +/** + * \brief Forward command controller for a set of joints. + * + * This class forwards the command signal down to a set of joints. + * Command signal and joint hardware interface are of the same type, e.g. effort commands for an effort-controlled + * joint. + * + * \tparam T Type implementing the JointCommandInterface. + * + * \section ROS interface + * + * \param type hardware interface type. + * \param joints Names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint commands to apply. + */ +template +class ForwardJointGroupCommandController: public controller_interface::MultiInterfaceController +{ +public: + ForwardJointGroupCommandController() : controller_interface::MultiInterfaceController (true) {} + ~ForwardJointGroupCommandController() {sub_command_.shutdown();} + + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle &n); + + 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 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_; + + std::shared_ptr mode_handle_; + + // 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(); + void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg); + + // Defaults to no change in default command + void updateDefaultCommand(); + +private: + /** + * @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/pr_ros_controllers/include/pr_ros_controllers/pr_joint_position_controller.h b/pr_ros_controllers/include/pr_ros_controllers/joint_group_effort_controller.h similarity index 55% rename from pr_ros_controllers/include/pr_ros_controllers/pr_joint_position_controller.h rename to pr_ros_controllers/include/pr_ros_controllers/joint_group_effort_controller.h index c1434f3..456578f 100644 --- a/pr_ros_controllers/include/pr_ros_controllers/pr_joint_position_controller.h +++ b/pr_ros_controllers/include/pr_ros_controllers/joint_group_effort_controller.h @@ -4,7 +4,7 @@ * Copyright (c) 2008, Willow Garage, Inc. * Copyright (c) 2012, hiDOF, Inc. * Copyright (c) 2013, PAL Robotics, S.L. - * Copyright (c) 2014, Carnegie Mellon University + * Copyright (c) 2014, Fraunhofer IPA * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -35,59 +35,27 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* requires: -#include -#include -#include -#include -*/ +#pragma once + + +#include namespace pr_ros_controllers { - + /** - * \brief PR Joint Position Controller (linear or angular) + * \brief Forward command controller for a set of effort controlled joints (torque or force). * - * This class passes the commanded position down to the joint + * This class forwards the commanded efforts down to a set of joints. * * \section ROS interface * - * \param type Must be "PrJointPositionController". - * \param joint Name of the joint to control. + * \param type Must be "JointGroupEffortController". + * \param joints List of names of the joints to control. * * Subscribes to: - * - \b command (std_msgs::Float64) : The joint position to apply + * - \b command (std_msgs::Float64MultiArray) : The joint efforts to apply */ - -class PrJointPositionController: public controller_interface::Controller -{ -public: - PrJointPositionController() : command_(0) {} - ~PrJointPositionController() {sub_command_.shutdown();} - - bool init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle &n) - { - std::string joint_name; - if (!n.getParam("joint", joint_name)) - { - ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str()); - return false; - } - - joint_ = hw->getHandle(joint_name); - sub_command_ = n.subscribe("command", 1, &PrJointPositionController::commandCB, this); - return true; - } - - void starting(const ros::Time& time) {command_ = joint_.getPosition();} - void update(const ros::Time& time, const ros::Duration& period) {joint_.setCommand(command_);} - - hardware_interface::JointHandle joint_; - double command_; - -private: - ros::Subscriber sub_command_; - void commandCB(const std_msgs::Float64ConstPtr& msg) {command_ = msg->data;} -}; - -}; /* namespace pr_ros_controllers */ +typedef forward_command_controller::ForwardJointGroupCommandController + JointGroupEffortController; +} diff --git a/pr_ros_controllers/src/pr_joint_position_controller.cpp b/pr_ros_controllers/include/pr_ros_controllers/joint_group_position_controller.h similarity index 72% rename from pr_ros_controllers/src/pr_joint_position_controller.cpp rename to pr_ros_controllers/include/pr_ros_controllers/joint_group_position_controller.h index 1e67af8..fad7ff4 100644 --- a/pr_ros_controllers/src/pr_joint_position_controller.cpp +++ b/pr_ros_controllers/include/pr_ros_controllers/joint_group_position_controller.h @@ -4,6 +4,7 @@ * Copyright (c) 2008, Willow Garage, Inc. * Copyright (c) 2012, hiDOF, Inc. * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -34,13 +35,28 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include -#include -#include +#pragma once + -#include +#include +#include -#include +namespace pr_ros_controllers +{ -PLUGINLIB_EXPORT_CLASS(pr_ros_controllers::PrJointPositionController,controller_interface::ControllerBase) +/** + * \brief Forward command controller for a set of position controlled joints (linear or angular). + * + * This class forwards the commanded positions down to a set of joints. + * + * \section ROS interface + * + * \param type Must be "JointGroupPositionController". + * \param joints List of names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint positions to apply + */ +typedef forward_command_controller::ForwardJointGroupCommandController + JointGroupPositionController; +} diff --git a/pr_ros_controllers/src/pr_joint_velocity_controller.cpp b/pr_ros_controllers/include/pr_ros_controllers/joint_group_velocity_controller.h similarity index 72% rename from pr_ros_controllers/src/pr_joint_velocity_controller.cpp rename to pr_ros_controllers/include/pr_ros_controllers/joint_group_velocity_controller.h index 143f352..a4976a7 100644 --- a/pr_ros_controllers/src/pr_joint_velocity_controller.cpp +++ b/pr_ros_controllers/include/pr_ros_controllers/joint_group_velocity_controller.h @@ -4,6 +4,7 @@ * Copyright (c) 2008, Willow Garage, Inc. * Copyright (c) 2012, hiDOF, Inc. * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -34,13 +35,29 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#pragma once + + +#include #include -#include -#include -#include +namespace pr_ros_controllers +{ -#include +/** + * \brief Forward command controller for a set of velocity controlled joints (linear or angular). + * + * This class forwards the commanded velocities down to a set of joints. + * + * \section ROS interface + * + * \param type Must be "JointGroupVelocityController". + * \param joints List of names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint velocities to apply + */ +typedef forward_command_controller::ForwardJointGroupCommandController + JointGroupVelocityController; -PLUGINLIB_EXPORT_CLASS(pr_ros_controllers::PrJointVelocityController,controller_interface::ControllerBase) +} diff --git a/pr_ros_controllers/include/pr_ros_controllers/joint_mode_interface.h b/pr_ros_controllers/include/pr_ros_controllers/joint_mode_interface.h deleted file mode 100644 index 07f54bb..0000000 --- a/pr_ros_controllers/include/pr_ros_controllers/joint_mode_interface.h +++ /dev/null @@ -1,108 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, University of Colorado, Boulder -* All rights reserved. -* -* 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 the Univ of CO, Boulder 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: Dave Coleman -Desc: This interface is for switching a hardware interface between different controller modes -i.e. position, velocity, force -*/ - -#ifndef HARDWARE_INTERFACE_JOINT_MODE_INTERFACE_H -#define HARDWARE_INTERFACE_JOINT_MODE_INTERFACE_H - -#include -#include - -namespace hardware_interface -{ - -enum JointCommandModes { - MODE_POSITION = 1, - MODE_VELOCITY = 2, - MODE_EFFORT = 3, - MODE_OTHER = 4 -}; - -/** \brief A handle used to read and mode a single joint. */ -class JointModeHandle -{ -public: - - /** -* \param mode Which mode to start in -*/ - JointModeHandle(std::string name, int* mode) - : name_(name), - mode_(mode) - { - if (!mode_) - { - throw HardwareInterfaceException("Cannot create mode interface. Mode data pointer is null."); - } - } - - std::string getName() const {return name_;} - - void setMode(int mode) {assert(mode_); *mode_ = mode;} - int getMode() const {assert(mode_); return *mode_;} - - // Helper function for console messages - std::string getModeName(int mode) - { - switch(mode) - { - case MODE_POSITION: - return "position"; - case MODE_VELOCITY: - return "velocity"; - case MODE_EFFORT: - return "effort"; - case MODE_OTHER: - return "other"; - } - return "unknown"; - } - -private: - int* mode_; - std::string name_; -}; - -/** \brief Hardware interface to support changing between control modes -* -*/ -class JointModeInterface : public HardwareResourceManager {}; - -} // namespace - -#endif diff --git a/pr_ros_controllers/include/pr_ros_controllers/pr_joint_velocity_controller.h b/pr_ros_controllers/include/pr_ros_controllers/pr_joint_velocity_controller.h deleted file mode 100644 index a3f5030..0000000 --- a/pr_ros_controllers/include/pr_ros_controllers/pr_joint_velocity_controller.h +++ /dev/null @@ -1,125 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, Willow Garage, Inc. - * Copyright (c) 2012, hiDOF, Inc. - * Copyright (c) 2013, PAL Robotics, S.L. - * Copyright (c) 2014, Carnegie Mellon University - * All rights reserved. - * - * 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 the Willow Garage 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. - *********************************************************************/ - -/* requires: -#include -#include -#include -#include -*/ - -namespace pr_ros_controllers -{ - -/** - * \brief PR Joint Velocity Controller (linear or angular) - * - * This class passes the commanded velocity down to the joint and does PID - * - * \section ROS interface - * - * \param type Must be "PrJointVelocityController". - * \param joint Name of the joint to control. - * - * Subscribes to: - * - \b command (std_msgs::Float64) : The joint velocity to apply - */ - -class PrJointVelocityController : public controller_interface::Controller -{ -public: - PrJointVelocityController() : desired_(0), last_e_(0) {} - ~PrJointVelocityController() {sub_desired_.shutdown();} - - bool init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle &n) - { - std::string joint_name; - if (!n.getParam("joint", joint_name)) - { - ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str()); - return false; - } - - joint_ = hw->getHandle(joint_name); - sub_desired_ = n.subscribe("command", 1, &PrJointVelocityController::commandCB, this); - - if (!n.getParam("p", p_)) - { - ROS_ERROR("No P gain given (namespace: %s)", n.getNamespace().c_str()); - return false; - } - - if (!n.getParam("i", i_)) - { - ROS_WARN("No I gain given (namespace: %s); defaulting to 0.", n.getNamespace().c_str()); - i_ = 0; - } - - if (!n.getParam("d", d_)) - { - ROS_WARN("No D gain given (namespace: %s); defaulting to 0.", n.getNamespace().c_str()); - d_ = 0; - } - return true; - } - - void starting(const ros::Time& time) {desired_ = 0;} - void update(const ros::Time& time, const ros::Duration& period) - { - // TODO: Currently just does P and D, not I. - double current_vel = joint_.getVelocity(); - double e = desired_ - current_vel; - joint_.setCommand(current_vel + p_*e + d_*(e - last_e_)); - last_e_ = e; - } - - hardware_interface::JointHandle joint_; - double desired_; - -private: - double last_e_; - - // PID gains - double p_; - double i_; - double d_; - - ros::Subscriber sub_desired_; - void commandCB(const std_msgs::Float64ConstPtr& msg) {desired_ = msg->data;} -}; - -}; /* namespace pr_ros_controllers */ diff --git a/pr_ros_controllers/package.xml b/pr_ros_controllers/package.xml index e36dcea..4841519 100644 --- a/pr_ros_controllers/package.xml +++ b/pr_ros_controllers/package.xml @@ -1,21 +1,21 @@ - + + pr_ros_controllers 0.0.0 pr_ros_controllers - Chris Dellin + Ethan K. Gordon BSD catkin - controller_interface - rosconsole - roscpp - std_msgs - - controller_interface - rosconsole - roscpp - std_msgs + actionlib + actionlib_msgs + controller_interface + hardware_interface + rosconsole + roscpp + std_msgs + pr_control_msgs diff --git a/pr_ros_controllers/plugins.xml b/pr_ros_controllers/plugins.xml index 158fae9..7acdc50 100644 --- a/pr_ros_controllers/plugins.xml +++ b/pr_ros_controllers/plugins.xml @@ -1,34 +1,26 @@ - - The PrJointPositionController tracks position commands. It expects a PositionJointInterface type of hardware interface. + The JointGroupPositionController tracks position commands. It expects a PositionJointInterface and is compatible with JointModeInterface. - - The PrJointVelocityController tracks Velocity commands. It expects a VelocityJointInterface type of hardware interface. + The JointGroupVelocityController tracks Velocity commands. It expects a VelocityJointInterface and is compatible with JointModeInterface. - - Controller to allow joint controllers to easily switch modes between position, velocity, and effort-based control - - - - - - Controller to send tare commands to a force torque sensor. + The JointGroupEffortController tracks Effort commands. It expects a EffortJointInterface and is compatible with JointModeInterface. diff --git a/pr_ros_controllers/src/joint_group_effort_controller.cpp b/pr_ros_controllers/src/joint_group_effort_controller.cpp new file mode 100644 index 0000000..dad9f27 --- /dev/null +++ b/pr_ros_controllers/src/joint_group_effort_controller.cpp @@ -0,0 +1,72 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#include +#include + +template +void forward_command_controller::ForwardJointGroupCommandController::starting(const ros::Time& time) +{ + // Switch joint mode if mode handle provided + if(mode_handle_) { + mode_handle_->setMode(hardware_interface::JointCommandModes::MODE_EFFORT); + } + + ROS_INFO_STREAM_NAMED(name_, "Starting Effort Controller"); + + // Start controller with 0.0 efforts + 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 +void forward_command_controller::ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + // Set as position command + setGoal(gh, gh.getGoal()->command.effort); +} + +PLUGINLIB_EXPORT_CLASS(pr_ros_controllers::JointGroupEffortController,controller_interface::ControllerBase) diff --git a/pr_ros_controllers/src/joint_group_position_controller.cpp b/pr_ros_controllers/src/joint_group_position_controller.cpp new file mode 100644 index 0000000..1de19f6 --- /dev/null +++ b/pr_ros_controllers/src/joint_group_position_controller.cpp @@ -0,0 +1,79 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#include +#include + +template +void forward_command_controller::ForwardJointGroupCommandController::starting(const ros::Time& time) +{ + // Switch joint mode if mode handle provided + if(mode_handle_) { + mode_handle_->setMode(hardware_interface::JointCommandModes::MODE_POSITION); + } + + ROS_INFO_STREAM_NAMED(name_, "Start Position Controller"); + + // Start controller with current joint positions + std::vector & commands = *commands_buffer_.readFromRT(); + for(unsigned int i=0; i +void forward_command_controller::ForwardJointGroupCommandController::updateDefaultCommand() +{ + // Set default to current position + for(unsigned int i=0; i +void forward_command_controller::ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + // Set as position command + setGoal(gh, gh.getGoal()->command.positions); +} + +PLUGINLIB_EXPORT_CLASS(pr_ros_controllers::JointGroupPositionController,controller_interface::ControllerBase) diff --git a/pr_ros_controllers/src/joint_group_velocity_controller.cpp b/pr_ros_controllers/src/joint_group_velocity_controller.cpp new file mode 100644 index 0000000..2bcc58e --- /dev/null +++ b/pr_ros_controllers/src/joint_group_velocity_controller.cpp @@ -0,0 +1,72 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2012, hiDOF, Inc. + * Copyright (c) 2013, PAL Robotics, S.L. + * Copyright (c) 2014, Fraunhofer IPA + * All rights reserved. + * + * 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 the Willow Garage 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. + *********************************************************************/ + +#include +#include + +template +void forward_command_controller::ForwardJointGroupCommandController::starting(const ros::Time& time) +{ + // Switch joint mode if mode handle provided + if(mode_handle_) { + mode_handle_->setMode(hardware_interface::JointCommandModes::MODE_VELOCITY); + } + + ROS_INFO_STREAM_NAMED(name_, "Starting Velocity Controller"); + + // Start controller with 0.0 velocities + 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 +void forward_command_controller::ForwardJointGroupCommandController::goalCB(GoalHandle gh) +{ + // Set as position command + setGoal(gh, gh.getGoal()->command.velocities); +} + +PLUGINLIB_EXPORT_CLASS(pr_ros_controllers::JointGroupVelocityController,controller_interface::ControllerBase) diff --git a/pr_ros_controllers/src/joint_mode_controller.cpp b/pr_ros_controllers/src/joint_mode_controller.cpp deleted file mode 100644 index cf8e07a..0000000 --- a/pr_ros_controllers/src/joint_mode_controller.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, University of Colorado, Boulder - * All rights reserved. - * - * 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 the Univ of CO, Boulder 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: Dave Coleman - Desc: Controller to allow joint controllers to easily switch modes between position, velocity, and effort-based control -*/ - -#include -#include -#include - -namespace pr_ros_controllers { - -class JointModeController: public controller_interface::Controller -{ - -private: - boost::shared_ptr mode_handle_; - - int joint_mode_; - -public: - JointModeController() - {} - - ~JointModeController() - {} - - bool init( - hardware_interface::JointModeInterface *hw, ros::NodeHandle &nh) - { - - // Get handle name for this mode mechanism from the parameter server - std::string handle_name; - if (!nh.getParam("mode_handle", handle_name)) - { - ROS_DEBUG_STREAM_NAMED("init","No mode_handle specified in namespace '" << nh.getNamespace() - << "', defaulting to 'joint_mode'"); - handle_name = "joint_mode"; // default name - } - - // Get the joint mode, which we represent as an int for speed. User chooses what that mode actually represents - if (!nh.getParam("joint_mode", joint_mode_)) - { - ROS_DEBUG_STREAM_NAMED("init","No joint_mode specified in namespace '" << nh.getNamespace() - << "', defaulting to position"); - joint_mode_ = hardware_interface::MODE_POSITION; // default joint mode - } - - // Save the mode interface handle for usage in the starting() function - mode_handle_.reset(new hardware_interface::JointModeHandle(hw->getHandle(handle_name))); - - return true; - } - - void update(const ros::Time& time, const ros::Duration& period) - {} - - void starting(const ros::Time& time) - { - // The controller actually changes the mode when the controller is started, not when it is loaded - mode_handle_->setMode(joint_mode_); - }; - -}; - -} // namespace - -PLUGINLIB_EXPORT_CLASS( pr_ros_controllers::JointModeController, controller_interface::ControllerBase)