├── README.md
├── SSI_Auctions.pdf
├── auction_goals.cpp
└── send_goal.cpp


/README.md:
--------------------------------------------------------------------------------
1 | # robotics
2 | Multi agent task allocation by auction.  
3 | C++ code and report.
4 | 


--------------------------------------------------------------------------------
/SSI_Auctions.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jakehoare/robotics/10981a22092ebb6390411c5579d1be0496c69e89/SSI_Auctions.pdf


--------------------------------------------------------------------------------
/auction_goals.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include <ros/ros.h>
  3 | #include <nav_msgs/GetPlan.h>
  4 | #include <geometry_msgs/PoseStamped.h>
  5 | #include <tf/transform_datatypes.h>
  6 | #include <visualization_msgs/Marker.h>
  7 | #include <string>
  8 | #include <vector>
  9 | using namespace std;
 10 | 
 11 | #define ROBOTS 10	// number of agents
 12 | #define TARGETS 50	// number of targets
 13 | #define SPEED 0.65	// linear movement speed
 14 | #define ANGULAR 1.0	// rotational movement speed
 15 | 
 16 | double goalTolerance = 0.5;
 17 | string worldFrame = "map";
 18 | 
 19 | double robot_locations[ROBOTS][2];	// start positions
 20 | double target_locations[TARGETS][2];	// targets to be visited
 21 | bool allocated_target[TARGETS] = {0};	// flag, initially all are unallocated
 22 | vector<int> allocations[ROBOTS];	// allocated targets for each robot
 23 | 
 24 | void fillPathRequest(nav_msgs::GetPlan::Request &req, int robot, int begin, int end);
 25 | double callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv, bool printout);
 26 | void setLocations();
 27 | string paramString(int robot, string param);
 28 | 
 29 | int main(int argc, char** argv) {
 30 | 
 31 | 	ros::init(argc, argv, "auction");
 32 | 	ros::NodeHandle nh;
 33 | 
 34 | 	string service_name = "/robot_0/move_base_node/make_plan";
 35 | 	while (!ros::service::waitForService(service_name, ros::Duration(3.0))) {
 36 | 		ROS_INFO("Waiting for service make_plan to become available");
 37 | 	}
 38 | 
 39 | 	ros::ServiceClient serviceClient = nh.serviceClient<nav_msgs::GetPlan>(service_name, true);
 40 | 	if (!serviceClient) {
 41 | 		ROS_FATAL("Could not initialize get plan service from %s", serviceClient.getService().c_str());
 42 | 		return -1;
 43 | 	}
 44 | 
 45 | 	// Load the initial and target locations
 46 | 	setLocations();
 47 | 
 48 | 	nav_msgs::GetPlan srv;
 49 | 
 50 | 	int unallocated = TARGETS;
 51 | 	double bid;
 52 | 	double lowest_bid;
 53 | 	int lowest_bidder;
 54 | 	int lowest_target;
 55 | 	int lowest_position;
 56 | 	int start_position;
 57 | 	double path_cost[ROBOTS] = {0.0};
 58 | 	double psi_bids[ROBOTS][TARGETS] = {0.0};
 59 | 	std::vector<int>::iterator it;
 60 | 
 61 | 	if (argc > 1) {
 62 | 		cout << "*** RUNNING SEQUENTIAL SINGLE ITEM AUCTION ***";
 63 | 		if (argc == 2)
 64 | 			cout << " (SIMPLE)" << endl;
 65 | 		else 
 66 | 			cout << " (FULL)" << endl;
 67 | 		// SEQUENTIAL SINGLE ITEM AUCTION
 68 | 		bool display = false;
 69 | 		while (unallocated != 0) {
 70 | 			lowest_bid = 999.0;
 71 | 			lowest_bidder = -1;
 72 | 			lowest_target = -1;
 73 | 			lowest_position = -1;
 74 | 			for (int robot = 0; robot < ROBOTS; ++robot) {
 75 | 				for (int target = 0; target < TARGETS; ++target) {
 76 | 					if (allocated_target[target])
 77 | 						continue;
 78 | 					// argc>2 means target can be inserted anywhere in current list of allocations for that robot
 79 | 					// else target can only be added to end of current list
 80 | 					if (argc > 2)
 81 | 						start_position = 0;
 82 | 					else
 83 | 						start_position = allocations[robot].size();
 84 | 					// Put target in all positions in current target list
 85 | 					for (int position = start_position; position <= allocations[robot].size(); ++position) {
 86 | 						// Calc cost from previous target (or start pos if none)
 87 | 						if (position == 0)
 88 | 							fillPathRequest(srv.request, robot, -1, target);
 89 | 						else
 90 | 							fillPathRequest(srv.request, robot, allocations[robot][position-1], target);
 91 | 						bid = callPlanningService(serviceClient, srv ,display);
 92 | 						// Add cost from target to next target (if any)
 93 | 						if (position < allocations[robot].size()) {
 94 | 							fillPathRequest(srv.request, robot, target, allocations[robot][position]);
 95 | 							bid += callPlanningService(serviceClient, srv ,display);
 96 | 							// Subtract cost between previous and next targets
 97 | 							if (position == 0)
 98 | 								fillPathRequest(srv.request, robot, -1, allocations[robot][position]);
 99 | 							else
100 | 								fillPathRequest(srv.request, robot, allocations[robot][position-1], allocations[robot][position]);
101 | 							bid -= callPlanningService(serviceClient, srv ,display);
102 | 
103 | 						}
104 | 						bid += path_cost[robot];
105 | 						//cout << "Robot " << robot << " bids " << bid << " for target " << target << " at pos " << position << endl;
106 | 						if (bid < lowest_bid) {
107 | 							lowest_bid = bid;
108 | 							lowest_bidder = robot;
109 | 							lowest_target = target;
110 | 							lowest_position = position;
111 | 						}
112 | 					}
113 | 				}
114 | 			}
115 | 			// End of of bidding - announce winner
116 | 			cout << "  Winner is " << lowest_bidder << " with " << lowest_bid << " for target " << lowest_target << " at pos " << lowest_position << endl;
117 | 			allocated_target[lowest_target] = true;
118 | 			--unallocated;
119 | 			it = allocations[lowest_bidder].begin();
120 | 			allocations[lowest_bidder].insert(it + lowest_position, lowest_target);
121 | 			path_cost[lowest_bidder] = lowest_bid;
122 | 		}
123 | 	} else {
124 | 		cout << "*** RUNNING PARALLEL SINGLE ITEM AUCTION ***" << endl;
125 | 		// PARALLEL SINGLE ITEM AUCTION	
126 | 		for (int target = 0; target < TARGETS; ++target) {
127 | 			lowest_bid = 999.0;		
128 | 			lowest_bidder = -1;
129 | 			lowest_target = -1;
130 | 			for (int robot = 0; robot < ROBOTS; ++robot) {
131 | 				// Get the path
132 | 				fillPathRequest(srv.request, robot, -1, target);
133 | 				if (!serviceClient) {
134 | 					ROS_FATAL("Connection to %s failed", serviceClient.getService().c_str());
135 | 					return -1;
136 | 				}
137 | 				psi_bids[robot][target] = callPlanningService(serviceClient, srv, false);
138 | 				if (psi_bids[robot][target] < lowest_bid) {
139 | 					lowest_bid = psi_bids[robot][target];
140 | 					lowest_bidder = robot;
141 | 					lowest_target = target;
142 | 				}
143 | 				//cout << "Robot " << robot << " bids " << psi_bids[robot][target] << " for target " << target << endl;				
144 | 			}
145 | 			// End of of bidding - announce winner		
146 | 			cout << "  Winner is " << lowest_bidder << " with " << lowest_bid;
147 | 			cout << " for target " << lowest_target << endl;
148 | 			allocations[lowest_bidder].push_back(lowest_target);
149 | 		}
150 | 		// For each robot calculate the path cost from allocated targets
151 | 		int begin, end;
152 | 		for (int robot = 0; robot < ROBOTS; ++robot) {
153 | 			begin = -1;
154 | 			for (int robotTarget = 0; robotTarget < allocations[robot].size(); ++robotTarget) {				
155 | 				end = allocations[robot][robotTarget];				
156 | 				fillPathRequest(srv.request, robot, begin, end);
157 | 				path_cost[robot] += callPlanningService(serviceClient, srv, false);
158 | 				begin = end;
159 | 			}
160 | 		}
161 | 	
162 | 	}
163 | 
164 | 	// Display paths and costs
165 | 	for (int robot = 0; robot < ROBOTS; ++robot) {
166 | 		cout << "Cost for robot " << robot << " is " << path_cost[robot] << " to visit targets ";
167 | 		for (int robotTarget = 0; robotTarget < allocations[robot].size(); ++robotTarget)
168 | 			cout << allocations[robot][robotTarget] << " ";
169 | 		cout << endl;
170 | 	}
171 | 	double max_cost = 0.0;
172 | 	for (int robot = 0; robot < ROBOTS; ++robot) {
173 | 		if (path_cost[robot] > max_cost)
174 | 			max_cost = path_cost[robot];
175 | 	}
176 | 	cout << "Maximum cost is " << max_cost << endl;  
177 | 
178 | 	return 0;	// TO RUN AUCTION ONLY, END HERE
179 | 
180 | 	// Now send robots to targets
181 | 	int target;
182 | 	int status;
183 | 	while (true) {
184 | 		// Check each robot
185 | 		for (int robot = 0; robot < ROBOTS; ++robot) {
186 | 			nh.getParam(paramString(robot, "status"), status);
187 | 			if (status >= allocations[robot].size()) 	// No more targets to be visited
188 | 				nh.setParam(paramString(robot, "control"), "notargets");  
189 | 			else {	// Update next target
190 | 				nh.setParam(paramString(robot, "control"), "targets");
191 | 				target = allocations[robot][status];
192 | 				//cout << "Current target for robot " << robot << " is " << target << endl;
193 | 				nh.setParam(paramString(robot, "goal_x"), target_locations[target][0]);
194 | 				nh.setParam(paramString(robot, "goal_y"), target_locations[target][1]);
195 | 				nh.setParam(paramString(robot, "goal_theta"), 0);
196 | 			}
197 | 			ros::Duration(0.1).sleep();
198 | 		}
199 | 	}
200 | 
201 | }
202 | 
203 | void fillPathRequest(nav_msgs::GetPlan::Request &request, int robot, int begin, int end) {
204 | 
205 | 	request.start.header.frame_id = worldFrame;
206 | 	if (begin == -1) {
207 | 		request.start.pose.position.x = robot_locations[robot][0];
208 | 		request.start.pose.position.y = robot_locations[robot][1];
209 | 	} else {
210 | 		request.start.pose.position.x = target_locations[begin][0];
211 | 		request.start.pose.position.y = target_locations[begin][1];
212 | 	}
213 | 	request.start.pose.orientation.w = 1.0;
214 | 
215 | 	request.goal.header.frame_id = worldFrame;
216 | 	request.goal.pose.position.x = target_locations[end][0];
217 | 	request.goal.pose.position.y = target_locations[end][1];
218 | 	request.goal.pose.orientation.w = 1.0;
219 | 
220 | 	request.tolerance = goalTolerance;
221 | 	//cout << "Target: " << target << " Robot: " << robot;
222 | 	//cout << " Calculating path from " << request.start.pose.position.x << ", " << request.start.pose.position.y 
223 | //		<< " to " << request.goal.pose.position.x << ", " << request.goal.pose.position.y << endl;
224 | }
225 | 
226 | double callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv, bool printout) {
227 | 	double cost = 0.0;
228 | 	double a_cost = 0.0;
229 | 	double prev_angle = 0.0;
230 | 	double x_move, y_move, prev_x, prev_y, angle;
231 | 	geometry_msgs::PoseStamped p;
232 | 
233 | 	if (serviceClient.call(srv)) {
234 | 		if (!srv.response.plan.poses.empty()) {
235 | 			prev_x = srv.response.plan.poses[0].pose.position.x;
236 | 			prev_y = srv.response.plan.poses[0].pose.position.y;
237 | 			for (int i = 1; i < srv.response.plan.poses.size() - 2; ++i) {
238 | 				p = srv.response.plan.poses[i];
239 | 				x_move = p.pose.position.x - prev_x;
240 | 				y_move = p.pose.position.y - prev_y;
241 | 				cost += sqrt(pow(x_move, 2) + pow(y_move, 2));
242 | 				// angle is between 0 and 360
243 | 				angle = atan(y_move/x_move) * (180.0/M_PI);
244 | 				if (x_move < 0.0 && angle > 0.0)
245 | 					angle -= 180.0;
246 | 				else if (x_move < 0.0 && angle <= 0.0)
247 | 					angle += 180.0;
248 | 				if (abs(angle-prev_angle) > 180.0) {
249 | 					if (prev_angle > 0.0)
250 | 						prev_angle -= 360.0;
251 | 					else
252 | 						prev_angle += 360.0;
253 | 				}
254 | 				a_cost += abs(angle-prev_angle);
255 | 				if (printout) {
256 | 					cout << "  Position: " << p.pose.position.x << " " << p.pose.position.y << " " << endl;
257 | 					cout << "   Move (x, y, theta, theta change): " << x_move*100 << ", " << y_move*100 << ", " << angle << ", " << (angle-prev_angle) << "  " << a_cost << endl;
258 | 					cout << "    Costs:" << cost << ", " << a_cost << endl;
259 | 				}
260 | 				prev_x = p.pose.position.x;
261 | 				prev_y = p.pose.position.y;
262 | 				prev_angle = angle;
263 | 			}
264 | 		}
265 | 		else {
266 | 			ROS_WARN("Empty plan");
267 | 		}
268 | 	}
269 | 	else {
270 | 		ROS_ERROR("Failed to call service %s - is the robot moving?", serviceClient.getService().c_str());
271 | 	}
272 | 	a_cost += abs(prev_angle);	// Add angular cost of rotation back to zero degrees
273 | 	a_cost *= (M_PI/180.0);
274 | 	cost = (cost/SPEED) + (a_cost/ANGULAR);
275 | 	if (printout)
276 | 		cout << "Final cost: " << cost << endl;
277 | 	return cost;
278 | }
279 | 
280 | void setLocations()
281 | {
282 | /*
283 | 	// USED FOR FLOORPLAN
284 | 	robot_locations[0][0] = 16.0;	// x coord
285 | 	robot_locations[0][1] = 10.0;	// y coord
286 | 	robot_locations[1][0] = 9.5;
287 | 	robot_locations[1][1] = 15.5;
288 | 	robot_locations[2][0] = 18.0;
289 | 	robot_locations[2][1] = 18.0;
290 | 
291 | 	target_locations[0][0] = 28.0;
292 | 	target_locations[0][1] = 15.0;
293 | 	target_locations[1][0] = 14.5;
294 | 	target_locations[1][1] = 10.0;
295 | 	target_locations[2][0] = 16.5;
296 | 	target_locations[2][1] = 16.5;
297 | 	target_locations[3][0] = 7.0;
298 | 	target_locations[3][1] = 18.0;
299 | 	target_locations[4][0] = 15.5;
300 | 	target_locations[4][1] = 10.0;
301 | 	target_locations[5][0] = 10.0;
302 | 	target_locations[5][1] = 12.0;
303 | 	target_locations[6][0] = 22.0;
304 | 	target_locations[6][1] = 17.0;
305 | 	target_locations[7][0] = 28.0;
306 | 	target_locations[7][1] = 11.0;
307 | */
308 | /*
309 | 	// USED FOR MAZE
310 | 	robot_locations[0][0] = 6.5;	// x coord
311 | 	robot_locations[0][1] = 36.0;	// y coord
312 | 	robot_locations[1][0] = 6.5;
313 | 	robot_locations[1][1] = 51.0;
314 | 
315 | 	target_locations[0][0] = 4.0;
316 | 	target_locations[0][1] = 48.0;
317 | 	target_locations[1][0] = 4.0;
318 | 	target_locations[1][1] = 51.0;
319 | 	target_locations[2][0] = 9.0;
320 | 	target_locations[2][1] = 36.0;
321 | 	target_locations[3][0] = 9.0;
322 | 	target_locations[3][1] = 45.0;
323 | */
324 | 
325 | 	// RANDOM
326 | 	// srand (time(NULL));
327 | 	srand(2);
328 | 	for (int robot = 0; robot < ROBOTS; ++robot) {
329 | 		robot_locations[robot][0] = static_cast <float> (rand()) / (static_cast <float> (RAND_MAX/50.0));	
330 | 		robot_locations[robot][1] = static_cast <float> (rand()) / (static_cast <float> (RAND_MAX/50.0));
331 | 		cout << robot <<" loc (x,y) " << robot_locations[robot][0] << " " << robot_locations[robot][1] << endl;
332 | 	}
333 | 
334 | 	for (int target = 0; target < TARGETS; ++target) {
335 | 		target_locations[target][0] = static_cast <float> (rand()) / (static_cast <float> (RAND_MAX/50.0));
336 | 		target_locations[target][1] = static_cast <float> (rand()) / (static_cast <float> (RAND_MAX/50.0));
337 | 		cout << target << " tgt (x,y) " << target_locations[target][0] << " " << target_locations[target][1] << endl;
338 | 	}
339 | 
340 | }
341 | 
342 | 
343 | string paramString(int robot, string param) {
344 | 	return "/robot_" + boost::lexical_cast<std::string>(robot) + "/"+ param;
345 | }
346 | 
347 | 
348 | 
349 | 
350 | 


