def _stop_handler(self, data):
try:
rospy.loginfo('Received: ' + data)
msg = messages.parse(data)
except Exception as e:
rospy.logwarn('Could not parse data: %s (%s)' % (str(data), e))
else:
response = [comm.params['id']]
response_msg = messages.create('stop-resp', comm.params['id'],
msg['orig'], 'comm_receiver',
response)
messages.send(comm.params['ports'][msg['orig']], response_msg)
rospy.loginfo('Sent: ' + response_msg)
def handleIncomingMsg(data, sid):
try:
inputMsg = m.parse(data)
type = inputMsg.type
except :
type = 'error'
if type == "new":
handleJoinGame(inputMsg, sid)
elif type == "move":
handleMove(inputMsg, sid)
else:
msg = 'Error reading game request. Please make sure message type is either [new] or [move]'
error = m.ResponseMessage('error', None, None, None, msg, False)
print 'Error reading game request.'
sendMessage(sid, error)
def _request_handler(self, data):
try:
rospy.loginfo('Received: ' + data)
msg = messages.parse(data)
except Exception as e:
rospy.logwarn('Could not parse data: %s (%s)' % (str(data), e))
else:
response = [
comm.params['id'], comm.current_node['id'],
comm.current_distance, comm.current_vel
]
response_msg = messages.create('request-info', comm.params['id'],
msg['orig'], 'comm_receiver',
response)
messages.send(comm.params['ports'][msg['orig']], response_msg)
rospy.loginfo("Sent: " + response_msg)
def handle_read(self):
"""receive, parse and pass packets to handle_message()"""
packet = self.recv(8192)
if packet == "":
#print "[WARNING] Socket closed by remote host %s:%s" % (
# self.address,self.port)
self.close()
return
packet_list = messages.separate_messages(packet)
#received_types = " + ".join(
# messages.get_message_type(messages.parse(packet))
# for packet in packet_list)
#print "From %s:%s received: " % (self.address, self.port), received_types
# Process a single message at a time
for packet in packet_list:
message = messages.parse(packet)
if messages.get_message_type(message) == "OFPT_ECHO_REQUEST":
self.buffer.append(messages.of_echo_reply)
else:
self.handle_message(message)
def _request_info_handler(self, data):
try:
rospy.loginfo('Received: ' + data + ' current_node: ' +
str(comm.current_node['id']))
#stop current running pub thread
self.stop_pub.set()
time.sleep(0.1)
msg = messages.parse(data)
n_id, n_node_id, n_dist, n_vel = msg['data']
except Exception as e:
rospy.logwarn('Could not parse data: %s (%s)' % (str(data), e))
else:
#Only if the robots that share same intersection will be added into the dict for coordination
self._maintain_approaching_robots(msg)
#Got the same number of response as queries, and then start to coordinate
comm.send_count += 1
if comm.send_count == len(comm.neighbor_list):
#if the list is empty, we don't need to apply velocity coordination
rospy.loginfo('approaching_dict: ' +
str(comm.approaching_dict))
if comm.approaching_dict:
vel = self._intersection_coordination()
rospy.loginfo(str(vel))
self.pub_vel_thread = threading.Thread(
target=self._publish_coordinated_vel, args=(vel, ))
self.stop_pub.clear()
self.pub_vel_thread.start()
else:
rospy.loginfo('rollback to single agent')
self.stop_pub.set(
) #no other approach robots, then just stop control
comm.send_count = 0
comm.state = 'DONE'
rospy.loginfo('Current State: %s' % comm.state)
while True:
# Accepts connections
(clientSocket, address) = serverSocket.accept()
print("New connection with: " + str(address))
# Receives the message in pieces, if necessary
message = ""
while True:
data = clientSocket.recv(4096)
if data:
message += data
else:
break
# Parsing the message
catalogueMessage = messages.parse(message)
if isinstance(catalogueMessage, messages.Register):
catalogue.register(
catalogueMessage.host + ":" + str(catalogueMessage.port),
catalogueMessage.id, catalogueMessage.type,
catalogueMessage.clusterId)
elif isinstance(catalogueMessage, messages.RegisterMaster):
catalogue.registerMaster(
catalogueMessage.host + ":" + str(catalogueMessage.port),
catalogueMessage.id, catalogueMessage.type,
catalogueMessage.clusterId)
else:
pass # ...
clientSocket.close()
def testFDSite(self):
xsize = 64
ysize = int(xsize * 3.0/4.0)
im = image.T(xsize,ysize)
(rfd,wfd) = os.pipe()
site = fract4dc.fdsite_create(wfd)
file = self.compileColorMandel()
for x in xrange(2):
handle = fract4dc.pf_load(file)
pfunc = fract4dc.pf_create(handle)
fract4dc.pf_init(pfunc,pos_params,self.color_mandel_params)
cmap = fract4dc.cmap_create(
[(0.0,0,0,0,255),
(1/256.0,255,255,255,255),
(1.0, 255, 255, 255, 255)])
fract4dc.calc(
params=[0.0, 0.0, 0.0, 0.0,
4.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
antialias=0,
maxiter=100,
yflip=0,
nthreads=1,
pfo=pfunc,
cmap=cmap,
auto_deepen=0,
periodicity=1,
render_type=0,
image=im._img,
site=site,
async=True)
nrecved = 0
while True:
if nrecved == x:
#print "hit message count"
fract4dc.interrupt(site)
nb = 2*4
bytes = os.read(rfd,nb)
if len(bytes) < nb:
self.fail("bad message")
break
(t,size) = struct.unpack("2i",bytes)
#print "read %d, len %d" % (t,size)
# read the rest of the message
bytes = os.read(rfd,size)
if len(bytes) < size:
self.fail("bad message")
break
msg = messages.parse(t, bytes)
#print "msg: %s" % msg.show()
if msg.name == "Status" and msg.status == 0:
#done
#print "done"
break
nrecved += 1
