Source Code for Module intera_interface.robot_params

  1  # Copyright (c) 2016, 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   
 29  import rospy 
 30  import socket 
 31   
 32 -class RobotParams(object): 
 33      """ 
 34      Interface class for essential ROS parameters on Intera robot. 
 35      """ 
 36   
 37 -    def __init__(self): 
 38          self._sdk_networking_url = "http://sdk.rethinkrobotics.com/intera/Networking" 
 39          self._ros_networking_url = "http://wiki.ros.org/ROS/NetworkSetup" 
 40          self._color_dict = {"INFO":'37',"WARN":'33',"ERROR":'31'} 
 41          self._color_prefix = "\033[1;{0}m" 
 42          self._color_suffix = "\033[1;m" 
 43   
 44 -    def get_camera_names(self): 
 45          """ Return the names of the camera. 
 46          @rtype: list [str] 
 47          @return: ordered list of camera names 
 48          """ 
 49          return self.get_camera_details().keys() 
 50   
 51 -    def get_camera_details(self): 
 52          """ 
 53          Return the details of the cameras. 
 54   
 55          @rtype: list [str] 
 56          @return: ordered list of camera details 
 57          """ 
 58          camera_dict = dict() 
 59          camera_config_param = "/robot_config/camera_config" 
 60          try: 
 61              camera_dict = rospy.get_param(camera_config_param) 
 62          except KeyError: 
 63              rospy.logerr("RobotParam:get_camera_details cannot detect any " 
 64                  "cameras under the parameter {0}".format(camera_config_param)) 
 65          except (socket.error, socket.gaierror): 
 66              self._log_networking_error() 
 67          return  camera_dict 
 68   
 69 -    def get_limb_names(self): 
 70          """ 
 71          Return the names of the robot's articulated limbs from ROS parameter. 
 72   
 73          @rtype: list [str] 
 74          @return: ordered list of articulated limbs names 
 75                   (e.g. right, left). on networked robot 
 76          """ 
 77          non_limb_assemblies = ['torso', 'head'] 
 78          return list(set(self.get_robot_assemblies()).difference(non_limb_assemblies)) 
 79   
 80 -    def get_robot_assemblies(self): 
 81          """ 
 82          Return the names of the robot's assemblies from ROS parameter. 
 83   
 84          @rtype: list [str] 
 85          @return: ordered list of assembly names 
 86                   (e.g. right, left, torso, head). on networked robot 
 87          """ 
 88          assemblies = list() 
 89          try: 
 90              assemblies = rospy.get_param("/robot_config/assembly_names") 
 91          except KeyError: 
 92              rospy.logerr("RobotParam:get_robot_assemblies cannot detect assembly names" 
 93                           " under param /robot_config/assembly_names") 
 94          except (socket.error, socket.gaierror): 
 95              self._log_networking_error() 
 96          return assemblies 
 97   
 98 -    def get_joint_names(self, limb_name): 
 99          """ 
100          Return the names of the joints for the specified 
101          limb from ROS parameter. 
102   
103          @type  limb_name: str 
104          @param limb_name: name of the limb for which to retrieve joint names 
105   
106          @rtype: list [str] 
107          @return: ordered list of joint names from proximal to distal 
108                   (i.e. shoulder to wrist). joint names for limb 
109          """ 
110          joint_names = list() 
111          try: 
112              joint_names = rospy.get_param( 
113                              "robot_config/{0}_config/joint_names".format(limb_name)) 
114          except KeyError: 
115              rospy.logerr(("RobotParam:get_joint_names cannot detect joint_names for" 
116                            " arm \"{0}\"").format(limb_name)) 
117          except (socket.error, socket.gaierror): 
118              self._log_networking_error() 
119          return joint_names 
120   
121 -    def get_robot_name(self): 
122          """ 
123          Return the name of class of robot from ROS parameter. 
124   
125          @rtype: str 
126          @return: name of the class of robot (eg. "sawyer", "baxter", etc.) 
127          """ 
128          robot_name = None 
129          try: 
130              robot_name = rospy.get_param("/manifest/robot_class") 
131          except KeyError: 
132              rospy.logerr("RobotParam:get_robot_name cannot detect robot name" 
133                           " under param /manifest/robot_class") 
134          except (socket.error, socket.gaierror): 
135              self._log_networking_error() 
136          return robot_name 
137   
138 -    def _log_networking_error(self): 
139          msg = ("Failed to connect to the ROS parameter server!\n" 
140                 "Please check to make sure your ROS networking is " 
141                 "properly configured:\n" 
142                 "Intera SDK Networking Reference: {0}\n" 
143                 "ROS Networking Reference:        {1}").format( 
144                          self._sdk_networking_url, 
145                          self._ros_networking_url) 
146          self.log_message(msg, "ERROR") 
147   
148 -    def log_message(self, msg, level="INFO"): 
149          """ 
150          Print the desired message on stdout using 
151          level-colored text. 
152   
153          @type  msg: str 
154          @param msg: Message text to be desplayed 
155   
156          @type  level: str 
157          @param level: Level to color the text. Valid levels: 
158                        ["INFO", "WARN", "ERROR"] 
159          """ 
160          print(("{0}[{1}] {2}{3}").format( 
161                          self._color_prefix.format(self._color_dict[level]), 
162                          level, msg, self._color_suffix)) 
163