hardware_interface_adapter.h
Go to the documentation of this file.
1 // Copyright (C) 2014, SRI International
3 // Copyright (C) 2013, PAL Robotics S.L.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright notice,
8 // this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of SRI International nor the names of its
13 // contributors may be used to endorse or promote products derived from
14 // this software without specific prior written permission.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 // POSSIBILITY OF SUCH DAMAGE.
28 
30 
31 #ifndef GRIPPER_ACTION_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
32 #define GRIPPER_ACTION_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
33 
34 #include <cassert>
35 #include <string>
36 #include <vector>
37 
38 #include <boost/shared_ptr.hpp>
39 
40 #include <ros/node_handle.h>
41 #include <ros/time.h>
42 
43 #include <control_toolbox/pid.h>
45 
53 template <class HardwareInterface>
55 {
56 public:
57  bool init(hardware_interface::JointHandle& joint_handle, ros::NodeHandle& controller_nh)
58  {
59  return false;
60  }
61 
62  void starting(const ros::Time& time) {}
63  void stopping(const ros::Time& time) {}
64 
65  double updateCommand(const ros::Time& time,
66  const ros::Duration& period,
67  double desired_position,
68  double desired_velocity,
69  double error_position,
70  double error_velocity,
71  double max_allowed_effort) { return 0.0;}
72 
73 };
74 
78 template<>
79 class HardwareInterfaceAdapter<hardware_interface::PositionJointInterface>
80 {
81 public:
82  HardwareInterfaceAdapter() : joint_handle_ptr_(0) {}
83 
84  bool init(hardware_interface::JointHandle& joint_handle, ros::NodeHandle& controller_nh)
85  {
86  // Store pointer to joint handles
87  joint_handle_ptr_ = &joint_handle;
88 
89  return true;
90  }
91 
92  void starting(const ros::Time& time) {}
93  void stopping(const ros::Time& time) {}
94 
95  double updateCommand(const ros::Time& /*time*/,
96  const ros::Duration& period,
97  double desired_position,
98  double desired_velocity,
99  double error_position,
100  double error_velocity,
101  double max_allowed_effort)
102  {
103  // Forward desired position to command
104  (*joint_handle_ptr_).setCommand(desired_position);
105  return max_allowed_effort;
106  }
107 
108 private:
110 };
111 
128 template<>
129 class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface>
130 {
131 public:
132  HardwareInterfaceAdapter() : joint_handle_ptr_(0) {}
133 
134  bool init(hardware_interface::JointHandle& joint_handle, ros::NodeHandle& controller_nh)
135  {
136  // Store pointer to joint handles
137  joint_handle_ptr_ = &joint_handle;
138 
139  // Initialize PIDs
140  ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handle.getName());
141 
142  // Init PID gains from ROS parameter server
143  pid_.reset(new control_toolbox::Pid());
144  if (!pid_->init(joint_nh))
145  {
146  ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
147  return false;
148  }
149 
150  return true;
151  }
152 
153  void starting(const ros::Time& time)
154  {
155  if (!joint_handle_ptr_)
156  {
157  return;
158  }
159  // Reset PIDs, zero effort commands
160  pid_->reset();
161  (*joint_handle_ptr_).setCommand(0.0);
162  }
163 
164  void stopping(const ros::Time& time) {}
165 
166  double updateCommand(const ros::Time& /*time*/,
167  const ros::Duration& period,
168  double desired_position,
169  double desired_velocity,
170  double error_position,
171  double error_velocity,
172  double max_allowed_effort)
173  {
174  // Preconditions
175  if (!joint_handle_ptr_) {return 0.0;}
176 
177  // Update PIDs
178  double command = pid_->computeCommand(error_position, error_velocity, period);
179  command = std::min<double>(fabs(max_allowed_effort), std::max<double>(-fabs(max_allowed_effort), command));
180  (*joint_handle_ptr_).setCommand(command);
181  return command;
182  }
183 
184 private:
186  PidPtr pid_;
188 };
189 
190 #endif // header guard
bool init(hardware_interface::JointHandle &joint_handle, ros::NodeHandle &controller_nh)
bool init(hardware_interface::JointHandle &joint_handle, ros::NodeHandle &controller_nh)
double updateCommand(const ros::Time &, const ros::Duration &period, double desired_position, double desired_velocity, double error_position, double error_velocity, double max_allowed_effort)
void stopping(const ros::Time &time)
void starting(const ros::Time &time)
double updateCommand(const ros::Time &time, const ros::Duration &period, double desired_position, double desired_velocity, double error_position, double error_velocity, double max_allowed_effort)
double updateCommand(const ros::Time &, const ros::Duration &period, double desired_position, double desired_velocity, double error_position, double error_velocity, double max_allowed_effort)
ROSLIB_DECL std::string command(const std::string &cmd)
#define ROS_WARN_STREAM(args)
Helper class to simplify integrating the GripperActionController with different hardware interfaces...
bool init(hardware_interface::JointHandle &joint_handle, ros::NodeHandle &controller_nh)


gripper_action_controller
Author(s): Sachin Chitta
autogenerated on Sat Apr 18 2020 03:58:15