Source Code for Module baxter_interface.robust_controller

  1  # Copyright (c) 2013-2014, Rethink Robotics 
  2  # All rights reserved. 
  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  # 
  7  # 1. Redistributions of source code must retain the above copyright notice, 
  8  #    this list of conditions and the following disclaimer. 
  9  # 2. 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  # 3. Neither the name of the Rethink Robotics 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. 
 27   
 28  import errno 
 29   
 30  import rospy 
 31   
 32  from baxter_core_msgs.msg import ( 
 33      RobustControllerStatus, 
 34  ) 
 35   
 36   
 37 -class RobustController(object): 
 38      (STATE_IDLE, 
 39       STATE_STARTING, 
 40       STATE_RUNNING, 
 41       STATE_STOPPING) = range(4) 
 42   
 43 -    def __init__(self, namespace, enable_msg, disable_msg, timeout=60): 
 44          """ 
 45          Wrapper around controlling a RobustController 
 46   
 47          @param namespace: namespace containing the enable and status topics 
 48          @param enable_msg: message to send to enable the RC 
 49          @param disable_msg: message to send to disable the RC 
 50          @param timeout: seconds to wait for the RC to finish [60] 
 51          """ 
 52          self._command_pub = rospy.Publisher( 
 53              namespace + '/enable', 
 54              type(enable_msg)) 
 55          self._status_sub = rospy.Subscriber( 
 56              namespace + '/status', 
 57              RobustControllerStatus, 
 58              self._callback) 
 59   
 60          self._enable_msg = enable_msg 
 61          self._disable_msg = disable_msg 
 62          self._timeout = timeout 
 63          self._state = self.STATE_IDLE 
 64          self._return = 0 
 65   
 66          rospy.on_shutdown(self._on_shutdown) 
 67   
 68 -    def _callback(self, msg): 
 69          if self._state == self.STATE_RUNNING: 
 70              if msg.complete == RobustControllerStatus.COMPLETE_W_SUCCESS: 
 71                  self._state = self.STATE_STOPPING 
 72                  self._return = 0 
 73   
 74              elif msg.complete == RobustControllerStatus.COMPLETE_W_FAILURE: 
 75                  self._state = self.STATE_STOPPING 
 76                  self._return = errno.EIO 
 77   
 78              elif not msg.isEnabled: 
 79                  self._state = self.STATE_IDLE 
 80                  self._return = errno.ENOMSG 
 81   
 82          elif self._state == self.STATE_STOPPING and not msg.isEnabled: 
 83              # Would be nice to use msg.state here, but it does not 
 84              # consistently reflect reality. 
 85              self._state = self.STATE_IDLE 
 86   
 87          elif self._state == self.STATE_STARTING and msg.isEnabled: 
 88              self._state = self.STATE_RUNNING 
 89   
 90 -    def _run_loop(self): 
 91          # RobustControllers need messages at < 1Hz in order to continue 
 92          # their current operation. 
 93          rate = rospy.Rate(2) 
 94          start = rospy.Time.now() 
 95   
 96          while not rospy.is_shutdown(): 
 97              if (self._state == self.STATE_RUNNING and 
 98                  (rospy.Time.now() - start).to_sec() > self._timeout): 
 99                  self._state = self.STATE_STOPPING 
100                  self._command_pub.publish(self._disable_msg) 
101                  self._return = errno.ETIMEDOUT 
102   
103              elif self._state in (self.STATE_STARTING, self.STATE_RUNNING): 
104                  self._command_pub.publish(self._enable_msg) 
105   
106              elif self._state == self.STATE_STOPPING: 
107                  self._command_pub.publish(self._disable_msg) 
108   
109              elif self._state == self.STATE_IDLE: 
110                  break 
111   
112              rate.sleep() 
113   
114 -    def _on_shutdown(self): 
115          rate = rospy.Rate(2) 
116   
117          while not self._state == self.STATE_IDLE: 
118              self._command_pub.publish(self._disable_msg) 
119              rate.sleep() 
120   
121          self._return = errno.ECONNABORTED 
122   
123 -    def run(self): 
124          """ 
125          Enable the RobustController and run until completion or error. 
126          """ 
127          self._state = self.STATE_STARTING 
128          self._command_pub.publish(self._enable_msg) 
129          self._run_loop() 
130          if self._return != 0: 
131              msgs = { 
132                  errno.EIO:          "Robust controller failed", 
133                  errno.ENOMSG:       "Robust controller failed to enable", 
134                  errno.ETIMEDOUT:    "Robust controller timed out", 
135                  errno.ECONNABORTED: "Robust controller interrupted by user", 
136                  } 
137   
138              msg = msgs.get(self._return, None) 
139              if msg: 
140                  raise IOError(self._return, msg) 
141              raise IOError(self._return) 
142