hardware_interface_adapter.h
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2014, SRI International
00003 // Copyright (C) 2013, PAL Robotics S.L.
00004 //
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //   * Redistributions of source code must retain the above copyright notice,
00008 //     this list of conditions and the following disclaimer.
00009 //   * Redistributions in binary form must reproduce the above copyright
00010 //     notice, this list of conditions and the following disclaimer in the
00011 //     documentation and/or other materials provided with the distribution.
00012 //   * Neither the name of SRI International nor the names of its
00013 //     contributors may be used to endorse or promote products derived from
00014 //     this software without specific prior written permission.
00015 //
00016 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026 // POSSIBILITY OF SUCH DAMAGE.
00028 
00030 
00031 #ifndef GRIPPER_ACTION_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
00032 #define GRIPPER_ACTION_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
00033 
00034 #include <cassert>
00035 #include <string>
00036 #include <vector>
00037 
00038 #include <boost/shared_ptr.hpp>
00039 
00040 #include <ros/node_handle.h>
00041 #include <ros/time.h>
00042 
00043 #include <control_toolbox/pid.h>
00044 #include <hardware_interface/joint_command_interface.h>
00045 
00053 template <class HardwareInterface>
00054 class HardwareInterfaceAdapter
00055 {
00056 public:
00057   bool init(hardware_interface::JointHandle& joint_handle, ros::NodeHandle& controller_nh)
00058   {
00059     return false;
00060   }
00061 
00062   void starting(const ros::Time& time) {}
00063   void stopping(const ros::Time& time) {}
00064 
00065   double updateCommand(const ros::Time&     time,
00066                        const ros::Duration& period,
00067                        double desired_position,
00068                        double desired_velocity,
00069                        double error_position,
00070                        double error_velocity,
00071                        double max_allowed_effort) { return 0.0;}
00072 
00073 };
00074 
00078 template<>
00079 class HardwareInterfaceAdapter<hardware_interface::PositionJointInterface>
00080 {
00081 public:
00082   HardwareInterfaceAdapter() : joint_handle_ptr_(0) {}
00083 
00084   bool init(hardware_interface::JointHandle& joint_handle, ros::NodeHandle& controller_nh)
00085   {
00086     // Store pointer to joint handles
00087     joint_handle_ptr_ = &joint_handle;
00088 
00089     return true;
00090   }
00091 
00092   void starting(const ros::Time& time) {}
00093   void stopping(const ros::Time& time) {}
00094 
00095   double updateCommand(const ros::Time&     /*time*/,
00096                        const ros::Duration& period,
00097                        double desired_position,
00098                        double desired_velocity,
00099                        double error_position,
00100                        double error_velocity,
00101                        double max_allowed_effort)
00102   {
00103     // Forward desired position to command
00104     (*joint_handle_ptr_).setCommand(desired_position);
00105     return max_allowed_effort;
00106   }
00107 
00108 private:
00109   hardware_interface::JointHandle* joint_handle_ptr_;
00110 };
00111 
00128 template<>
00129 class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface>
00130 {
00131 public:
00132   HardwareInterfaceAdapter() : joint_handle_ptr_(0) {}
00133 
00134   bool init(hardware_interface::JointHandle& joint_handle, ros::NodeHandle& controller_nh)
00135   {
00136     // Store pointer to joint handles
00137     joint_handle_ptr_ = &joint_handle;
00138 
00139     // Initialize PIDs
00140     ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handle.getName());
00141 
00142     // Init PID gains from ROS parameter server
00143     pid_.reset(new control_toolbox::Pid());
00144     if (!pid_->init(joint_nh))
00145     {
00146       ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
00147       return false;
00148     }
00149   
00150     return true;
00151   }
00152 
00153   void starting(const ros::Time& time)
00154   {
00155     if (!joint_handle_ptr_) 
00156     {
00157       return;
00158     }
00159     // Reset PIDs, zero effort commands
00160     pid_->reset();
00161     (*joint_handle_ptr_).setCommand(0.0);
00162   }
00163 
00164   void stopping(const ros::Time& time) {}
00165 
00166   double updateCommand(const ros::Time&     /*time*/,
00167                        const ros::Duration& period,
00168                        double desired_position,
00169                        double desired_velocity,
00170                        double error_position,
00171                        double error_velocity,
00172                        double max_allowed_effort)
00173   {
00174     // Preconditions
00175     if (!joint_handle_ptr_) {return 0.0;}
00176 
00177     // Update PIDs
00178       double command = pid_->computeCommand(error_position, error_velocity, period);
00179       command = std::min<double>(fabs(max_allowed_effort), std::max<double>(-fabs(max_allowed_effort), command));
00180       (*joint_handle_ptr_).setCommand(command);
00181       return command;
00182   }
00183 
00184 private:
00185   typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
00186   PidPtr pid_;
00187   hardware_interface::JointHandle* joint_handle_ptr_;
00188 };
00189 
00190 #endif // header guard


gripper_action_controller
Author(s): Sachin Chitta
autogenerated on Fri Aug 28 2015 12:37:03