def main(): if not ServerSolution.resolveRosmaster(): return devices_conf = ServerSolution.resolveParameters('radar_packet/devices',os.path.dirname(__file__)+'/radar_read_param.py') if devices_conf is None: return options_conf = ServerSolution.resolveParameters('radar_packet/options',os.path.dirname(__file__)+'/radar_read_param.py') if options_conf is None: return if ServerSolution.checkNodeStarted('radar_gui'): return rospy.init_node('radar_gui', anonymous=False) app = QtGui.QApplication(sys.argv) radar_gui_list = {} for device in devices_conf: _radar_type = 'esr' if re.search('esr',device['name_id']) else None if _radar_type: print device['interface'], device['name_id'], ' interface: ', _radar_type createRadarGui(radar_gui_list, device['name_id'], device['interface'], _radar_type, options_conf['name_resolution']) else: print device['interface'], device['name_id'], ' interface: WARNING not detected!' ex = RadarControlGui(app) # ex.setDisabled(True) # ex2 = RadarStatusGui() sys.exit(app.exec_()) return app
def startRosNode(node_name): if not ServerSolution.resolveRosmaster(): return # devices_conf = ServerSolution.resolveParameters('radar_packet/devices','rosrun foobar radar_read_param.py') devices_conf = ServerSolution.resolveParameters('radar_packet/devices',os.path.dirname(__file__)+'/radar_read_param.py') if devices_conf is None: return # options_conf = ServerSolution.resolveParameters('radar_packet/options','rosrun foobar radar_read_param.py') options_conf = ServerSolution.resolveParameters('radar_packet/options',os.path.dirname(__file__)+'/radar_read_param.py') if options_conf is None: return esr_vehicle_conf = ServerSolution.resolveParameters('radar_packet/esr_vehicle',os.path.dirname(__file__)+'/radar_read_param.py') if esr_vehicle_conf is None: return print esr_vehicle_conf if ServerSolution.checkNodeStarted(node_name): return rospy.init_node(node_name, anonymous=False) print 'start to read radar packets on these devices...' # print options_conf # return _bad_thing_happened = False try: radar_list = {} for device in devices_conf: _radar_type = 'esr' if re.search('esr',device['name_id']) else None if _radar_type: # radar_list[device['name_id']] = {} # _pub_can_send = rospy.Publisher ('radar_packet/'+device['interface']+'/send' , CanBusMsg, queue_size=10) # _pub_result = rospy.Publisher ('radar_packet/'+device['interface']+'/processed', String, queue_size=10) # radar_list[device['name_id']]['pub_can_send'] = _pub_can_send # radar_list[device['name_id']]['pub_result'] = _pub_result print device['interface'], device['name_id'], ' interface: ', _radar_type createRadarHandler(radar_list, device['name_id'], device['interface'], _radar_type, options_conf['name_resolution'], esr_vehicle_conf) # rospy.Subscriber('radar_packet/'+device['interface']+'/recv', CanBusMsg, processRadarEsr) else: print device['interface'], device['name_id'], ' interface: WARNING not detected!' except Exception, e: _bad_thing_happened = True _the_bad_thing = e
self.pub_marker_array.publish(self.marker_array) else: self.filtered_first_track = True self.filterTracking() # self.createCubeMarkerDel() # self.pub_marker.publish(self.marker) self.createCubeMarker() self.createPointsInMarker(msg) self.pub_marker.publish(self.marker) self.createMarkerArray() self.createTextsInMarkerArray(msg) self.pub_marker_array.publish(self.marker_array) self.data_counter = self.data_counter + 1 # print self.data_counter if __name__ == '__main__': try: if not ServerSolution.resolveRosmaster(): raise rospy.ROSInterruptException rosnode = RadarVizNode('radar_viz', maintain_last_plot = True) # rosnode = RadarVizNode('radar_viz', maintain_last_plot = False) # rosnode.startVisuzalizationNode('radar_viz') except rospy.ROSInterruptException: pass
def startRosNode(node_name): if not ServerSolution.resolveRosmaster(): return # print None # print os.path.dirname(os.path.realpath(__file__)) # devices_conf = ServerSolution.resolveParameters('radar_packet/devices',os.path.dirname(os.path.realpath(__file__))+'radar_read_param.py') # devices_conf = ServerSolution.resolveParameters('radar_packet/devices','radar_read_param.py') devices_conf = ServerSolution.resolveParameters('radar_packet/devices',os.path.dirname(__file__)+'/radar_read_param.py') # devices_conf = ServerSolution.resolveParameters('radar_packet/devices','rosrun foobar radar_read_param.py') if devices_conf is None: return options_conf = ServerSolution.resolveParameters('radar_packet/options',os.path.dirname(__file__)+'/radar_read_param.py') # options_conf = ServerSolution.resolveParameters('radar_packet/options','rosrun foobar radar_read_param.py') if options_conf is None: return if ServerSolution.checkNodeStarted(node_name): return rospy.init_node(node_name, anonymous=False) print 'start to operating on these devices...' for device in devices_conf: print device['interface'], device['name_id'] # return thread_list = {} pub_can_msg = {} pub_log_msg = {} sub_can_msg = {} pub_log_msg_central = rospy.Publisher ('radar_packet/log' , String , queue_size=10) for device in devices_conf: solved_name_res = getNameResolution(options_conf['name_resolution'],device['name_id'],device['interface']) thread_list[device['interface']] = {} thread_list[device['interface']]['name_id'] = device['name_id'] pub_can_msg[device['interface']] = rospy.Publisher ('radar_packet/'+solved_name_res+'/recv', CanBusMsg, queue_size=100) pub_log_msg[device['interface']] = rospy.Publisher ('radar_packet/'+solved_name_res+'/log' , String , queue_size=10) _thread = startCanThread(pub_can_msg[device['interface']], device['interface'], device['name_id']) thread_list[device['interface']]['can_publish_thread'] = _thread[0] thread_list[device['interface']]['can_polling_thread'] = _thread[1] thread_list[device['interface']]['can_send_thread'] = _thread[2] sub_can_msg[device['interface']] = rospy.Subscriber('radar_packet/'+solved_name_res+'/send', CanBusMsg, thread_list[device['interface']]['can_send_thread'].callback) thread_list[device['interface']]['pub_log_msg'] = pub_log_msg[device['interface']] print 'started ' + device['interface'] # print thread_list # print pub_can_msg # print pub_log_msg # print sub_can_msg rate = rospy.Rate(1) while not rospy.is_shutdown(): # print can_publish_thread.getCounter() # pub_log_msg.publish('data received: '+str(can_publish_thread.getCounter())) # can_publish_thread.resetCounter() _string_log = 'data received: ' for thread in thread_list: _str_recv_counter = str(thread_list[thread]['can_publish_thread'].getCounter()) _str_send_counter = str(thread_list[thread]['can_send_thread'].getCounter()) thread_list[thread]['pub_log_msg'].publish(_str_recv_counter) _string_log = _string_log + thread + ' ' + thread_list[thread]['name_id'] + ' recv, ' + _str_recv_counter + ' sent, ' + _str_send_counter + ' ' thread_list[thread]['can_publish_thread'].resetCounter() thread_list[thread]['can_send_thread'].resetCounter() # print _string_log pub_log_msg_central.publish(_string_log) rate.sleep() print 'stopping canbuses...' for thread in thread_list: thread_list[thread]['can_publish_thread'].setThreadStop(True) thread_list[thread]['can_polling_thread'].setThreadStop(True) thread_list[thread]['can_publish_thread'].join() thread_list[thread]['can_polling_thread'].join() print 'stopped ' + thread