action_based_controller_handle.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Unbounded Robotics Inc.
5  * Copyright (c) 2012, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */
37 
38 #ifndef MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE
39 #define MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE
40 
44 #include <memory>
45 
47 {
48 /*
49  * This exist solely to inject addJoint/getJoints into base non-templated class.
50  */
52 {
53 public:
55  {
56  }
57 
58  virtual void addJoint(const std::string& name) = 0;
59  virtual void getJoints(std::vector<std::string>& joints) = 0;
60 };
61 
63 
64 /*
65  * This is a simple base class, which handles all of the action creation/etc
66  */
67 template <typename T>
69 {
70 public:
71  ActionBasedControllerHandle(const std::string& name, const std::string& ns)
72  : ActionBasedControllerHandleBase(name), nh_("~"), done_(true), namespace_(ns)
73  {
74  controller_action_client_.reset(new actionlib::SimpleActionClient<T>(getActionName(), true));
75  unsigned int attempts = 0;
76  double timeout;
77  nh_.param("trajectory_execution/controller_connection_timeout", timeout, 15.0);
78 
79  if (timeout == 0.0)
80  {
81  while (ros::ok() && !controller_action_client_->waitForServer(ros::Duration(5.0)))
82  {
83  ROS_WARN_STREAM_NAMED("ActionBasedController", "Waiting for " << getActionName() << " to come up");
84  ros::Duration(1).sleep();
85  }
86  }
87  else
88  {
89  while (ros::ok() && !controller_action_client_->waitForServer(ros::Duration(timeout / 3)) && ++attempts < 3)
90  {
91  ROS_WARN_STREAM_NAMED("ActionBasedController", "Waiting for " << getActionName() << " to come up");
92  ros::Duration(1).sleep();
93  }
94  }
95  if (!controller_action_client_->isServerConnected())
96  {
97  ROS_ERROR_STREAM_NAMED("ActionBasedController", "Action client not connected: " << getActionName());
98  controller_action_client_.reset();
99  }
100 
102  }
103 
104  bool isConnected() const
105  {
106  return static_cast<bool>(controller_action_client_);
107  }
108 
109  virtual bool cancelExecution()
110  {
111  if (!controller_action_client_)
112  return false;
113  if (!done_)
114  {
115  ROS_INFO_STREAM_NAMED("ActionBasedController", "Cancelling execution for " << name_);
116  controller_action_client_->cancelGoal();
118  done_ = true;
119  }
120  return true;
121  }
122 
123  virtual bool waitForExecution(const ros::Duration& timeout = ros::Duration(0))
124  {
125  if (controller_action_client_ && !done_)
126  return controller_action_client_->waitForResult(timeout);
127  return true;
128  }
129 
131  {
132  return last_exec_;
133  }
134 
135  virtual void addJoint(const std::string& name)
136  {
137  joints_.push_back(name);
138  }
139 
140  virtual void getJoints(std::vector<std::string>& joints)
141  {
142  joints = joints_;
143  }
144 
145 protected:
147  std::string getActionName(void) const
148  {
149  if (namespace_.empty())
150  return name_;
151  else
152  return name_ + "/" + namespace_;
153  }
154 
156  {
157  ROS_DEBUG_STREAM_NAMED("ActionBasedController", "Controller " << name_ << " is done with state " << state.toString()
158  << ": " << state.getText());
165  else
167  done_ = true;
168  }
169 
170  /* execution status */
172  bool done_;
173 
174  /* the controller namespace, for instance, topics will map to name/ns/goal,
175  * name/ns/result, etc */
176  std::string namespace_;
177 
178  /* the joints controlled by this controller */
179  std::vector<std::string> joints_;
180 
181  /* action client */
182  std::shared_ptr<actionlib::SimpleActionClient<T> > controller_action_client_;
183 };
184 
185 } // end namespace moveit_simple_controller_manager
186 
187 #endif // MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE
virtual void getJoints(std::vector< std::string > &joints)=0
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
bool sleep() const
#define ROS_INFO_STREAM_NAMED(name, args)
std::string toString() const
virtual bool waitForExecution(const ros::Duration &timeout=ros::Duration(0))
ROSCPP_DECL bool ok()
ActionBasedControllerHandle(const std::string &name, const std::string &ns)
std::string getText() const
std::shared_ptr< actionlib::SimpleActionClient< T > > controller_action_client_
virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
void finishControllerExecution(const actionlib::SimpleClientGoalState &state)
MOVEIT_CLASS_FORWARD(ActionBasedControllerHandleBase)
#define ROS_WARN_STREAM_NAMED(name, args)


moveit_simple_controller_manager
Author(s): Michael Ferguson
autogenerated on Sat Jun 27 2020 03:51:48