auction_action.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <geometry_msgs/Pose.h>
#include <cpswarm_msgs/TaskAllocationEvent.h>
#include <cpswarm_msgs/TaskAllocatedEvent.h>
#include <cpswarm_msgs/TaskAllocationAction.h>

using namespace std;
using namespace ros;

typedef actionlib::SimpleActionServer<cpswarm_msgs::TaskAllocationAction> Server;

Publisher publisher;

int task_id;

geometry_msgs::Pose task_pose;

bool auction_open;

double timeout;

double highest_bid;

string winner;

void bid_callback(const cpswarm_msgs::TaskAllocationEvent::ConstPtr& msg)
{
    if (auction_open && (task_id == msg->id)) {
        ROS_DEBUG("TASK_AUCTION - New bid received from %s", msg->swarmio.node.c_str());
        // new highest bid received
        if (msg->bid > highest_bid) {
            winner = msg->swarmio.node;
            highest_bid = msg->bid;
            ROS_INFO("TASK_AUCTION - Highest bid %.4f received from %s", msg->bid, msg->swarmio.node.c_str());
        }
    }
}

void auction_callback(const cpswarm_msgs::TaskAllocationGoal::ConstPtr& goal, Server* as)
{
    // start a new auction
    task_id = goal->task_id;
    task_pose = goal->task_pose.pose;
    highest_bid = 0;
    winner = "";
    auction_open = true;
    ROS_INFO("TASK_AUCTION - Starting task allocation auction for task %d at position (%.6f, %.6f)", task_id, task_pose.position.x, task_pose.position.y);

    // wait for bids
    NodeHandle nh;
    double loop_rate;
    nh.param(this_node::getName() + "/loop_rate", loop_rate, 5.0);
    Rate rate(loop_rate);
    Time start_time = Time::now();
    while (ok() && !as->isPreemptRequested() && Time::now() - start_time < Duration(timeout)) {
        spinOnce();
        rate.sleep();
    }

    // close auction
    auction_open = false;
    if (winner.compare("") != 0) {
        cpswarm_msgs::TaskAllocatedEvent allocation;
        allocation.header.stamp = Time::now();
        allocation.header.frame_id = "local_origin_ned";
        allocation.swarmio.name = "cps_selected";
        allocation.task_id = task_id;
        allocation.cps_id = winner;
        publisher.publish(allocation);
        ROS_INFO("TASK_AUCTION - Task %d allocated to %s", task_id, winner.c_str());
    }

    // action server has been preempted
    if (as->isPreemptRequested()) {
        as->setPreempted();
    }

    else {
        // set action result
        cpswarm_msgs::TaskAllocationResult result;
        result.winner = winner;
        result.task_id = task_id;
        result.task_pose.pose = task_pose;

        // task allocation failed, return negative result
        if (winner.compare("") == 0) {
            as->setAborted(result);
        }

        // task allocation succeeded
        else {
            as->setSucceeded(result);
        }
    }
}

int main(int argc, char **argv)
{
    // init ros node
    init(argc, argv, "task_allocation_auction");
    NodeHandle nh;

    // init global variables
    auction_open = false;
    task_id = -1;
    winner = "";

    // read parameters
    nh.param(this_node::getName() + "/timeout", timeout, 10.0);
    int queue_size;
    nh.param(this_node::getName() + "/queue_size", queue_size, 10);

    // publishers and subscribers
    Subscriber cost_subscriber = nh.subscribe<cpswarm_msgs::TaskAllocationEvent>("bridge/events/cps_selection", queue_size, bid_callback);
    publisher = nh.advertise<cpswarm_msgs::TaskAllocatedEvent>("cps_selected", queue_size, true);

    // start the action server and wait
    Server server(nh, "cmd/task_allocation_auction", boost::bind(&auction_callback, _1, &server), false);
    server.start();
    ROS_INFO("TASK_AUCTION - Task allocation auction action available");
    spin();
    return 0;
}