auction_action.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <actionlib/server/simple_action_server.h>
3 #include <geometry_msgs/Pose.h>
4 #include <cpswarm_msgs/TaskAllocationEvent.h>
5 #include <cpswarm_msgs/TaskAllocatedEvent.h>
6 #include <cpswarm_msgs/TaskAllocationAction.h>
7 
8 using namespace std;
9 using namespace ros;
10 
14 typedef actionlib::SimpleActionServer<cpswarm_msgs::TaskAllocationAction> Server;
15 
19 Publisher publisher;
20 
24 int task_id;
25 
29 geometry_msgs::Pose task_pose;
30 
35 
39 double timeout;
40 
44 double highest_bid;
45 
49 string winner;
50 
55 void bid_callback(const cpswarm_msgs::TaskAllocationEvent::ConstPtr& msg)
56 {
57  if (auction_open && (task_id == msg->id)) {
58  ROS_DEBUG("TASK_AUCTION - New bid received from %s", msg->swarmio.node.c_str());
59  // new highest bid received
60  if (msg->bid > highest_bid) {
61  winner = msg->swarmio.node;
62  highest_bid = msg->bid;
63  ROS_INFO("TASK_AUCTION - Highest bid %.4f received from %s", msg->bid, msg->swarmio.node.c_str());
64  }
65  }
66 }
67 
73 void auction_callback(const cpswarm_msgs::TaskAllocationGoal::ConstPtr& goal, Server* as)
74 {
75  // start a new auction
76  task_id = goal->task_id;
77  task_pose = goal->task_pose.pose;
78  highest_bid = 0;
79  winner = "";
80  auction_open = true;
81  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);
82 
83  // wait for bids
84  NodeHandle nh;
85  double loop_rate;
86  nh.param(this_node::getName() + "/loop_rate", loop_rate, 5.0);
87  Rate rate(loop_rate);
88  Time start_time = Time::now();
89  while (ok() && !as->isPreemptRequested() && Time::now() - start_time < Duration(timeout)) {
90  spinOnce();
91  rate.sleep();
92  }
93 
94  // close auction
95  auction_open = false;
96  if (winner.compare("") != 0) {
97  cpswarm_msgs::TaskAllocatedEvent allocation;
98  allocation.header.stamp = Time::now();
99  allocation.header.frame_id = "local_origin_ned";
100  allocation.swarmio.name = "cps_selected";
101  allocation.task_id = task_id;
102  allocation.cps_id = winner;
103  publisher.publish(allocation);
104  ROS_INFO("TASK_AUCTION - Task %d allocated to %s", task_id, winner.c_str());
105  }
106 
107  // action server has been preempted
108  if (as->isPreemptRequested()) {
109  as->setPreempted();
110  }
111 
112  else {
113  // set action result
114  cpswarm_msgs::TaskAllocationResult result;
115  result.winner = winner;
116  result.task_id = task_id;
117  result.task_pose.pose = task_pose;
118 
119  // task allocation failed, return negative result
120  if (winner.compare("") == 0) {
121  as->setAborted(result);
122  }
123 
124  // task allocation succeeded
125  else {
126  as->setSucceeded(result);
127  }
128  }
129 }
130 
137 int main(int argc, char **argv)
138 {
139  // init ros node
140  init(argc, argv, "task_allocation_auction");
141  NodeHandle nh;
142 
143  // init global variables
144  auction_open = false;
145  task_id = -1;
146  winner = "";
147 
148  // read parameters
149  nh.param(this_node::getName() + "/timeout", timeout, 10.0);
150  int queue_size;
151  nh.param(this_node::getName() + "/queue_size", queue_size, 10);
152 
153  // publishers and subscribers
154  Subscriber cost_subscriber = nh.subscribe<cpswarm_msgs::TaskAllocationEvent>("bridge/events/cps_selection", queue_size, bid_callback);
155  publisher = nh.advertise<cpswarm_msgs::TaskAllocatedEvent>("cps_selected", queue_size, true);
156 
157  // start the action server and wait
158  Server server(nh, "cmd/task_allocation_auction", boost::bind(&auction_callback, _1, &server), false);
159  server.start();
160  ROS_INFO("TASK_AUCTION - Task allocation auction action available");
161  spin();
162  return 0;
163 }
string winner
The UUID of the CPS that is the current highest bidder.
actionlib::SimpleActionServer< cpswarm_msgs::TaskAllocationAction > Server
The server object that offers the task allocation auction as action server.
bool auction_open
Whether the auction is currently open for bidding.
void auction_callback(const cpswarm_msgs::TaskAllocationGoal::ConstPtr &goal, Server *as)
Callback function to start a task allocation auction.
void bid_callback(const cpswarm_msgs::TaskAllocationEvent::ConstPtr &msg)
Callback function to receive auction bids.
int task_id
ID of the task that is auctioned.
double highest_bid
The highest current bid.
int main(int argc, char **argv)
A ROS node that exchanges relative kinematics between CPSs in a swarm.
geometry_msgs::Pose task_pose
Location of the task that is auctioned.
double timeout
The auction duration.
Publisher publisher
Publisher to announce the winner of the auction.
cpswarm_msgs::TaskAllocatedEvent allocation
The result of the task allocation auction.
Definition: bid_action.cpp:31


task_allocation
Author(s):
autogenerated on Thu Oct 31 2019 16:22:51