def handle_auctioneer_server_callback(auction_req): # define global variables global winner_id global winner_cost # update number of messages in parameter server if rospy.has_param('/num_messages'): num_messages = rospy.get_param('/num_messages') num_messages += 2 rospy.set_param('/num_messages', num_messages) # obtain auctioneer_position auctioneer_position = {'auctioneer_position': rospy.get_param('~position')} # Obtain nodes list to relay information with k=1 neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list(auction_req) # Prepare auction information if auction_req.auction_data.command == 'join_auction': role = "be_buyer" else: role = 'none' auction_type = 'k-sap' sending_node = rospy.get_name() auctioneer_node = rospy.get_name() # updated nodes_collected if rospy.has_param('/nodes_collected'): nodes_collected = rospy.get_param('/nodes_collected')+','+rospy.get_name() rospy.set_param('/nodes_collected',nodes_collected) else: nodes_collected = rospy.get_param('~neighbour_nodes_list') auction_data = auction_req.auction_data # send requests for neighbours for node in neighbour_nodes_relay_list: # prepare neighbours to be buyers service_path = node+'/auction_config_server' rospy.wait_for_service(service_path) neighbour_node_auction_config_server = rospy.ServiceProxy(service_path, auction_srvs.srv.AuctionConfigService) try: neighbour_node_auction_config_server_resp = neighbour_node_auction_config_server(role,auction_type,sending_node) except rospy.ServiceException, e: rospy.loginfo("Service call failed: %s",e) # send the auction information to the buyer node service_path = node+'/buyer_server' rospy.wait_for_service(service_path) buyer_service = rospy.ServiceProxy(service_path, auction_srvs.srv.BuyerService,headers=auctioneer_position) try: buyer_server_resp = buyer_service(auctioneer_node,sending_node,nodes_collected,auction_data) except rospy.ServiceException, e: print "Service did not process request: %s"%str(e)
def handle_auction_server_callback(auction_req): # define global variables global winner_id global winner_cost # update number of messages in parameter server if rospy.has_param('/num_messages'): num_messages = rospy.get_param('/num_messages') num_messages += 2 rospy.set_param('/num_messages', num_messages) # default bid bid = auction_msgs.msg.Bid() # obtain auctioneer_position auctioneer_position = {'auctioneer_position': rospy.get_param('~position')} # Obtain nodes list to relay information with k=1 neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list( auction_req) print neighbour_nodes_relay_list # Prepare auction information if auction_req.auction_data.command == 'close_auction': auction_req.role = 'none' else: auction_req.role = "be_buyer" auction_req.sending_node = rospy.get_name() # updated nodes_collected if rospy.has_param('/nodes_collected'): auction_req.nodes_collected = rospy.get_param( '/nodes_collected') + ',' + rospy.get_name() rospy.set_param('/nodes_collected', auction_req.nodes_collected) else: auction_req.nodes_collected = rospy.get_param('~neighbour_nodes_list') # Call the Auction Service from each neighbour node for node in neighbour_nodes_relay_list: # compose service name (to be changed) service_path = node + '/auction_server' # wait for the service in the neighbour node to be available rospy.wait_for_service(service_path) neighbour_node_auction_server = rospy.ServiceProxy( service_path, auction_srvs.srv.AuctionService, headers=auctioneer_position) try: bid_response = neighbour_node_auction_server(auction_req) bid = bid_response.bid_data # Evaluate bids, Min(cost_distance) if winner_cost >= bid.cost_distance: if bid.buyer_id != '': winner_cost = bid.cost_distance winner_id = bid.buyer_id # log info for momentary winner # rospy.loginfo("(winning at the moment) %s with offer %d",winner_id, winner_cost) except rospy.ServiceException, e: rospy.loginfo("Service call failed: %s", e)
def handle_auction_server_callback(auction_req): # update number of messages in parameter server if rospy.has_param('/num_messages'): num_messages = rospy.get_param('/num_messages') num_messages += 2 rospy.set_param('/num_messages', num_messages) # Graph parameters graph_parameters = eval(rospy.get_param("graph_info")) N = int(graph_parameters[0]) l = int(graph_parameters[1]) d = int(graph_parameters[2]) r = math.sqrt((d * l * l) / (math.pi * (N - 1))) print N, l, d, r # Calculate k (number of hop) node_position = eval(rospy.get_param('~position')) auctioneer_position = eval( auction_req._connection_header['auctioneer_position']) x = float(node_position[0]) - float(auctioneer_position[0]) y = float(node_position[1]) - float(auctioneer_position[1]) z = float(node_position[2]) - float(auctioneer_position[2]) distance_from_node_to_auctioneer = math.sqrt(x * x + y * y + z * z) k = int(distance_from_node_to_auctioneer / r) print distance_from_node_to_auctioneer, k # Create a bid messsage to put an offer for the item in auction_req! bid_response = auction_msgs.msg.Bid() bid_response.header.frame_id = 'base_link' # to be rechecked bid_response.header.stamp = rospy.Time.now() bid_response.buyer_id = rospy.get_name() if auction_req.auction_data.metrics == "distance": # to be given by the cost to go to position of the ocurring event # the cost for the metrics==distance is calculated using the euclidean # distance between the parameter position of the node and the task_position # given in the auction_req node_position = eval(rospy.get_param('~position')) x = float(node_position[0]) - auction_req.auction_data.task_location.x y = float(node_position[1]) - auction_req.auction_data.task_location.y z = float(node_position[2]) - auction_req.auction_data.task_location.z bid_response.cost_distance = float(math.sqrt(x * x + y * y + z * z)) else: rospy.loginfo("Metrics unkown") bid_response.cost_distance = 999999 print auction_req.auction_data.subject # Check if node is in the k-hops required range if k < int(auction_req.auction_data.subject): # Relay information to neighbour nodes! neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list( auction_req) print neighbour_nodes_relay_list if neighbour_nodes_relay_list: # Prepare auction information if auction_req.auction_data.command == 'join_auction': role = 'be_buyer' else: role = 'none' auction_req.sending_node = rospy.get_name() # updated nodes_collected if rospy.has_param('/nodes_collected'): auction_req.nodes_collected = rospy.get_param( '/nodes_collected') + ',' + rospy.get_name() rospy.set_param('/nodes_collected', auction_req.nodes_collected) else: auction_req.nodes_collected = rospy.get_param( '~neighbour_nodes_list') for node in neighbour_nodes_relay_list: # compose service name service_path = node + '/auction_server' rospy.wait_for_service(service_path) neighbour_node_auction_server = rospy.ServiceProxy( service_path, auction_srvs.srv.AuctionService, headers={'auctioneer_position': auctioneer_position}) try: neighbour_node_bid_response = neighbour_node_auction_server( auction_req) # log bid information from the neighbour node (debug) # rospy.loginfo(neighbour_node_bid_response) except rospy.ServiceException, e: rospy.loginfo("Service call failed: %s", e) if neighbour_node_bid_response.bid_data.cost_distance < bid_response.cost_distance: bid_response.buyer_id = neighbour_node_bid_response.bid_data.buyer_id bid_response.cost_distance = neighbour_node_bid_response.bid_data.cost_distance
def handle_auction_server_callback(auction_req): # update number of messages in parameter server if rospy.has_param('/num_messages'): num_messages = rospy.get_param('/num_messages') num_messages += 2 rospy.set_param('/num_messages', num_messages) # Graph parameters graph_parameters = eval(rospy.get_param("graph_info")) N = int(graph_parameters[0]) l = int(graph_parameters[1]) d = int(graph_parameters[2]) r = math.sqrt((d*l*l)/(math.pi*(N-1))) print N, l, d, r # Calculate k (number of hop) node_position = eval(rospy.get_param('~position')) auctioneer_position = eval(auction_req._connection_header['auctioneer_position']) x = float(node_position[0])-float(auctioneer_position[0]) y = float(node_position[1])-float(auctioneer_position[1]) z = float(node_position[2])-float(auctioneer_position[2]) distance_from_node_to_auctioneer = math.sqrt(x*x+y*y+z*z) k = int(distance_from_node_to_auctioneer/r) print distance_from_node_to_auctioneer, k # Create a bid messsage to put an offer for the item in auction_req! bid_response = auction_msgs.msg.Bid() bid_response.header.frame_id = 'base_link' # to be rechecked bid_response.header.stamp = rospy.Time.now() bid_response.buyer_id = rospy.get_name() if auction_req.auction_data.metrics == "distance": # to be given by the cost to go to position of the ocurring event # the cost for the metrics==distance is calculated using the euclidean # distance between the parameter position of the node and the task_position # given in the auction_req node_position = eval(rospy.get_param('~position')) x = float(node_position[0])-auction_req.auction_data.task_location.x y = float(node_position[1])-auction_req.auction_data.task_location.y z = float(node_position[2])-auction_req.auction_data.task_location.z bid_response.cost_distance = float(math.sqrt(x*x+y*y+z*z)) else: rospy.loginfo("Metrics unkown") bid_response.cost_distance = 999999; print auction_req.auction_data.subject # Check if node is in the k-hops required range if k < int(auction_req.auction_data.subject): # Relay information to neighbour nodes! neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list(auction_req) print neighbour_nodes_relay_list if neighbour_nodes_relay_list: # Prepare auction information if auction_req.auction_data.command == 'join_auction': role = 'be_buyer' else: role = 'none' auction_req.sending_node = rospy.get_name() # updated nodes_collected if rospy.has_param('/nodes_collected'): auction_req.nodes_collected = rospy.get_param('/nodes_collected')+','+rospy.get_name() rospy.set_param('/nodes_collected',auction_req.nodes_collected) else: auction_req.nodes_collected = rospy.get_param('~neighbour_nodes_list') for node in neighbour_nodes_relay_list: # compose service name service_path = node+'/auction_server' rospy.wait_for_service(service_path) neighbour_node_auction_server = rospy.ServiceProxy(service_path, auction_srvs.srv.AuctionService, headers={ 'auctioneer_position': auctioneer_position}) try: neighbour_node_bid_response = neighbour_node_auction_server(auction_req) # log bid information from the neighbour node (debug) # rospy.loginfo(neighbour_node_bid_response) except rospy.ServiceException, e: rospy.loginfo("Service call failed: %s",e) if neighbour_node_bid_response.bid_data.cost_distance < bid_response.cost_distance: bid_response.buyer_id= neighbour_node_bid_response.bid_data.buyer_id bid_response.cost_distance= neighbour_node_bid_response.bid_data.cost_distance
def handle_auction_server_callback(auction_req): # update number of messages in parameter server if rospy.has_param('/num_messages'): num_messages = rospy.get_param('/num_messages') num_messages += 2 rospy.set_param('/num_messages', num_messages) # Create a bid messsage to put an offer for the item in auction_req! bid_response = auction_msgs.msg.Bid() bid_response.header.frame_id = 'base_link' # to be rechecked bid_response.header.stamp = rospy.Time.now() bid_response.buyer_id = rospy.get_name() if auction_req.auction_data.metrics == "distance": # to be given by the cost to go to position of the ocurring event # the cost for the metrics==distance is calculated using the euclidean # distance between the parameter position of the node and the task_position # given in the auction_req node_position = eval(rospy.get_param('~position')) x = float(node_position[0]) - auction_req.auction_data.task_location.x y = float(node_position[1]) - auction_req.auction_data.task_location.y z = float(node_position[2]) - auction_req.auction_data.task_location.z bid_response.cost_distance = float(math.sqrt(x * x + y * y + z * z)) else: rospy.loginfo("Metrics unkown") bid_response.cost_distance = 999999 # Relay information to neighbour nodes! neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list( auction_req) if neighbour_nodes_relay_list: # Prepare auction information if auction_req.auction_data.command == 'join_auction': role = 'be_buyer' else: role = 'none' auction_req.sending_node = rospy.get_name() # updated nodes_collected if rospy.has_param('/nodes_collected'): auction_req.nodes_collected = rospy.get_param( '/nodes_collected') + ',' + rospy.get_name() rospy.set_param('/nodes_collected', auction_req.nodes_collected) else: auction_req.nodes_collected = rospy.get_param( '~neighbour_nodes_list') # call the Auction Service from each neighbour of the node for node in neighbour_nodes_relay_list: # obtain response from neighbour buyer node (in k=1) bid_response_neighbour_node = auction_common.neighbour_node_auction_client( node, auction_req) if bid_response_neighbour_node.cost_distance < bid_response.cost_distance: bid_response.buyer_id = bid_response_neighbour_node.buyer_id bid_response.cost_distance = bid_response_neighbour_node.cost_distance # return best bid return { 'response_info': 'valid' + rospy.get_name(), 'bid_data': bid_response }
service_path = auction_req.auctioneer_node + '/auctioneer_bid_reception_server' rospy.wait_for_service(service_path) auctioneer_bid_reception_service = rospy.ServiceProxy( service_path, auction_srvs.srv.AuctioneerBidReceptionService) try: sending_node = rospy.get_name() auctioneer_bid_reception_server_resp = auctioneer_bid_reception_service( sending_node, bid) except rospy.ServiceException, e: rospy.logwarn("Service did not process request: %s", e) # Relay information to neighbour nodes! neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list( auction_req) if neighbour_nodes_relay_list: # Prepare information if auction_req.auction_data.command == 'join_auction': role = 'be_buyer' else: role = 'none' auction_type = 'sap' sending_node = rospy.get_name() auctioneer_node = auction_req.auctioneer_node # updated nodes_collected
def handle_auction_server_callback(auction_req): # update number of messages in parameter server if rospy.has_param('/num_messages'): num_messages = rospy.get_param('/num_messages') num_messages += 2 rospy.set_param('/num_messages', num_messages) # Create a bid messsage to put an offer for the item in auction_req! bid_response = auction_msgs.msg.Bid() bid_response.header.frame_id = 'base_link' # to be rechecked bid_response.header.stamp = rospy.Time.now() bid_response.buyer_id = rospy.get_name() if auction_req.auction_data.metrics == "distance": # to be given by the cost to go to position of the ocurring event # the cost for the metrics==distance is calculated using the euclidean # distance between the parameter position of the node and the task_position # given in the auction_req node_position = eval(rospy.get_param('~position')) x = float(node_position[0])-auction_req.auction_data.task_location.x y = float(node_position[1])-auction_req.auction_data.task_location.y z = float(node_position[2])-auction_req.auction_data.task_location.z bid_response.cost_distance = float(math.sqrt(x*x+y*y+z*z)) else: rospy.loginfo("Metrics unkown") bid_response.cost_distance = 999999; # Relay information to neighbour nodes! neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list(auction_req) if neighbour_nodes_relay_list: # Prepare auction information if auction_req.auction_data.command == 'join_auction': role = 'be_buyer' else: role = 'none' auction_req.sending_node = rospy.get_name() # updated nodes_collected if rospy.has_param('/nodes_collected'): auction_req.nodes_collected = rospy.get_param('/nodes_collected')+','+rospy.get_name() rospy.set_param('/nodes_collected',auction_req.nodes_collected) else: auction_req.nodes_collected = rospy.get_param('~neighbour_nodes_list') # call the Auction Service from each neighbour of the node for node in neighbour_nodes_relay_list: # obtain response from neighbour buyer node (in k=1) bid_response_neighbour_node = auction_common.neighbour_node_auction_client(node,auction_req) if bid_response_neighbour_node.cost_distance < bid_response.cost_distance: bid_response.buyer_id= bid_response_neighbour_node.buyer_id bid_response.cost_distance= bid_response_neighbour_node.cost_distance # return best bid return {'response_info': 'valid'+rospy.get_name(), 'bid_data':bid_response}
try: sending_node = rospy.get_name() auctioneer_bid_reception_server_resp = auctioneer_bid_reception_service(sending_node,bid) except rospy.ServiceException, e: print "Service did not process request: %s"%str(e) print int(auction_req.auction_data.subject) # check if node is in the k-hops required range if k < int(auction_req.auction_data.subject): # Relay information to neighbour nodes! neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list(auction_req) if neighbour_nodes_relay_list: # Prepare information if auction_req.auction_data.command == 'join_auction': role = "be_buyer" else: role = 'none' auction_type = 'k-sap' sending_node = rospy.get_name() auctioneer_node = auction_req.auctioneer_node # updated nodes_collected
def handle_auction_server_callback(auction_req): # define global variables global winner_id global winner_cost # update number of messages in parameter server if rospy.has_param('/num_messages'): num_messages = rospy.get_param('/num_messages') num_messages += 2 rospy.set_param('/num_messages', num_messages) # default bid bid = auction_msgs.msg.Bid() # obtain auctioneer_position auctioneer_position = {'auctioneer_position': rospy.get_param('~position')} # Obtain nodes list to relay information with k=1 neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list(auction_req) print neighbour_nodes_relay_list # Prepare auction information if auction_req.auction_data.command == 'close_auction': auction_req.role = 'none' else: auction_req.role = "be_buyer" auction_req.sending_node = rospy.get_name() # updated nodes_collected if rospy.has_param('/nodes_collected'): auction_req.nodes_collected = rospy.get_param('/nodes_collected')+','+rospy.get_name() rospy.set_param('/nodes_collected',auction_req.nodes_collected) else: auction_req.nodes_collected = rospy.get_param('~neighbour_nodes_list') # Call the Auction Service from each neighbour node for node in neighbour_nodes_relay_list: # compose service name (to be changed) service_path = node+'/auction_server' # wait for the service in the neighbour node to be available rospy.wait_for_service(service_path) neighbour_node_auction_server = rospy.ServiceProxy(service_path, auction_srvs.srv.AuctionService,headers=auctioneer_position) try: bid_response = neighbour_node_auction_server(auction_req) bid = bid_response.bid_data # Evaluate bids, Min(cost_distance) if winner_cost >= bid.cost_distance: if bid.buyer_id != '': winner_cost = bid.cost_distance winner_id = bid.buyer_id # log info for momentary winner # rospy.loginfo("(winning at the moment) %s with offer %d",winner_id, winner_cost) except rospy.ServiceException, e: rospy.loginfo("Service call failed: %s",e)
def handle_auctioneer_server_callback(auction_req): # define global variables global winner_id global winner_cost # update number of messages in parameter server if rospy.has_param('/num_messages'): num_messages = rospy.get_param('/num_messages') num_messages += 2 rospy.set_param('/num_messages', num_messages) # Obtain nodes list to relay information with k=1 neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list(auction_req) # Prepare auction information if auction_req.auction_data.command == 'join_auction': role = "be_buyer" else: role = 'none' auction_type = 'sap' sending_node = rospy.get_name() auctioneer_node = rospy.get_name() # updated nodes_collected if rospy.has_param('/nodes_collected'): nodes_collected = rospy.get_param('/nodes_collected')+','+rospy.get_name() rospy.set_param('/nodes_collected',nodes_collected) else: nodes_collected = rospy.get_param('~neighbour_nodes_list') auction_data = auction_req.auction_data # send requests for neighbours for node in neighbour_nodes_relay_list: # prepare neighbours to be buyers service_path = node+'/auction_config_server' rospy.wait_for_service(service_path) neighbour_node_auction_config_server = rospy.ServiceProxy(service_path, auction_srvs.srv.AuctionConfigService) try: neighbour_node_auction_config_server_resp = neighbour_node_auction_config_server(role,auction_type,sending_node) except rospy.ServiceException, e: rospy.logwarn("[%s] Service call failed: %s",rospy.get_name(),e) # send the auction information to the buyer node service_path = node+'/buyer_server' rospy.wait_for_service(service_path) buyer_service = rospy.ServiceProxy(service_path, auction_srvs.srv.BuyerService) try: buyer_server_resp = buyer_service(auctioneer_node,sending_node,nodes_collected,auction_data) except rospy.ServiceException, e: rospy.logwarn("Service did not process request: %s",e)
def handle_auction_server_callback(auction_req): # define global variables global winner_id global winner_cost # update number of messages in parameter server if rospy.has_param('/num_messages'): num_messages = rospy.get_param('/num_messages') num_messages += 2 rospy.set_param('/num_messages', num_messages) # check for auction_req.auction_data.command (if close_auction -> clear role assignment if auction_req.auction_data.command == 'close_auction': auction_req.role = 'none' # Calculates its own bid offer for the item in auction_req #return {'response_info': 'invalid_bid'} elif auction_req.auction_data.command == 'join_auction': # Obtain nodes list to relay information with k=1 neighbour_nodes_relay_list = auction_common.create_neighbour_nodes_list(auction_req) # Change sending_node in auction_req to be sent to neighbour nodes auction_req.sending_node = rospy.get_name() # updated nodes_collected if rospy.has_param('/nodes_collected'): auction_req.nodes_collected = rospy.get_param('/nodes_collected')+','+rospy.get_name() rospy.set_param('/nodes_collected',auction_req.nodes_collected) else: auction_req.nodes_collected = rospy.get_param('~neighbour_nodes_list') auction_req.role = "be_buyer" # Call the Auction Service from each neighbour node for node in neighbour_nodes_relay_list: # obtain response from neighbour buyer node (in k=1), relaying auction_req bid_response = auction_common.neighbour_node_auction_client(node, auction_req) # Evaluate bids, Min(cost_distance) if winner_cost >= bid_response.cost_distance: if bid_response.buyer_id != '': winner_cost = bid_response.cost_distance winner_id = bid_response.buyer_id # log info for momentary winner # rospy.loginfo("(winning at the moment) %s with offer %d",winner_id, winner_cost) # verbose for auction status (received all the bids) rospy.loginfo("winner was: %s with offer %d",winner_id, winner_cost) """ # (close auction and inform winner) # (client to neighbour nodes with close_auction req) # (in close_auction req the nodes reset their roles!!) # Change the command in auction_req to be sent to neighbour nodes auction_req.auction_data.command = "close_auction" # Call the Auction Service Reset from each neighbour node for node in neighbour_nodes_relay_list: # obtain response from neighbour buyer node (in k=1), relaying auction_req reset_response = auction_common.neighbour_node_auction_client(node, auction_req) # Ok, now we can reset our role role_assigned = False node_role = 'none' return {'response_info': 'valid', 'bid_data':bid_response} """ # return response # return auction_srvs.srv.AuctionServiceResponse(bid_response) return {'response_info': 'valid', 'bid_data':bid_response}