Source Code for Module intera_interface.robot_params

  1  # Copyright (c) 2013-2017, Rethink Robotics Inc. 
  2  # 
  3  # Licensed under the Apache License, Version 2.0 (the "License"); 
  4  # you may not use this file except in compliance with the License. 
  5  # You may obtain a copy of the License at 
  6  # 
  7  #     http://www.apache.org/licenses/LICENSE-2.0 
  8  # 
  9  # Unless required by applicable law or agreed to in writing, software 
 10  # distributed under the License is distributed on an "AS IS" BASIS, 
 11  # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
 12  # See the License for the specific language governing permissions and 
 13  # limitations under the License. 
 14   
 15  import rospy 
 16  import socket 
 17   
 18 -class RobotParams(object): 
 19      """ 
 20      Interface class for essential ROS parameters on Intera robot. 
 21      """ 
 22   
 23 -    def __init__(self): 
 24          self._sdk_networking_url = "http://sdk.rethinkrobotics.com/intera/Networking" 
 25          self._ros_networking_url = "http://wiki.ros.org/ROS/NetworkSetup" 
 26          self._color_dict = {"INFO":'37',"WARN":'33',"ERROR":'31'} 
 27          self._color_prefix = "\033[1;{0}m" 
 28          self._color_suffix = "\033[1;m" 
 29   
 30 -    def get_camera_names(self): 
 31          """ Return the names of the camera. 
 32          @rtype: list [str] 
 33          @return: ordered list of camera names 
 34          """ 
 35          return self.get_camera_details().keys() 
 36   
 37 -    def get_camera_details(self): 
 38          """ 
 39          Return the details of the cameras. 
 40   
 41          @rtype: list [str] 
 42          @return: ordered list of camera details 
 43          """ 
 44          camera_dict = dict() 
 45          camera_config_param = "/robot_config/camera_config" 
 46          try: 
 47              camera_dict = rospy.get_param(camera_config_param) 
 48          except KeyError: 
 49              rospy.logerr("RobotParam:get_camera_details cannot detect any " 
 50                  "cameras under the parameter {0}".format(camera_config_param)) 
 51          except (socket.error, socket.gaierror): 
 52              self._log_networking_error() 
 53          return  camera_dict 
 54   
 55 -    def get_limb_names(self): 
 56          """ 
 57          Return the names of the robot's articulated limbs from ROS parameter. 
 58   
 59          @rtype: list [str] 
 60          @return: ordered list of articulated limbs names 
 61                   (e.g. right, left). on networked robot 
 62          """ 
 63          non_limb_assemblies = ['torso', 'head'] 
 64          return list(set(self.get_robot_assemblies()).difference(non_limb_assemblies)) 
 65   
 66 -    def get_robot_assemblies(self): 
 67          """ 
 68          Return the names of the robot's assemblies from ROS parameter. 
 69   
 70          @rtype: list [str] 
 71          @return: ordered list of assembly names 
 72                   (e.g. right, left, torso, head). on networked robot 
 73          """ 
 74          assemblies = list() 
 75          try: 
 76              assemblies = rospy.get_param("/robot_config/assembly_names") 
 77          except KeyError: 
 78              rospy.logerr("RobotParam:get_robot_assemblies cannot detect assembly names" 
 79                           " under param /robot_config/assembly_names") 
 80          except (socket.error, socket.gaierror): 
 81              self._log_networking_error() 
 82          return assemblies 
 83   
 84 -    def get_joint_names(self, limb_name): 
 85          """ 
 86          Return the names of the joints for the specified 
 87          limb from ROS parameter. 
 88   
 89          @type  limb_name: str 
 90          @param limb_name: name of the limb for which to retrieve joint names 
 91   
 92          @rtype: list [str] 
 93          @return: ordered list of joint names from proximal to distal 
 94                   (i.e. shoulder to wrist). joint names for limb 
 95          """ 
 96          joint_names = list() 
 97          try: 
 98              joint_names = rospy.get_param( 
 99                              "robot_config/{0}_config/joint_names".format(limb_name)) 
100          except KeyError: 
101              rospy.logerr(("RobotParam:get_joint_names cannot detect joint_names for" 
102                            " arm \"{0}\"").format(limb_name)) 
103          except (socket.error, socket.gaierror): 
104              self._log_networking_error() 
105          return joint_names 
106   
107 -    def get_robot_name(self): 
108          """ 
109          Return the name of class of robot from ROS parameter. 
110   
111          @rtype: str 
112          @return: name of the class of robot (eg. "sawyer", "baxter", etc.) 
113          """ 
114          robot_name = None 
115          try: 
116              robot_name = rospy.get_param("/manifest/robot_class") 
117          except KeyError: 
118              rospy.logerr("RobotParam:get_robot_name cannot detect robot name" 
119                           " under param /manifest/robot_class") 
120          except (socket.error, socket.gaierror): 
121              self._log_networking_error() 
122          return robot_name 
123   
124 -    def _log_networking_error(self): 
125          msg = ("Failed to connect to the ROS parameter server!\n" 
126                 "Please check to make sure your ROS networking is " 
127                 "properly configured:\n" 
128                 "Intera SDK Networking Reference: {0}\n" 
129                 "ROS Networking Reference:        {1}").format( 
130                          self._sdk_networking_url, 
131                          self._ros_networking_url) 
132          self.log_message(msg, "ERROR") 
133   
134 -    def log_message(self, msg, level="INFO"): 
135          """ 
136          Print the desired message on stdout using 
137          level-colored text. 
138   
139          @type  msg: str 
140          @param msg: Message text to be desplayed 
141   
142          @type  level: str 
143          @param level: Level to color the text. Valid levels: 
144                        ["INFO", "WARN", "ERROR"] 
145          """ 
146          print(("{0}[{1}] {2}{3}").format( 
147                          self._color_prefix.format(self._color_dict[level]), 
148                          level, msg, self._color_suffix)) 
149