Source Code for Module baxter_interface.robust_controller

  1  # Copyright (c) 2013-2015, 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              queue_size=10) 
 56          self._status_sub = rospy.Subscriber( 
 57              namespace + '/status', 
 58              RobustControllerStatus, 
 59              self._callback) 
 60   
 61          self._enable_msg = enable_msg 
 62          self._disable_msg = disable_msg 
 63          self._timeout = timeout 
 64          self._state = self.STATE_IDLE 
 65          self._return = 0 
 66   
 67          rospy.on_shutdown(self._on_shutdown) 
 68   
 69 -    def _callback(self, msg): 
 70          if self._state == self.STATE_RUNNING: 
 71              if msg.complete == RobustControllerStatus.COMPLETE_W_SUCCESS: 
 72                  self._state = self.STATE_STOPPING 
 73                  self._return = 0 
 74   
 75              elif msg.complete == RobustControllerStatus.COMPLETE_W_FAILURE: 
 76                  self._state = self.STATE_STOPPING 
 77                  self._return = errno.EIO 
 78   
 79              elif not msg.isEnabled: 
 80                  self._state = self.STATE_IDLE 
 81                  self._return = errno.ENOMSG 
 82   
 83          elif self._state == self.STATE_STOPPING and not msg.isEnabled: 
 84              # Would be nice to use msg.state here, but it does not 
 85              # consistently reflect reality. 
 86              self._state = self.STATE_IDLE 
 87   
 88          elif self._state == self.STATE_STARTING and msg.isEnabled: 
 89              self._state = self.STATE_RUNNING 
 90   
 91 -    def _run_loop(self): 
 92          # RobustControllers need messages at < 1Hz in order to continue 
 93          # their current operation. 
 94          rate = rospy.Rate(2) 
 95          start = rospy.Time.now() 
 96   
 97          while not rospy.is_shutdown(): 
 98              if (self._state == self.STATE_RUNNING and 
 99                  (rospy.Time.now() - start).to_sec() > self._timeout): 
100                  self._state = self.STATE_STOPPING 
101                  self._command_pub.publish(self._disable_msg) 
102                  self._return = errno.ETIMEDOUT 
103   
104              elif self._state in (self.STATE_STARTING, self.STATE_RUNNING): 
105                  self._command_pub.publish(self._enable_msg) 
106   
107              elif self._state == self.STATE_STOPPING: 
108                  self._command_pub.publish(self._disable_msg) 
109   
110              elif self._state == self.STATE_IDLE: 
111                  break 
112   
113              rate.sleep() 
114   
115 -    def _on_shutdown(self): 
116          rate = rospy.Rate(2) 
117   
118          while not self._state == self.STATE_IDLE: 
119              self._command_pub.publish(self._disable_msg) 
120              rate.sleep() 
121   
122          self._return = errno.ECONNABORTED 
123   
124 -    def run(self): 
125          """ 
126          Enable the RobustController and run until completion or error. 
127          """ 
128          self._state = self.STATE_STARTING 
129          self._command_pub.publish(self._enable_msg) 
130          self._run_loop() 
131          if self._return != 0: 
132              msgs = { 
133                  errno.EIO:          "Robust controller failed", 
134                  errno.ENOMSG:       "Robust controller failed to enable", 
135                  errno.ETIMEDOUT:    "Robust controller timed out", 
136                  errno.ECONNABORTED: "Robust controller interrupted by user", 
137                  } 
138   
139              msg = msgs.get(self._return, None) 
140              if msg: 
141                  raise IOError(self._return, msg) 
142              raise IOError(self._return) 
143