controller_base.h
Go to the documentation of this file.
1 // Copyright (C) 2012, hiDOF INC.
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 hiDOF, Inc. 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 
28 /*
29  * Author: Wim Meeussen
30  */
31 
32 #pragma once
33 
34 
35 #include <ros/node_handle.h>
37 #include <memory>
38 
39 namespace controller_interface
40 {
41 
49 {
50 public:
51  ControllerBase() = default;
52  virtual ~ControllerBase() = default;
53  ControllerBase(const ControllerBase&) = delete;
54  ControllerBase& operator=(const ControllerBase&) = delete;
55  ControllerBase(ControllerBase&&) = delete;
57 
66  virtual void starting(const ros::Time& /*time*/) {}
67 
73  virtual void update(const ros::Time& time, const ros::Duration& period) = 0;
74 
80  virtual void stopping(const ros::Time& /*time*/) {}
81 
87  virtual void waiting(const ros::Time& /*time*/) {}
88 
94  virtual void aborting(const ros::Time& /*time*/) {}
95 
99  bool isInitialized() const
100  {
101  return state_ == INITIALIZED;
102  }
103 
107  bool isRunning() const
108  {
109  return state_ == RUNNING;
110  }
111 
115  bool isStopped() const
116  {
117  return state_ == STOPPED;
118  }
119 
123  bool isWaiting() const
124  {
125  return state_ == WAITING;
126  }
127 
131  bool isAborted() const
132  {
133  return state_ == ABORTED;
134  }
135 
137  void updateRequest(const ros::Time& time, const ros::Duration& period)
138  {
139  if (state_ == RUNNING)
140  {
141  update(time, period);
142  }
143  }
144 
146  bool startRequest(const ros::Time& time)
147  {
148  // start works from any state, except CONSTRUCTED
149  if (state_ != CONSTRUCTED)
150  {
151  starting(time);
152  state_ = RUNNING;
153  return true;
154  }
155  else
156  {
157  ROS_FATAL("Failed to start controller. It is not initialized.");
158  return false;
159  }
160  }
161 
163  bool stopRequest(const ros::Time& time)
164  {
165  // stop works from any state, except CONSTRUCTED
166  if (state_ != CONSTRUCTED)
167  {
168  stopping(time);
169  state_ = STOPPED;
170  return true;
171  }
172  else
173  {
174  ROS_FATAL("Failed to stop controller. It is not initialized.");
175  return false;
176  }
177  }
178 
180  bool waitRequest(const ros::Time& time)
181  {
182  // wait works from any state, except CONSTRUCTED
183  if (state_ != CONSTRUCTED)
184  {
185  waiting(time);
186  state_ = WAITING;
187  return true;
188  }
189  else
190  {
191  ROS_FATAL("Failed to wait controller. It is not initialized.");
192  return false;
193  }
194  }
195 
197  bool abortRequest(const ros::Time& time)
198  {
199  // abort works from any state, except CONSTRUCTED
200  if (state_ != CONSTRUCTED)
201  {
202  aborting(time);
203  state_ = ABORTED;
204  return true;
205  }
206  else
207  {
208  ROS_FATAL("Failed to abort controller. It is not initialized.");
209  return false;
210  }
211  }
212 
213  /*\}*/
214 
218  typedef std::vector<hardware_interface::InterfaceResources> ClaimedResources;
219 
236  virtual bool initRequest(hardware_interface::RobotHW* robot_hw,
237  ros::NodeHandle& root_nh,
238  ros::NodeHandle& controller_nh,
239  ClaimedResources& claimed_resources) = 0;
240 
241  /*\}*/
242 
245 };
246 
247 typedef std::shared_ptr<ControllerBase> ControllerBaseSharedPtr;
248 
249 }
#define ROS_FATAL(...)
std::shared_ptr< ControllerBase > ControllerBaseSharedPtr
bool abortRequest(const ros::Time &time)
Calls abort unless this controller is just constructed.
std::vector< hardware_interface::InterfaceResources > ClaimedResources
bool isAborted() const
Check if the controller is aborted.
virtual void stopping(const ros::Time &)
This is called from within the realtime thread just after the last update call before the controller ...
bool waitRequest(const ros::Time &time)
Calls waiting unless this controller is just constructed.
virtual void update(const ros::Time &time, const ros::Duration &period)=0
This is called periodically by the realtime thread when the controller is running.
Abstract Controller Interface.
ControllerBase & operator=(const ControllerBase &)=delete
void updateRequest(const ros::Time &time, const ros::Duration &period)
Calls update only if this controller is running.
bool isStopped() const
Check if the controller is stopped.
bool startRequest(const ros::Time &time)
Calls starting unless this controller is just constructed.
virtual bool initRequest(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources)=0
Request that the controller be initialized.
bool stopRequest(const ros::Time &time)
Calls stopping unless this controller is just constructed.
bool isWaiting() const
Check if the controller is waiting.
enum controller_interface::ControllerBase::@0 state_
The current execution state of the controller.
virtual void aborting(const ros::Time &)
This is called from within the realtime thread when the controller needs to be aborted.
virtual void waiting(const ros::Time &)
This is called from within the realtime thread while the controller is waiting to start...
virtual void starting(const ros::Time &)
This is called from within the realtime thread just before the first call to update.
bool isRunning() const
Check if the controller is running.
bool isInitialized() const
Check if the controller is initialized.


controller_interface
Author(s): Wim Meeussen
autogenerated on Mon Feb 28 2022 23:30:15