00001 00002 // Copyright (C) 2012, hiDOF INC. 00003 // 00004 // Redistribution and use in source and binary forms, with or without 00005 // modification, are permitted provided that the following conditions are met: 00006 // * Redistributions of source code must retain the above copyright notice, 00007 // this list of conditions and the following disclaimer. 00008 // * Redistributions in binary form must reproduce the above copyright 00009 // notice, this list of conditions and the following disclaimer in the 00010 // documentation and/or other materials provided with the distribution. 00011 // * Neither the name of hiDOF, Inc. nor the names of its 00012 // contributors may be used to endorse or promote products derived from 00013 // this software without specific prior written permission. 00014 // 00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00025 // POSSIBILITY OF SUCH DAMAGE. 00027 00028 /* 00029 * Author: Wim Meeussen 00030 */ 00031 00032 00033 #ifndef CONTROLLER_INTERFACE_CONTROLLER_BASE_H 00034 #define CONTROLLER_INTERFACE_CONTROLLER_BASE_H 00035 00036 #include <ros/node_handle.h> 00037 #include <hardware_interface/robot_hw.h> 00038 00039 00040 namespace controller_interface 00041 { 00042 00049 class ControllerBase 00050 { 00051 public: 00052 ControllerBase(): state_(CONSTRUCTED){} 00053 virtual ~ControllerBase(){} 00054 00063 virtual void starting(const ros::Time& time) {}; 00064 00070 virtual void update(const ros::Time& time, const ros::Duration& period) = 0; 00071 00077 virtual void stopping(const ros::Time& time) {}; 00078 00082 bool isRunning() 00083 { 00084 return (state_ == RUNNING); 00085 } 00086 00088 void updateRequest(const ros::Time& time, const ros::Duration& period) 00089 { 00090 if (state_ == RUNNING) 00091 update(time, period); 00092 } 00093 00095 bool startRequest(const ros::Time& time) 00096 { 00097 // start succeeds even if the controller was already started 00098 if (state_ == RUNNING || state_ == INITIALIZED){ 00099 starting(time); 00100 state_ = RUNNING; 00101 return true; 00102 } 00103 else 00104 return false; 00105 } 00106 00108 bool stopRequest(const ros::Time& time) 00109 { 00110 // stop succeeds even if the controller was already stopped 00111 if (state_ == RUNNING || state_ == INITIALIZED){ 00112 stopping(time); 00113 state_ = INITIALIZED; 00114 return true; 00115 } 00116 else 00117 return false; 00118 } 00119 00120 /*\}*/ 00121 00125 00126 virtual std::string getHardwareInterfaceType() const = 0; 00127 00143 virtual bool initRequest(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh, 00144 std::set<std::string>& claimed_resources) = 0; 00145 00146 /*\}*/ 00147 00149 enum {CONSTRUCTED, INITIALIZED, RUNNING} state_; 00150 00151 00152 private: 00153 ControllerBase(const ControllerBase &c); 00154 ControllerBase& operator =(const ControllerBase &c); 00155 00156 }; 00157 00158 } 00159 00160 00161 #endif