ethernet_rmp.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 --------------------------------------------------------------------
00005 COPYRIGHT 2013 SEGWAY Inc.
00006 
00007 Software License Agreement:
00008 
00009 The software supplied herewith by Segway Inc. (the "Company") for its
00010 RMP Robotic Platforms is intended and supplied to you, the Company's
00011 customer, for use solely and exclusively with Segway products. The
00012 software is owned by the Company and/or its supplier, and is protected
00013 under applicable copyright laws.  All rights are reserved. Any use in
00014 violation of the foregoing restrictions may subject the user to criminal
00015 sanctions under applicable laws, as well as to civil liability for the
00016 breach of the terms and conditions of this license. The Company may
00017 immediately terminate this Agreement upon your use of the software with
00018 any products that are not Segway products.
00019 
00020 The software was written using Python programming language.  Your use
00021 of the software is therefore subject to the terms and conditions of the
00022 OSI- approved open source license viewable at http://www.python.org/.
00023 You are solely responsible for ensuring your compliance with the Python
00024 open source license.
00025 
00026 You shall indemnify, defend and hold the Company harmless from any claims,
00027 demands, liabilities or expenses, including reasonable attorneys fees, incurred
00028 by the Company as a result of any claim or proceeding against the Company
00029 arising out of or based upon:
00030 
00031 (i) The combination, operation or use of the software by you with any hardware,
00032         products, programs or data not supplied or approved in writing by the Company,
00033         if such claim or proceeding would have been avoided but for such combination,
00034         operation or use.
00035 
00036 (ii) The modification of the software by or on behalf of you
00037 
00038 (iii) Your use of the software.
00039 
00040 THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES,
00041 WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED
00042 TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00043 PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT,
00044 IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR
00045 CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
00046 --------------------------------------------------------------------
00047 """
00048 
00049 """
00050 ROS wrapper for the python ethernet RMP driver. This is an adaptation of example code provided by Segway Inc.
00051 
00052 Author:  Segway Inc.
00053 Author:  Chris Dunkers, Worcester Polytechnic Institute
00054 Author:  Russell Toris, Worcester Polytechnic Institute
00055 Version: June 10, 2014
00056 """
00057 
00058 from rmp_msgs.msg import RMPCommand, RMPFeedback
00059 from python_ethernet_rmp.rmp_interface import RMP
00060 from python_ethernet_rmp.system_defines import *
00061 from python_ethernet_rmp.user_event_handlers import RMPEventHandlers
00062 from python_ethernet_rmp.rmp_config_params import *
00063 from python_ethernet_rmp.utils import *
00064 from geometry_msgs.msg import Twist
00065 from rmp_config_limits import *
00066 import sys,time,threading,Queue
00067 import math
00068 import rospy
00069 
00070 """
00071 The platform address may be different than the one in your config
00072 (rmp_config_params.py). This would be the case if you wanted to update 
00073 ethernet configuration. If the ethernet configuration is updated the
00074 system needs to be power cycled for it to take effect and this should
00075 be changed to match the new values you defined in your config
00076 """
00077 rmp_addr = ("192.168.0.40",8080) #this is the default value and matches the config
00078 
00079 """
00080 Define the main function for the example. It creates a thread to run RMP and handles
00081 passing the events to the user defined handlers in user_event_handlers.py
00082 """
00083 
00084 class RMPExchange:
00085         def __init__(self):
00086                 """
00087                 Initialze the thread 
00088                 """
00089                                 
00090                 """
00091                 Read in the Ros Params and add them to a array to set the config params
00092                 """
00093                 global rmp_addr
00094                 update_delay_sec = rospy.get_param('~update_delay_sec', 0.05)
00095                 log_data = rospy.get_param('~log_data', False)
00096                 ip_addr = rospy.get_param('~current_rmp_ip_addr', DEFAULT_IP_ADDRESS)
00097                 port_num = rospy.get_param('~current_rmp_port_num', DEFAULT_PORT_NUMBER)
00098                 self.isOmni = rospy.get_param('~is_omni ',False)
00099                 try:
00100                         dottedQuadToNum(ip_addr)
00101                         if port_num > 0:
00102                                 rmp_addr = (ip_addr,port_num)
00103                         else:
00104                                 rospy.logwarn("current_rmp_port_num is not a valid port number")        
00105                 except:
00106                         rospy.logwarn("current_rmp_ip_addr in not in dotted quad format")
00107 
00108                 if update_delay_sec < 0.01:
00109                         rospy.logwarn("Update delay time is too fast -- setting to 0.01 seconds.")
00110                         update_delay_sec = 0.01
00111 
00112                 self.rmpParams = []
00113                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_MAXIMUM_VELOCITY,rospy.get_param('~my_velocity_limit_mps',DEFAULT_MAXIMUM_VELOCITY_MPS)])
00114                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_MAXIMUM_ACCELERATION,rospy.get_param('~my_accel_limit_mps2',DEFAULT_MAXIMUM_ACCELERATION_MPS2)])
00115                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_MAXIMUM_DECELERATION,rospy.get_param('~my_decel_limit_mps2',DEFAULT_MAXIMUM_DECELERATION_MPS2)])
00116                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_MAXIMUM_DTZ_DECEL_RATE,rospy.get_param('~my_dtz_rate_mps2',DEFAULT_MAXIMUM_DTZ_DECEL_RATE_MPS2)])
00117                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_COASTDOWN_ACCEL,rospy.get_param('~my_coastdown_accel_mps2',DEFAULT_COASTDOWN_ACCEL_MPS2)])
00118                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_MAXIMUM_TURN_RATE,rospy.get_param('~my_yaw_rate_limit_rps',DEFAULT_MAXIMUM_YAW_RATE_RPS)])
00119                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_MAXIMUM_TURN_ACCEL,rospy.get_param('~my_yaw_accel_limit_rps2',DEFAULT_MAX_YAW_ACCEL_RPS2)])
00120                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_TIRE_DIAMETER,rospy.get_param('~my_tire_diameter_m',DEFAULT_TIRE_DIAMETER_M)])
00121                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_WHEEL_BASE_LENGTH,rospy.get_param('~my_wheel_base_length_m',DEFAULT_WHEEL_BASE_LENGTH_M)])
00122                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_WHEEL_TRACK_WIDTH,rospy.get_param('~my_wheel_track_width_m',DEFAULT_WHEEL_TRACK_WIDTH_M)])
00123                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_TRANSMISSION_RATIO,rospy.get_param('~my_gear_ratio',DEFAULT_TRANSMISSION_RATIO)])
00124                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_INPUT_CONFIG_BITMAP,rospy.get_param('~my_config_bitmap',DEFAULT_CONFIG_BITMAP)])
00125                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_ETH_IP_ADDRESS,rospy.get_param('~my_ip_address',DEFAULT_IP_ADDRESS)])
00126                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_ETH_PORT_NUMBER,rospy.get_param('~my_port_num',DEFAULT_PORT_NUMBER)])
00127                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_ETH_SUBNET_MASK,rospy.get_param('~my_subnet_mask',DEFAULT_SUBNET_MASK)])
00128                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_ETH_GATEWAY,rospy.get_param('~my_gateway',DEFAULT_GATEWAY)])
00129                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_USER_FB_1_BITMAP,rospy.get_param('~my_user_defined_feedback_bitmap_1',DEFAULT_USER_FB_1_BITMAP)])
00130                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_USER_FB_2_BITMAP,rospy.get_param('~my_user_defined_feedback_bitmap_2',DEFAULT_USER_FB_2_BITMAP)])
00131                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_USER_FB_3_BITMAP,rospy.get_param('~my_user_defined_feedback_bitmap_3',DEFAULT_USER_FB_3_BITMAP)])
00132                 self.rmpParams.append([RMP_CFG_CMD_ID,RMP_CMD_SET_USER_FB_4_BITMAP,rospy.get_param('~my_user_defined_feedback_bitmap_4',DEFAULT_USER_FB_4_BITMAP)])
00133                                 
00134                 """
00135                 Create and response and command queue. The responses will be in the form of 
00136                 a dictionary containing the vaiable name as the key and a converted value
00137                 the names are defined in the feedback_X_bitmap_menu_items dictionaries if a particular
00138                 variable is of interest
00139                 """
00140                 self.rsp_queue = Queue.Queue()
00141                 self.cmd_queue = Queue.Queue()
00142                 self.in_flags  = Queue.Queue()
00143                 self.out_flags = Queue.Queue()
00144                 
00145                 """
00146                 Create the thread to run RMP 
00147                 """
00148                 self.my_thread = threading.Thread(target=RMP, args=(rmp_addr,self.rsp_queue,self.cmd_queue,self.in_flags,self.out_flags,update_delay_sec, log_data))
00149                 self.my_thread.daemon = True
00150                 self.my_thread.start()
00151                 
00152                 """
00153                 Initialize my event handler class
00154                 """
00155                 self.EventHandler = RMPEventHandlers(self.cmd_queue,self.rsp_queue,self.in_flags,self)
00156                 
00157                 """
00158                 Initialize the feedback publisher
00159                 """
00160                 self.rmpFeedback = RMPFeedback()
00161                 self.feedbackPub = rospy.Publisher('rmp_feedback', RMPFeedback, queue_size = 'None')
00162 
00163                 rospy.Subscriber("rmp_command", RMPCommand, self.sendRMPCommand)
00164                 rospy.Subscriber("cmd_vel", Twist, self.sendMotionCommand)
00165 
00166                 """
00167                 Add a Feedback listener to the segway driver
00168                 """
00169                 self.EventHandler.AddListener(self.publishFeedback)
00170 
00171                 """
00172                 Initialize the parameters to the Segway
00173                 """
00174                 self.initRMPParams()
00175 
00176                 """
00177                 Generate a goto tractor event
00178                 """
00179                 self.EventHandler.GotoTractor()
00180                 
00181         def __del__(self):
00182                 """
00183                 send the signal to kill the thread
00184                 """
00185                 self.in_flags.put(RMP_KILL)
00186                 
00187                 """
00188                 Wait for the thread to die
00189                 """
00190                 while self.my_thread.isAlive():
00191                         pass
00192                 
00193                 """
00194                 Exit main
00195                 """
00196                 #sys.exit()
00197                 print 'exited rmp_exchange'
00198                                 
00199         def sendMotionCommand(self,command):
00200                 if not self.isOmni:
00201                         self.EventHandler.AddCommand([RMP_MOTION_CMD_ID,command.linear.x,-command.angular.z])
00202                 else:
00203                         forw_vel = math.sqrt(command.linear.x**2 + command.linear.y**2)/math.sqrt(2)
00204                         angle = math.atan2(command.linear.y, command.linear.x) + math.pi
00205                         self.EventHandler.AddCommand([RMP_OMNI_MOTION_CMD_ID,forw_vel,-command.angular.z, angle])
00206         
00207         def sendRMPCommand(self,command):
00208                 if command.cmd_id == 1280 or command.cmd_id == 1281: 
00209                         cmd = [command.cmd_id, command.arg1, command.arg2] 
00210                         if self.isValidCommand(cmd):
00211                                 self.EventHandler.AddCommand(cmd)
00212                 elif command.cmd_id == 1536:
00213                         cmd = [command.cmd_id, command.arg1, command.arg2, command.arg3] 
00214                         if self.isValidCommand(cmd):
00215                                 self.EventHandler.AddCommand(cmd)
00216                 
00217         def publishFeedback(self,fb_dict):
00218                 snrValues = []
00219                 snrItems = []
00220                 fltValues = []
00221                 fltItems = []
00222                 ipValues = []
00223                 ipItems = []
00224                 
00225                 """
00226                 get all of the values from the Feedback provided by the RMP
00227                 """
00228                 for key, value in fb_dict.items():
00229                         try:
00230                                 value = float(value)
00231                                 snrValues.append(value)
00232                                 snrItems.append(key)
00233                         except:
00234                                 try:
00235                                         value = int(value,16)
00236                                         fltValues.append(value)
00237                                         fltItems.append(key)
00238                                 except:
00239                                         ipValues.append(value)
00240                                         ipItems.append(key)
00241                                         
00242                 self.rmpFeedback.header.stamp = rospy.Time.now()
00243                 self.rmpFeedback.header.frame_id = 'base_link'
00244                 self.rmpFeedback.sensor_items = snrItems
00245                 self.rmpFeedback.sensor_values = snrValues
00246                 self.rmpFeedback.fault_status_items = fltItems
00247                 self.rmpFeedback.fault_status_values = fltValues
00248                 self.rmpFeedback.ip_info = ipItems
00249                 self.rmpFeedback.ip_values = ipValues
00250                 self.feedbackPub.publish(self.rmpFeedback)
00251                         
00252         def isValidCommand(self, command):
00253                 if command[0] == 1280:
00254                         return [True,command]
00255                 elif command[0] == 1536:
00256                         if command[3] > 360 or command[3] < 0:
00257                                 rospy.logwarn("arg3 is out of range. Must be between 0-360")
00258                                 return [False,command]
00259                         else:
00260                                 return [True,command]
00261                 elif command[0] == 1281:
00262                         param_func = config_params_function.setdefault(command[1],is_not_param)
00263                         checks = param_func(command[1],command[2])
00264                         if checks[1] == True:
00265                                 command[2] = checks[0]
00266                                 return [True,command]
00267                         else:
00268                                 return [False,command]
00269                 else:
00270                         rospy.logwarn("cmd_id is invalid")
00271                         return [False,command]
00272         
00273         def initRMPParams(self):
00274                 for command in self.rmpParams:
00275                         check = self.isValidCommand(command)
00276                         if check[0] == True:
00277                                 self.EventHandler.AddCommand(check[1])
00278         
00279         def rmp_send_recv(self):
00280                 """
00281                 Main loop to continually empty yhe out_flags queue
00282                 """
00283                 while not rospy.is_shutdown() and self.EventHandler._continue:
00284                         while not self.out_flags.empty() and self.EventHandler._continue:
00285                                 self.EventHandler.handle_event[self.out_flags.get()]()
00286                         
00287 if __name__ == "__main__":
00288         rospy.init_node('ros_ethernet_rmp')
00289         rmp_command = RMPExchange()
00290         rospy.loginfo("ROS Ethernet RMP Node Started")
00291         rmp_command.rmp_send_recv()     


ros_ethernet_rmp
Author(s): SEGWAY Inc., Chris Dunkers , David Kent
autogenerated on Thu Jun 6 2019 21:43:14