00001 """-------------------------------------------------------------------- 00002 COPYRIGHT 2013 SEGWAY Inc. 00003 00004 Software License Agreement: 00005 00006 The software supplied herewith by Segway Inc. (the "Company") for its 00007 RMP Robotic Platforms is intended and supplied to you, the Company's 00008 customer, for use solely and exclusively with Segway products. The 00009 software is owned by the Company and/or its supplier, and is protected 00010 under applicable copyright laws. All rights are reserved. Any use in 00011 violation of the foregoing restrictions may subject the user to criminal 00012 sanctions under applicable laws, as well as to civil liability for the 00013 breach of the terms and conditions of this license. The Company may 00014 immediately terminate this Agreement upon your use of the software with 00015 any products that are not Segway products. 00016 00017 The software was written using Python programming language. Your use 00018 of the software is therefore subject to the terms and conditions of the 00019 OSI- approved open source license viewable at http://www.python.org/. 00020 You are solely responsible for ensuring your compliance with the Python 00021 open source license. 00022 00023 You shall indemnify, defend and hold the Company harmless from any claims, 00024 demands, liabilities or expenses, including reasonable attorneys fees, incurred 00025 by the Company as a result of any claim or proceeding against the Company 00026 arising out of or based upon: 00027 00028 (i) The combination, operation or use of the software by you with any hardware, 00029 products, programs or data not supplied or approved in writing by the Company, 00030 if such claim or proceeding would have been avoided but for such combination, 00031 operation or use. 00032 00033 (ii) The modification of the software by or on behalf of you 00034 00035 (iii) Your use of the software. 00036 00037 THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES, 00038 WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED 00039 TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 00040 PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT, 00041 IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR 00042 CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER. 00043 00044 \file rmp_interface.py 00045 00046 \brief This module contains the interface to the RMP 00047 00048 \Platform: Cross Platform 00049 --------------------------------------------------------------------""" 00050 from crc16 import * 00051 from utils import * 00052 from system_defines import * 00053 from rmp_config_params import * 00054 from user_event_handlers import * 00055 from io_eth_cmd import IO_ETHERNET 00056 import time,os,sys 00057 00058 00059 """ 00060 Define the variable for fixed point Q15 format for 00061 converting certain parameters for OMNI 00062 """ 00063 Q15 = 32767 00064 00065 """ 00066 Main class for the RMP interface 00067 """ 00068 class RMP: 00069 def __init__(self,rmp_addr,rsp_queue,cmd_queue,in_flags,out_flags,update_rate=MIN_UPDATE_PERIOD_SEC,log_data=False): 00070 """ 00071 generate the CRC table 00072 """ 00073 generate_crc_table() 00074 00075 """ 00076 Create a dictionary for the feedback 00077 """ 00078 self.user_defined_feedback = dict() 00079 00080 """ 00081 Indicates a logfile has been created 00082 """ 00083 self.logfile_started = False 00084 00085 """ 00086 Initialize the bitmaps and create a dictionary for holding the 00087 user defined feedback data 00088 """ 00089 self.bitmap = [0]*4 00090 self.bitmap[0] = CONFIG_PARAMS[RMP_CMD_SET_USER_FB_1_BITMAP-1][2] 00091 self.bitmap[1] = CONFIG_PARAMS[RMP_CMD_SET_USER_FB_2_BITMAP-1][2] 00092 self.bitmap[2] = CONFIG_PARAMS[RMP_CMD_SET_USER_FB_3_BITMAP-1][2] 00093 self.bitmap[3] = CONFIG_PARAMS[RMP_CMD_SET_USER_FB_4_BITMAP-1][2] 00094 00095 """ 00096 Create arrays of bitmaps defining the type of feedback contained 00097 in each UDFB 00098 """ 00099 self.fp_mask = [RMP_FLOATING_POINT_FEEDBACK_1_MASK, 00100 RMP_FLOATING_POINT_FEEDBACK_2_MASK, 00101 RMP_FLOATING_POINT_FEEDBACK_3_MASK, 00102 RMP_FLOATING_POINT_FEEDBACK_4_MASK] 00103 00104 self.hex_mask = [RMP_HEX_FEEDBACK_1_MASK, 00105 RMP_HEX_FEEDBACK_2_MASK, 00106 RMP_HEX_FEEDBACK_3_MASK, 00107 RMP_HEX_FEEDBACK_4_MASK] 00108 00109 self.ip_mask = [RMP_IP_FEEDBACK_1_MASK, 00110 RMP_IP_FEEDBACK_2_MASK, 00111 RMP_IP_FEEDBACK_3_MASK, 00112 RMP_IP_FEEDBACK_4_MASK] 00113 00114 """ 00115 Create a header dictionary array 00116 """ 00117 self.hdr_dicts = [feedback_1_bitmap_menu_items, 00118 feedback_2_bitmap_menu_items, 00119 feedback_3_bitmap_menu_items, 00120 feedback_4_bitmap_menu_items] 00121 00122 """ 00123 Update the local feedback dictionary 00124 """ 00125 self.update_feedback_dict(None,True) 00126 00127 """ 00128 Get the RMP address 00129 """ 00130 self.comm = IO_ETHERNET(rmp_addr) 00131 00132 self.in_flags = in_flags 00133 self.out_flags = out_flags 00134 00135 if (update_rate >= MIN_UPDATE_PERIOD_SEC): 00136 self.delay = update_rate 00137 else: 00138 print "Bad Update Period needs to be longer than 0.01s....." 00139 print "exiting......" 00140 self.out_flags.put(RMP_INIT_FAILED) 00141 self.Close() 00142 00143 if (False == self.comm.success): 00144 print "Could not connect to RMP UDP socket....." 00145 print "exiting......" 00146 self.out_flags.put(RMP_INIT_FAILED) 00147 self.Close() 00148 00149 if (False == self.set_and_verify_config_params(CONFIG_PARAMS)): 00150 print "Could not configure RMP......" 00151 print "exiting......" 00152 self.out_flags.put(RMP_INIT_FAILED) 00153 self.Close() 00154 00155 """ 00156 Get the queues and the time stamps now that initialization was successful 00157 """ 00158 self.cmd_queue = cmd_queue 00159 self.rsp_queue = rsp_queue 00160 self.last_update_time = time.time() 00161 self.log_file_start_time = time.time() 00162 00163 """ 00164 If the caller wants to log data check for the log directory, create the file and write 00165 the header. 00166 """ 00167 self.log_data = log_data 00168 if self.log_data: 00169 dirpath = os.getcwd()+"/RMP_DATA_LOGS" 00170 if (False == os.path.exists(dirpath)): 00171 os.mkdir(dirpath) 00172 filename = dirpath + "/" + "RMP_DATA_LOG_" + time.strftime("%m%d%Y_%H%M%S") + ".csv" 00173 self.logfile = open(filename,'w') 00174 self.logfile.write(','.join(self.log_file_header)) 00175 self.logfile_started = True 00176 00177 """ 00178 Run the thread 00179 """ 00180 self.run() 00181 00182 def run(self): 00183 while True: 00184 00185 """ 00186 Check the flags coming in. Presently there is only a kill. 00187 """ 00188 while not self.in_flags.empty(): 00189 if (RMP_KILL == self.in_flags.get()): 00190 print "RMP thread has been killed by application......" 00191 print "exiting........." 00192 self.out_flags.put(RMP_IS_DEAD) 00193 self.Close() 00194 sys.exit() 00195 00196 """ 00197 If enough time has passed since the last transmit, check the command 00198 queue and send the next command. Then set the TX_RDY flag so the parent 00199 knows that the queue can be repopulated 00200 """ 00201 if ((time.time() - self.last_update_time) > self.delay): 00202 if not self.cmd_queue.empty(): 00203 self.update_rmp_commands(self.cmd_queue.get()) 00204 00205 self.out_flags.put(RMP_TX_RDY) 00206 self.last_update_time = time.time() 00207 00208 """ 00209 Check for data each time and put it in the queue is it exists. 00210 """ 00211 data = self.comm.Receive(self.expected_items) 00212 if (self.update_feedback_dict(data,False)): 00213 self.rsp_queue.put(self.user_defined_feedback) 00214 self.out_flags.put(RMP_RSP_DATA_RDY) 00215 00216 def set_and_verify_config_params(self,config): 00217 """ 00218 The commands that force the feedback array to just contain the configurable elements 00219 but not update the UDFB to do so. This allows the user to verify the configuration 00220 while it is changing. 00221 """ 00222 force_nvm_feedback = [RMP_CFG_CMD_ID,RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS,1] 00223 set_user_feedback = [RMP_CFG_CMD_ID,RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS,0] 00224 00225 """ 00226 Start by sending the force config feedback command and check for the 00227 appropriate response 00228 """ 00229 attempts = 0 00230 success = False 00231 while ((False == success) and (attempts<10)): 00232 self.update_rmp_commands(force_nvm_feedback) 00233 time.sleep(0.05) 00234 loaded_params = self.comm.Receive(FORCED_CONFIG_FEEDBACK_ITEMS) 00235 if (None != loaded_params): 00236 success = True 00237 else: 00238 attempts += 1 00239 00240 """ 00241 If the command to force the config feedback was successful check to see if there 00242 are non-matching configuration parameters 00243 """ 00244 if (False == success): 00245 print "Could not set RMP_CMD_FORCE_CONFIG_FEEDBACK_BITMAPS....." 00246 print "The platform did not respond, ensure it is operational and the IP address is correct...." 00247 return False 00248 else: 00249 non_matching_params = [] 00250 for i in range(0,NUMBER_OF_NVM_CONFIG_PARAMS): 00251 if (loaded_params[i] != config[i][2]): 00252 non_matching_params.append(i) 00253 00254 """ 00255 Load each configuration parameter which does not match the configuration file 00256 """ 00257 attempts = 0 00258 for i in range(0,len(non_matching_params)): 00259 idx = non_matching_params[i] 00260 success = False 00261 while ((False == success) and (attempts<10)): 00262 attempts+=1 00263 self.update_rmp_commands(config[idx]) 00264 time.sleep(0.05) 00265 loaded_params = self.comm.Receive(FORCED_CONFIG_FEEDBACK_ITEMS) 00266 if (None != loaded_params): 00267 if (loaded_params[idx] == config[idx][2]): 00268 success = True 00269 attempts = 0 00270 if (False == success): 00271 break 00272 00273 """ 00274 Notify the user if we could not set the parameter 00275 """ 00276 if (False == success): 00277 print "Could not set param %(1)s....." %{"1":config_param_dict[idx+1]} 00278 print "The parameter is likely not valid, check it in rmp_config_params.py" 00279 return False 00280 00281 """ 00282 Switch back to the user feedback array and make sure we get an appropriate response 00283 """ 00284 attempts = 0 00285 success = False 00286 while ((False == success) and (attempts<10)): 00287 self.update_rmp_commands(set_user_feedback) 00288 time.sleep(0.05) 00289 data = self.comm.Receive(self.expected_items) 00290 if (None != data): 00291 success = True 00292 else: 00293 attempts += 1 00294 00295 """ 00296 Could not reset the feedback 00297 """ 00298 if (False == success): 00299 print "Could not set user defined feedback" %{"1":idx+1} 00300 print "The platform did not respond, " 00301 return False 00302 00303 return True 00304 00305 def update_rmp_commands(self,input_cmd): 00306 00307 """ 00308 Populate the message to the RMP platform if it is not a 00309 valid format return False 00310 """ 00311 try: 00312 cmds = [0]*3 00313 cmds[0] = input_cmd[0] 00314 send_cmd = True 00315 00316 if (cmds[0] == RMP_OMNI_MOTION_CMD_ID): 00317 vel_cmd = int((input_cmd[1] * Q15)) 00318 yaw_cmd = int((input_cmd[2] * Q15)) 00319 cmds[1]= (((vel_cmd << 16) & 0xFFFF0000) | (yaw_cmd & 0x0000FFFF)) 00320 cmds[2] = int(convert_float_to_u32(input_cmd[3])) 00321 elif (cmds[0] == RMP_MOTION_CMD_ID): 00322 cmds[1] = int(convert_float_to_u32(input_cmd[1])) 00323 cmds[2] = int(convert_float_to_u32(input_cmd[2])) 00324 elif (cmds[0] == RMP_CFG_CMD_ID): 00325 cmds[1] = int(input_cmd[1]) 00326 cmds[2] = int(input_cmd[2]) 00327 else: 00328 send_cmd = False 00329 except: 00330 send_cmd = False 00331 00332 """ 00333 If we have a valid command send it 00334 """ 00335 if (True == send_cmd): 00336 00337 output = self.Convert_RMP_Cmds_for_Serial_Interface(cmds) 00338 self.comm.Send(output) 00339 00340 return send_cmd 00341 00342 def update_feedback_dict(self,data=None,init=False): 00343 ret = True 00344 00345 """ 00346 Determine the way the variables should be converted to human readable form. There are 00347 four ways, floating point, hex, IP strings, and the default is integer 00348 """ 00349 item = 0 00350 if init: 00351 self.log_file_header = ["Time_stamp"] 00352 for x in range(0,4): 00353 for i in range(0,32): 00354 if (self.bitmap[x] & (1<<i)): 00355 if (self.fp_mask[x] & (1<<i)): 00356 self.log_file_header.append(self.hdr_dicts[x][(1<<i)]) 00357 self.user_defined_feedback[self.hdr_dicts[x][(1<<i)]] = 0.0 00358 elif (self.hex_mask[x] & (1 << i)): 00359 self.log_file_header.append(self.hdr_dicts[x][(1<<i)]) 00360 self.user_defined_feedback[self.hdr_dicts[x][(1<<i)]] = hex(0) 00361 elif (self.ip_mask[x] & (1 << i)): 00362 self.log_file_header.append(self.hdr_dicts[x][(1<<i)]) 00363 self.user_defined_feedback[self.hdr_dicts[x][(1<<i)]] = numToDottedQuad(0) 00364 else: 00365 self.log_file_header.append(self.hdr_dicts[x][(1<<i)]) 00366 self.user_defined_feedback[self.hdr_dicts[x][(1<<i)]] = 0 00367 00368 item +=1 00369 """ 00370 Add one for the CRC 00371 """ 00372 self.expected_items = item+1 00373 00374 """ 00375 Append a line end to the logfile header 00376 """ 00377 self.log_file_header.append("\n") 00378 00379 elif (data != None): 00380 """ 00381 If we have data convert it and add it to the dictionary 00382 """ 00383 temp = [] 00384 temp.append(str(time.time() - self.log_file_start_time)) 00385 self.user_defined_feedback.clear(); 00386 for x in range(0,4): 00387 for i in range(0,32): 00388 if (self.bitmap[x] & (1<<i)): 00389 if (self.fp_mask[x] & (1<<i)): 00390 temp.append(str(round(convert_u32_to_float(data[item]),3))) 00391 self.user_defined_feedback[self.hdr_dicts[x][(1<<i)]] = round(convert_u32_to_float(data[item]),3); 00392 elif (self.hex_mask[x] & (1 << i)): 00393 temp.append("0x%(1)08X" %{"1":data[item]}) 00394 self.user_defined_feedback[self.hdr_dicts[x][(1<<i)]] = hex(data[item]); 00395 elif (self.ip_mask[x] & (1 << i)): 00396 temp.append(str(numToDottedQuad(data[item]))) 00397 self.user_defined_feedback[self.hdr_dicts[x][(1<<i)]] = numToDottedQuad(data[item]); 00398 else: 00399 temp.append(str(int(data[item]))) 00400 self.user_defined_feedback[self.hdr_dicts[x][(1<<i)]] = int(data[item]); 00401 00402 item += 1 00403 00404 """ 00405 Log the data if called for 00406 """ 00407 if(True == self.logfile_started): 00408 temp.append("\n") 00409 self.logfile.write(",".join(temp)) 00410 else: 00411 ret = False 00412 00413 return ret 00414 00415 def Close(self): 00416 if (True == self.logfile_started): 00417 self.logfile.close() 00418 self.comm.Close() 00419 sys.exit() 00420 00421 def Convert_RMP_Cmds_for_Serial_Interface(self,cmds): 00422 """ 00423 Convert a set of commands for the UDP Ethernet interface 00424 """ 00425 rmp_cmd = [0]*NUM_USB_ETH_BYTES; 00426 00427 rmp_cmd[RMP_USB_ETH_CAN_ID_HIGH_INDEX] = int((cmds[0] & 0xFF00) >> 8) 00428 rmp_cmd[RMP_USB_ETH_CAN_ID_LOW_INDEX] = int((cmds[0] & 0x00FF)) 00429 rmp_cmd[RMP_USB_ETH_CAN_DATA_0_INDEX] = int((cmds[1] & 0xFF000000) >> 24) 00430 rmp_cmd[RMP_USB_ETH_CAN_DATA_1_INDEX] = int((cmds[1] & 0x00FF0000) >> 16) 00431 rmp_cmd[RMP_USB_ETH_CAN_DATA_2_INDEX] = int((cmds[1] & 0x0000FF00) >> 8) 00432 rmp_cmd[RMP_USB_ETH_CAN_DATA_3_INDEX] = int((cmds[1] & 0x000000FF)) 00433 rmp_cmd[RMP_USB_ETH_CAN_DATA_4_INDEX] = int((cmds[2] & 0xFF000000) >> 24) 00434 rmp_cmd[RMP_USB_ETH_CAN_DATA_5_INDEX] = int((cmds[2] & 0x00FF0000) >> 16) 00435 rmp_cmd[RMP_USB_ETH_CAN_DATA_6_INDEX] = int((cmds[2] & 0x0000FF00) >> 8) 00436 rmp_cmd[RMP_USB_ETH_CAN_DATA_7_INDEX] = int((cmds[2] & 0x000000FF)) 00437 00438 """ 00439 Compute the CRC for the command 00440 """ 00441 compute_buffer_crc(rmp_cmd,NUM_USB_ETH_BYTES) 00442 00443 """ 00444 Convert the string to char data and return it 00445 """ 00446 rmp_cmd_chars = [] 00447 for x in range(0,len(rmp_cmd)): 00448 rmp_cmd_chars.append(chr(rmp_cmd[x])) 00449 00450 output = ''.join(rmp_cmd_chars) 00451 00452 return output 00453 00454 00455