rmp_interface.py
Go to the documentation of this file.
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     


python_ethernet_rmp
Author(s): SEGWAY Inc.
autogenerated on Sat Jun 8 2019 19:17:25