--------------------------------------------------------------------------------
/send_goal.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include <move_base_msgs/MoveBaseAction.h>
 3 | #include <actionlib/client/simple_action_client.h>
 4 | #include <tf/transform_datatypes.h>
 5 | 
 6 | typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
 7 | 
 8 | using namespace std;
 9 | 
10 | int main(int argc, char** argv) {
11 | 
12 | 	char *robot_id = argv[1];
13 | 
14 | 	ros::init(argc, argv, "send_goals");
15 | 	ros::NodeHandle nh;
16 | 
17 | 	double goal_x, goal_y, goal_theta;
18 | 	string control;
19 | 
20 | 	// Create the string "robot_X/move_base"
21 | 	string move_base_str = "/robot_";
22 | 	move_base_str += robot_id;
23 | 	move_base_str += "/move_base";
24 | 
25 | 	// create the action client
26 | 	MoveBaseClient ac(move_base_str, true);
27 | 
28 | 	// Wait for the action server to become available
29 | 	ROS_INFO("Waiting for the move_base action server");
30 | 	ac.waitForServer(ros::Duration(5));
31 | 	ROS_INFO("Connected to move base server");
32 | 
33 | 	// Check until control paramater is set to "targets"
34 | 	nh.getParam("control", control);
35 | 	while (control.compare("notargets") == 0) {
36 | 		ros::Duration(0.5).sleep();
37 | 		nh.getParam("control", control);
38 | 	}
39 | 
40 | 	// Loop to visit the goals
41 | 	int visited = 0;
42 | 	ros::Time begin = ros::Time::now();
43 | 	while (control.compare("targets") == 0) {
44 | 		if (!nh.getParam("goal_x", goal_x) || !nh.getParam("goal_y", goal_y) || !nh.getParam("goal_theta", goal_theta))
45 | 		    cout << " Error receiving goal for robot " << robot_id << endl;
46 | 	
47 | 		// Send a goal to move_base
48 | 		move_base_msgs::MoveBaseGoal goal;
49 | 		goal.target_pose.header.frame_id = "map";
50 | 		goal.target_pose.header.stamp = ros::Time::now();
51 | 
52 | 		goal.target_pose.pose.position.x = goal_x;
53 | 		goal.target_pose.pose.position.y = goal_y;
54 | 
55 | 		double radians = goal_theta * (M_PI/180);
56 | 		tf::Quaternion quaternion;
57 | 		quaternion = tf::createQuaternionFromYaw(radians);
58 | 		geometry_msgs::Quaternion qMsg;
59 | 		tf::quaternionTFToMsg(quaternion, qMsg);
60 | 		goal.target_pose.pose.orientation = qMsg;
61 | 
62 | 		ROS_INFO("Sending goal to robot no. %s: x = %f, y = %f", robot_id, goal_x, goal_y);
63 | 		ac.sendGoal(goal);
64 | 		ac.waitForResult();
65 | 		ros::Duration elapsed = ros::Time::now() - begin;
66 | 
67 | 		if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
68 | 			ROS_INFO("Robot %s has reached the goal at time %f", robot_id, elapsed.toSec());
69 | 		else
70 | 			ROS_INFO("The base of robot %s failed for some reason", robot_id);
71 | 		nh.setParam("status", ++visited);	// tell auction node to send new goal if any
72 | 		ros::Duration(1.0).sleep(); 		// allow new goal coordinates update
73 | 		nh.getParam("control", control);
74 | 	}
75 | 	return 0;
76 | }
77 | 
78 | 
79 | 
80 | 


--------------------------------------------------------------------------------