00001
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)
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
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()