gripper_action_controller_impl.h
Go to the documentation of this file.
1 // Copyright (C) 2014, SRI International
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of SRI International nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #ifndef GRIPPER_ACTION_CONTROLLER_GRIPPER_ACTION_CONTROLLER_IMPL_H
31 #define GRIPPER_ACTION_CONTROLLER_GRIPPER_ACTION_CONTROLLER_IMPL_H
32 
34 {
35 namespace internal
36 {
37 std::string getLeafNamespace(const ros::NodeHandle& nh)
38 {
39  const std::string complete_ns = nh.getNamespace();
40  std::size_t id = complete_ns.find_last_of("/");
41  return complete_ns.substr(id + 1);
42 }
43 
44 urdf::ModelSharedPtr getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
45 {
46  urdf::ModelSharedPtr urdf(new urdf::Model);
47 
48  std::string urdf_str;
49  // Check for robot_description in proper namespace
50  if (nh.getParam(param_name, urdf_str))
51  {
52  if (!urdf->initString(urdf_str))
53  {
54  ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
55  nh.getNamespace() << ").");
56  return urdf::ModelSharedPtr();
57  }
58  }
59  // Check for robot_description in root
60  else if (!urdf->initParam("robot_description"))
61  {
62  ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter");
63  return urdf::ModelSharedPtr();
64  }
65  return urdf;
66 }
67 
68 std::vector<urdf::JointConstSharedPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
69 {
70  std::vector<urdf::JointConstSharedPtr> out;
71  for (unsigned int i = 0; i < joint_names.size(); ++i)
72  {
73  urdf::JointConstSharedPtr urdf_joint = urdf.getJoint(joint_names[i]);
74  if (urdf_joint)
75  {
76  out.push_back(urdf_joint);
77  }
78  else
79  {
80  ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
81  return std::vector<urdf::JointConstSharedPtr>();
82  }
83  }
84  return out;
85 }
86 
87 } // namespace
88 
89 template <class HardwareInterface>
91 starting(const ros::Time& time)
92 {
93  command_struct_rt_.position_ = joint_.getPosition();
94  command_struct_rt_.max_effort_ = default_max_effort_;
95  command_.initRT(command_struct_rt_);
96 
97  // Hardware interface adapter
98  hw_iface_adapter_.starting(ros::Time(0.0));
99  last_movement_time_ = time;
100 }
101 
102 template <class HardwareInterface>
104 stopping(const ros::Time& time)
105 {
106  preemptActiveGoal();
107 }
108 
109 template <class HardwareInterface>
112 {
113  RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
114 
115  // Cancels the currently active goal
116  if (current_active_goal)
117  {
118  // Marks the current goal as canceled
119  rt_active_goal_.reset();
120  if(current_active_goal->gh_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
121  current_active_goal->gh_.setCanceled();
122  }
123 }
124 
125 template <class HardwareInterface>
128 : verbose_(false) // Set to true during debugging
129 {}
130 
131 template <class HardwareInterface>
133  ros::NodeHandle& root_nh,
134  ros::NodeHandle& controller_nh)
135 {
136  using namespace internal;
137 
138  // Cache controller node handle
139  controller_nh_ = controller_nh;
140 
141  // Controller name
143 
144  // Action status checking update rate
145  double action_monitor_rate = 20.0;
146  controller_nh_.getParam("action_monitor_rate", action_monitor_rate);
147  action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate);
148  ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz.");
149 
150  // Controlled joint
152  if (joint_name_.empty())
153  {
154  ROS_ERROR_STREAM_NAMED(name_, "Could not find joint name on param server");
155  return false;
156  }
157 
158  // URDF joints
159  urdf::ModelSharedPtr urdf = getUrdf(root_nh, "robot_description");
160  if (!urdf)
161  {
162  return false;
163  }
164 
165  std::vector<std::string> joint_names;
166  joint_names.push_back(joint_name_);
167  std::vector<urdf::JointConstSharedPtr> urdf_joints = getUrdfJoints(*urdf, joint_names);
168  if (urdf_joints.empty())
169  {
170  return false;
171  }
172 
173  // Initialize members
174  // Joint handle
175  try
176  {
177  joint_ = hw->getHandle(joint_name_);
178  }
179  catch (...)
180  {
181  ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << joint_name_ << "' in '" <<
182  this->getHardwareInterfaceType() << "'.");
183  return false;
184  }
185 
186  ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" <<
187  "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'" <<
188  "\n");
189 
190  // Default tolerances
191  controller_nh_.param<double>("goal_tolerance", goal_tolerance_, 0.01);
192  goal_tolerance_ = fabs(goal_tolerance_);
193  // Max allowable effort
194  controller_nh_.param<double>("max_effort", default_max_effort_, 0.0);
195  default_max_effort_ = fabs(default_max_effort_);
196  // Stall - stall velocity threshold, stall timeout
197  controller_nh_.param<double>("stall_velocity_threshold", stall_velocity_threshold_, 0.001);
198  controller_nh_.param<double>("stall_timeout", stall_timeout_, 1.0);
199 
200  // Hardware interface adapter
202 
203  // Command - non RT version
206 
207  // Result
208  pre_alloc_result_.reset(new control_msgs::GripperCommandResult());
210  pre_alloc_result_->reached_goal = false;
211  pre_alloc_result_->stalled = false;
212 
213  // ROS API: Action interface
214  action_server_.reset(new ActionServer(controller_nh_, "gripper_cmd",
215  boost::bind(&GripperActionController::goalCB, this, _1),
216  boost::bind(&GripperActionController::cancelCB, this, _1),
217  false));
218  action_server_->start();
219  return true;
220 }
221 
222 template <class HardwareInterface>
224 update(const ros::Time& time, const ros::Duration& period)
225 {
226  command_struct_rt_ = *(command_.readFromRT());
227 
228  double current_position = joint_.getPosition();
229  double current_velocity = joint_.getVelocity();
230 
231  double error_position = command_struct_rt_.position_ - current_position;
232  double error_velocity = - current_velocity;
233 
234  checkForSuccess(time, error_position, current_position, current_velocity);
235 
236  // Hardware interface adapter: Generate and send commands
239  error_position, error_velocity, command_struct_rt_.max_effort_);
240 }
241 
242 template <class HardwareInterface>
245 {
246  ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal");
247 
248  // Precondition: Running controller
249  if (!this->isRunning())
250  {
251  ROS_ERROR_NAMED(name_, "Can't accept new action goals. Controller is not running.");
252  control_msgs::GripperCommandResult result;
253  gh.setRejected(result);
254  return;
255  }
256 
257  // Try to update goal
258  RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
259 
260  // Accept new goal
262  gh.setAccepted();
263 
264  // This is the non-realtime command_struct
265  // We use command_ for sharing
266  command_struct_.position_ = gh.getGoal()->command.position;
267  command_struct_.max_effort_ = gh.getGoal()->command.max_effort;
268  command_.writeFromNonRT(command_struct_);
269 
270  pre_alloc_result_->reached_goal = false;
271  pre_alloc_result_->stalled = false;
272 
274 
275  // Setup goal status checking timer
278  rt_goal);
280  rt_active_goal_ = rt_goal;
281 }
282 
283 template <class HardwareInterface>
286 {
287  RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
288 
289  // Check that cancel request refers to currently active goal (if any)
290  if (current_active_goal && current_active_goal->gh_ == gh)
291  {
292  // Reset current goal
293  rt_active_goal_.reset();
294 
295  // Enter hold current position mode
297  ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib.");
298 
299  // Mark the current goal as canceled
300  current_active_goal->gh_.setCanceled();
301  }
302 }
303 
304 template <class HardwareInterface>
307 {
310  command_.writeFromNonRT(command_struct_);
311 }
312 
313 template <class HardwareInterface>
315 checkForSuccess(const ros::Time& time, double error_position, double current_position, double current_velocity)
316 {
317  RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
318 
319  if(!current_active_goal)
320  return;
321 
322  if(current_active_goal->gh_.getGoalStatus().status != actionlib_msgs::GoalStatus::ACTIVE)
323  return;
324 
325  if(fabs(error_position) < goal_tolerance_)
326  {
328  pre_alloc_result_->position = current_position;
329  pre_alloc_result_->reached_goal = true;
330  pre_alloc_result_->stalled = false;
331  current_active_goal->setSucceeded(pre_alloc_result_);
332  }
333  else
334  {
335  if(fabs(current_velocity) > stall_velocity_threshold_)
336  {
337  last_movement_time_ = time;
338  }
339  else if( (time - last_movement_time_).toSec() > stall_timeout_)
340  {
342  pre_alloc_result_->position = current_position;
343  pre_alloc_result_->reached_goal = false;
344  pre_alloc_result_->stalled = true;
345  current_active_goal->setAborted(pre_alloc_result_);
346  }
347  }
348 }
349 
350 } // namespace
351 
352 #endif // header guard
bool init(hardware_interface::JointHandle &joint_handle, ros::NodeHandle &controller_nh)
urdf::ModelSharedPtr getUrdf(const ros::NodeHandle &nh, const std::string &param_name)
#define ROS_DEBUG_STREAM_NAMED(name, args)
realtime_tools::RealtimeBuffer< Commands > command_
#define ROS_ERROR_STREAM_NAMED(name, args)
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
bool init(HardwareInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
void start()
boost::shared_ptr< const Goal > getGoal() const
std::vector< urdf::JointConstSharedPtr > getUrdfJoints(const urdf::Model &urdf, const std::vector< std::string > &joint_names)
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)
std::string getLeafNamespace(const ros::NodeHandle &nh)
void setAccepted(const std::string &text=std::string(""))
#define ROS_DEBUG_NAMED(name,...)
HwIfaceAdapter hw_iface_adapter_
Adapts desired goal state to HW interface.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void checkForSuccess(const ros::Time &time, double error_position, double current_position, double current_velocity)
Check for success and publish appropriate result and feedback.
void starting(const ros::Time &time)
Holds the current position.
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
const std::string & getNamespace() const
RealtimeGoalHandlePtr rt_active_goal_
Currently active action goal, if any.
actionlib::ActionServer< control_msgs::GripperCommandAction > ActionServer
hardware_interface::JointHandle joint_
Handles to controlled joints.
void runNonRealtime(const ros::TimerEvent &te)
void update(const ros::Time &time, const ros::Duration &period)
bool getParam(const std::string &key, std::string &s) const
realtime_tools::RealtimeServerGoalHandle< control_msgs::GripperCommandAction > RealtimeGoalHandle
void stopping(const ros::Time &time)
Cancels the active action goal, if any.
#define ROS_ERROR_NAMED(name,...)
static Time now()
#define ROS_ERROR_STREAM(args)
control_msgs::GripperCommandResultPtr pre_alloc_result_


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