Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00027
00029
00030 #ifndef JOINT_TRAJECTORY_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
00031 #define JOINT_TRAJECTORY_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
00032
00033 #include <cassert>
00034 #include <string>
00035 #include <vector>
00036
00037 #include <boost/shared_ptr.hpp>
00038
00039 #include <ros/node_handle.h>
00040 #include <ros/time.h>
00041
00042 #include <control_toolbox/pid.h>
00043 #include <hardware_interface/joint_command_interface.h>
00044
00054 template <class HardwareInterface, class State>
00055 class HardwareInterfaceAdapter
00056 {
00057 public:
00058 bool init(std::vector<typename HardwareInterface::ResourceHandleType>& joint_handles, ros::NodeHandle& controller_nh)
00059 {
00060 return false;
00061 }
00062
00063 void starting(const ros::Time& time) {}
00064 void stopping(const ros::Time& time) {}
00065
00066 void updateCommand(const ros::Time& time,
00067 const ros::Duration& period,
00068 const State& desired_state,
00069 const State& state_error) {}
00070 };
00071
00075 template <class State>
00076 class HardwareInterfaceAdapter<hardware_interface::PositionJointInterface, State>
00077 {
00078 public:
00079 HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
00080
00081 bool init(std::vector<hardware_interface::JointHandle>& joint_handles, ros::NodeHandle& controller_nh)
00082 {
00083
00084 joint_handles_ptr_ = &joint_handles;
00085
00086 return true;
00087 }
00088
00089 void starting(const ros::Time& time) {}
00090 void stopping(const ros::Time& time) {}
00091
00092 void updateCommand(const ros::Time& ,
00093 const ros::Duration& ,
00094 const State& desired_state,
00095 const State& )
00096 {
00097
00098 const unsigned int n_joints = joint_handles_ptr_->size();
00099 for (unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);}
00100 }
00101
00102 private:
00103 std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
00104 };
00105
00132 template <class State>
00133 class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, State>
00134 {
00135 public:
00136 HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
00137
00138 bool init(std::vector<hardware_interface::JointHandle>& joint_handles, ros::NodeHandle& controller_nh)
00139 {
00140
00141 joint_handles_ptr_ = &joint_handles;
00142
00143
00144 pids_.resize(joint_handles.size());
00145 for (unsigned int i = 0; i < pids_.size(); ++i)
00146 {
00147
00148 ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handles[i].getName());
00149
00150
00151 pids_[i].reset(new control_toolbox::Pid());
00152 if (!pids_[i]->init(joint_nh))
00153 {
00154 ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
00155 return false;
00156 }
00157 }
00158
00159 return true;
00160 }
00161
00162 void starting(const ros::Time& time)
00163 {
00164 if (!joint_handles_ptr_) {return;}
00165
00166
00167 for (unsigned int i = 0; i < pids_.size(); ++i)
00168 {
00169 pids_[i]->reset();
00170 (*joint_handles_ptr_)[i].setCommand(0.0);
00171 }
00172 }
00173
00174 void stopping(const ros::Time& time) {}
00175
00176 void updateCommand(const ros::Time& ,
00177 const ros::Duration& period,
00178 const State& ,
00179 const State& state_error)
00180 {
00181 const unsigned int n_joints = joint_handles_ptr_->size();
00182
00183
00184 if (!joint_handles_ptr_) {return;}
00185 assert(n_joints == state_error.position.size());
00186 assert(n_joints == state_error.velocity.size());
00187
00188
00189 for (unsigned int i = 0; i < n_joints; ++i)
00190 {
00191 const double command = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
00192 (*joint_handles_ptr_)[i].setCommand(command);
00193 }
00194 }
00195
00196 private:
00197 typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
00198 std::vector<PidPtr> pids_;
00199
00200 std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
00201 };
00202
00203 #endif // header guard