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
