00001 """ 00002 File used to determine the commands sent to the robot are valid commands. The file organizes the parameters defined in 00003 python_ethernet_rmp.system_defines.py into dictionaries. 00004 00005 Author: Chris Dunkers, Worcester Polytechnic Institute 00006 Author: Russell Toris, Worcester Polytechnic Institute 00007 Version: June 12, 2014 00008 """ 00009 00010 from python_ethernet_rmp.system_defines import * 00011 from python_ethernet_rmp.utils import * 00012 import rospy 00013 import sys 00014 00015 def is_range_set(arg1, arg2): 00016 """ 00017 Function to check the range of the parameter value. If out of range the parameter is set as the boundary. 00018 :param arg1: The name of the parameter. 00019 :param arg2: The parameter value. 00020 :return The bounded parameter value converted to a uint32 and a true flag. 00021 """ 00022 if arg2 > config_params_max[arg1]: 00023 rospy.logwarn("%s over limit. Set to %s", config_params_names[arg1], config_params_max[arg1]) 00024 return [convert_float_to_u32(config_params_max[arg1]), True] 00025 elif arg2 < config_params_min[arg1]: 00026 rospy.logwarn("%s under limit. Set to %s", config_params_names[arg1], config_params_min[arg1]) 00027 return [convert_float_to_u32(config_params_min[arg1]), True] 00028 else: 00029 return [convert_float_to_u32(arg2), True] 00030 00031 00032 def is_range_no_set(arg1, arg2): 00033 """ 00034 Function to check the range of the parameter value. If out of range the parameter is not sent to the robot. 00035 :param arg1: The name of the parameter. 00036 :param arg2: The parameter value. 00037 :return The parameter value and a flag indicating if the value is within the ranges. 00038 """ 00039 if arg2 > config_params_max[arg1]: 00040 rospy.logwarn("%s over limit. Command not sent.", config_params_names[arg1], config_params_max[arg1]) 00041 return [arg2, False] 00042 elif arg2 < config_params_min[arg1]: 00043 rospy.logwarn("%s under limit. Command not sent.", config_params_names[arg1], config_params_min[arg1]) 00044 return [arg2, False] 00045 else: 00046 return [arg2, True] 00047 00048 def is_range(arg1, arg2): 00049 """ 00050 Function to check the range of the parameter value. If out of range the parameter is not sent to the robot 00051 :param arg1: The name of the parameter. 00052 :param arg2: The parameter value. 00053 :return The parameter value cast to an int and a flag indicating if the value is within the ranges. 00054 """ 00055 if arg2 > config_params_max[arg1]: 00056 rospy.logwarn("%s over limit. Command not sent.", config_params_names[arg1], config_params_max[arg1]) 00057 return [int(arg2), False] 00058 elif arg2 < config_params_min[arg1]: 00059 rospy.logwarn("%s under limit. Command not sent.", config_params_names[arg1], config_params_min[arg1]) 00060 return [int(arg2), False] 00061 else: 00062 return [int(arg2), True] 00063 00064 00065 def is_quad(arg1, arg2): 00066 """ 00067 Checks to make sure the IP info is properly configured. 00068 :param arg1: The name of the parameter. 00069 :param arg2: The parameter value. 00070 :return The parameter value and a flag indicating if the value is within the ranges. 00071 """ 00072 try: 00073 return [dottedQuadToNum(arg2), True] 00074 except: 00075 rospy.logwarn("%s is invalid. Command not sent.",config_params_names[arg1]) 00076 return [arg2, False] 00077 00078 00079 def is_true(arg1, arg2): 00080 """ 00081 Returns True as the command is always valid. 00082 :param arg1: The name of the parameter (unused). 00083 :param arg2: The parameter value. 00084 :return The parameter value and a flag indicating true. 00085 """ 00086 return [arg2, True] 00087 00088 00089 def is_not_param(arg1, arg2): 00090 """ 00091 Prints a rospy warning that the command does not exist. 00092 :param arg1: The name of the parameter. 00093 :param arg2: The parameter value. 00094 :return The parameter value and a flag indicating false. 00095 """ 00096 rospy.logwarn("%s is invalid. Command not sent.", arg1) 00097 return [arg2, False] 00098 00099 """ 00100 Dictionary to define the maximum values. 00101 """ 00102 config_params_max = dict({ 00103 RMP_CMD_NONE: 0, 00104 RMP_CMD_SET_MAXIMUM_VELOCITY: MAX_VELOCITY_MPS, 00105 RMP_CMD_SET_MAXIMUM_ACCELERATION: MAX_ACCELERATION_MPS2, 00106 RMP_CMD_SET_MAXIMUM_DECELERATION: MAX_DECELERATION_MPS2, 00107 RMP_CMD_SET_MAXIMUM_DTZ_DECEL_RATE: MAX_DTZ_DECEL_RATE_MPS2, 00108 RMP_CMD_SET_COASTDOWN_ACCEL: MAX_COASTDOWN_ACCEL_MPS2, 00109 RMP_CMD_SET_MAXIMUM_TURN_RATE: MAX_YAW_RATE_RPS, 00110 RMP_CMD_SET_MAXIMUM_TURN_ACCEL: MAX_YAW_ACCEL_RPS2, 00111 RMP_CMD_SET_TIRE_DIAMETER: MAX_TIRE_DIAMETER_M, 00112 RMP_CMD_SET_WHEEL_BASE_LENGTH: MAX_WHEEL_BASE_LENGTH_M, 00113 RMP_CMD_SET_WHEEL_TRACK_WIDTH: MAX_WHEEL_TRACK_WIDTH_M, 00114 RMP_CMD_SET_TRANSMISSION_RATIO: MAX_TRANSMISSION_RATIO, 00115 RMP_CMD_SET_INPUT_CONFIG_BITMAP: 4294967295, 00116 RMP_CMD_SET_ETH_IP_ADDRESS: None, 00117 RMP_CMD_SET_ETH_PORT_NUMBER: 65535, 00118 RMP_CMD_SET_ETH_SUBNET_MASK: None, 00119 RMP_CMD_SET_ETH_GATEWAY: None, 00120 RMP_CMD_SET_USER_FB_1_BITMAP: 4294967295, 00121 RMP_CMD_SET_USER_FB_2_BITMAP: 4294967295, 00122 RMP_CMD_SET_USER_FB_3_BITMAP: 4294967295, 00123 RMP_CMD_SET_USER_FB_4_BITMAP: 0, 00124 RMP_CMD_SET_AUDIO_COMMAND: 16, 00125 RMP_CMD_SET_OPERATIONAL_MODE: 6, 00126 RMP_CMD_SEND_SP_FAULTLOG: 1, 00127 RMP_CMD_RESET_INTEGRATORS: 31, 00128 RMP_CMD_RESET_PARAMS_TO_DEFAULT: 0, 00129 RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS: 1}) 00130 00131 """ 00132 Dictionary to define the minimum values. 00133 """ 00134 config_params_min = dict({ 00135 RMP_CMD_NONE: 0, 00136 RMP_CMD_SET_MAXIMUM_VELOCITY: MIN_VELOCITY_MPS, 00137 RMP_CMD_SET_MAXIMUM_ACCELERATION: MIN_ACCELERATION_MPS2, 00138 RMP_CMD_SET_MAXIMUM_DECELERATION: MIN_DECELERATION_MPS2, 00139 RMP_CMD_SET_MAXIMUM_DTZ_DECEL_RATE: MIN_DTZ_DECEL_RATE_MPS2, 00140 RMP_CMD_SET_COASTDOWN_ACCEL: MIN_COASTDOWN_ACCEL_MPS2, 00141 RMP_CMD_SET_MAXIMUM_TURN_RATE: MIN_YAW_RATE_RPS, 00142 RMP_CMD_SET_MAXIMUM_TURN_ACCEL: MIN_YAW_ACCEL_RPS2, 00143 RMP_CMD_SET_TIRE_DIAMETER: MIN_TIRE_DIAMETER_M, 00144 RMP_CMD_SET_WHEEL_BASE_LENGTH: MIN_WHEEL_BASE_LENGTH_M, 00145 RMP_CMD_SET_WHEEL_TRACK_WIDTH: MIN_WHEEL_TRACK_WIDTH_M, 00146 RMP_CMD_SET_TRANSMISSION_RATIO: MIN_TRANSMISSION_RATIO, 00147 RMP_CMD_SET_INPUT_CONFIG_BITMAP: 0, 00148 RMP_CMD_SET_ETH_IP_ADDRESS: None, 00149 RMP_CMD_SET_ETH_PORT_NUMBER: 0, 00150 RMP_CMD_SET_ETH_SUBNET_MASK: None, 00151 RMP_CMD_SET_ETH_GATEWAY: None, 00152 RMP_CMD_SET_USER_FB_1_BITMAP: 0, 00153 RMP_CMD_SET_USER_FB_2_BITMAP: 0, 00154 RMP_CMD_SET_USER_FB_3_BITMAP: 0, 00155 RMP_CMD_SET_USER_FB_4_BITMAP: 0, 00156 RMP_CMD_SET_AUDIO_COMMAND: 0, 00157 RMP_CMD_SET_OPERATIONAL_MODE: 1, 00158 RMP_CMD_SEND_SP_FAULTLOG: 0, 00159 RMP_CMD_RESET_INTEGRATORS: 0, 00160 RMP_CMD_RESET_PARAMS_TO_DEFAULT: 0, 00161 RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS: 0}) 00162 00163 """ 00164 Dictionary to define the parameter names for error messages. 00165 """ 00166 config_params_names = dict({ 00167 RMP_CMD_NONE: "RMP_CMD_NONE", 00168 RMP_CMD_SET_MAXIMUM_VELOCITY: "RMP_CMD_SET_MAXIMUM_VELOCITY", 00169 RMP_CMD_SET_MAXIMUM_ACCELERATION: "RMP_CMD_SET_MAXIMUM_ACCELERATION", 00170 RMP_CMD_SET_MAXIMUM_DECELERATION: "RMP_CMD_SET_MAXIMUM_DECELERATION", 00171 RMP_CMD_SET_MAXIMUM_DTZ_DECEL_RATE: "RMP_CMD_SET_MAXIMUM_DTZ_DECEL_RATE", 00172 RMP_CMD_SET_COASTDOWN_ACCEL: "RMP_CMD_SET_COASTDOWN_ACCEL", 00173 RMP_CMD_SET_MAXIMUM_TURN_RATE: "RMP_CMD_SET_MAXIMUM_TURN_RATE", 00174 RMP_CMD_SET_MAXIMUM_TURN_ACCEL: "RMP_CMD_SET_MAXIMUM_TURN_ACCEL", 00175 RMP_CMD_SET_TIRE_DIAMETER: "RMP_CMD_SET_TIRE_DIAMETER", 00176 RMP_CMD_SET_WHEEL_BASE_LENGTH: "RMP_CMD_SET_WHEEL_BASE_LENGTH", 00177 RMP_CMD_SET_WHEEL_TRACK_WIDTH: "RMP_CMD_SET_WHEEL_TRACK_WIDTH", 00178 RMP_CMD_SET_TRANSMISSION_RATIO: "RMP_CMD_SET_TRANSMISSION_RATIO", 00179 RMP_CMD_SET_INPUT_CONFIG_BITMAP: "RMP_CMD_SET_INPUT_CONFIG_BITMAP", 00180 RMP_CMD_SET_ETH_IP_ADDRESS: "RMP_CMD_SET_ETH_IP_ADDRESS", 00181 RMP_CMD_SET_ETH_PORT_NUMBER: "RMP_CMD_SET_ETH_PORT_NUMBER", 00182 RMP_CMD_SET_ETH_SUBNET_MASK: "RMP_CMD_SET_ETH_SUBNET_MASK", 00183 RMP_CMD_SET_ETH_GATEWAY: "RMP_CMD_SET_ETH_GATEWAY", 00184 RMP_CMD_SET_USER_FB_1_BITMAP: "RMP_CMD_SET_USER_FB_1_BITMAP", 00185 RMP_CMD_SET_USER_FB_2_BITMAP: "RMP_CMD_SET_USER_FB_2_BITMAP", 00186 RMP_CMD_SET_USER_FB_3_BITMAP: "RMP_CMD_SET_USER_FB_3_BITMAP", 00187 RMP_CMD_SET_USER_FB_4_BITMAP: "RMP_CMD_SET_USER_FB_4_BITMAP", 00188 RMP_CMD_SET_AUDIO_COMMAND: "RMP_CMD_SET_AUDIO_COMMAND", 00189 RMP_CMD_SET_OPERATIONAL_MODE: "RMP_CMD_SET_OPERATIONAL_MODE", 00190 RMP_CMD_SEND_SP_FAULTLOG: "RMP_CMD_SEND_SP_FAULTLOG", 00191 RMP_CMD_RESET_INTEGRATORS: "RMP_CMD_RESET_INTEGRATORS", 00192 RMP_CMD_RESET_PARAMS_TO_DEFAULT: "RMP_CMD_RESET_PARAMS_TO_DEFAULT", 00193 RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS: "RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS"}) 00194 00195 """ 00196 Dictionary to define the checking function, defined above, to be called for each valid parameter. 00197 """ 00198 config_params_function = dict({ 00199 RMP_CMD_NONE: is_true, 00200 RMP_CMD_SET_MAXIMUM_VELOCITY: is_range_set, 00201 RMP_CMD_SET_MAXIMUM_ACCELERATION: is_range_set, 00202 RMP_CMD_SET_MAXIMUM_DECELERATION: is_range_set, 00203 RMP_CMD_SET_MAXIMUM_DTZ_DECEL_RATE: is_range_set, 00204 RMP_CMD_SET_COASTDOWN_ACCEL: is_range_set, 00205 RMP_CMD_SET_MAXIMUM_TURN_RATE: is_range_set, 00206 RMP_CMD_SET_MAXIMUM_TURN_ACCEL: is_range_set, 00207 RMP_CMD_SET_TIRE_DIAMETER: is_range_set, 00208 RMP_CMD_SET_WHEEL_BASE_LENGTH: is_range_set, 00209 RMP_CMD_SET_WHEEL_TRACK_WIDTH: is_range_set, 00210 RMP_CMD_SET_TRANSMISSION_RATIO: is_range_set, 00211 RMP_CMD_SET_INPUT_CONFIG_BITMAP: is_range, 00212 RMP_CMD_SET_ETH_IP_ADDRESS: is_quad, 00213 RMP_CMD_SET_ETH_PORT_NUMBER: is_range, 00214 RMP_CMD_SET_ETH_SUBNET_MASK: is_quad, 00215 RMP_CMD_SET_ETH_GATEWAY: is_quad, 00216 RMP_CMD_SET_USER_FB_1_BITMAP: is_range, 00217 RMP_CMD_SET_USER_FB_2_BITMAP: is_range, 00218 RMP_CMD_SET_USER_FB_3_BITMAP: is_range, 00219 RMP_CMD_SET_USER_FB_4_BITMAP: is_range, 00220 RMP_CMD_SET_AUDIO_COMMAND: is_range_no_set, 00221 RMP_CMD_SET_OPERATIONAL_MODE: is_range_no_set, 00222 RMP_CMD_SEND_SP_FAULTLOG: is_range_no_set, 00223 RMP_CMD_RESET_INTEGRATORS: is_range_no_set, 00224 RMP_CMD_RESET_PARAMS_TO_DEFAULT: is_true, 00225 RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS: is_range_no_set})