/scout/libscout/src/behaviors/Scheduler.cpp - Diff - Colony Scout - Robotics Club

Revision 3f72678f

ID	3f72678f171cbcb5499524f6cbfe994f4a416364
Parent	e7b4f56d
Child	fa8e76a4

Added by Priya about 12 years ago

Changing scheduler and WH robot to ROS messages

     /** @Brief: Initialize data structures for the scheduler. */
     Scheduler::Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler")
+    {
     	WH_Robot* rob1 = new WH_Robot("Robot1", this);
     	WH_Robot* rob2 = new WH_Robot("Robot2", this);
     	WH_Robot* rob3 = new WH_Robot("Robot3", this);
       ROS_INFO("Scheduler: Creating order PQ...");
       unassignedOrders = new PQWrapper(NUM_TASKS);
     	robots.push_back(rob1);
     	robots.push_back(rob2);
     	robots.push_back(rob3);
       ROS_INFO("Scheduler: PQ orders initialized. ");
       sched_to_robot = n.advertise("sched_to_robot", 1000);
     	create_orders();
       ROS_INFO("Scheduler: I am initialized!");
+    }
     /** @Brief: Free allocatetd memory. */
     Scheduler::~Scheduler()
+    {
        delete robots.at(0);
        delete robots.at(1);
        delete robots.at(2);
       while(robots.size() != 0)
         delete robots.at(0);
        delete unassignedOrders;
       delete unassignedOrders;
+    }
     /** @Brief: Add robot to the waiting queue.
-...
      *  the robot of the waiting queue, and gives it a
      *  task.
      */
     void Scheduler::get_task(WH_Robot* r)
     void Scheduler::get_task(int robot)
+    {
     	waitingRobots.push(r);
     	waitingRobots.push(robots[robot]);
+    }
     /** @Brief: Statically add orders to the Priority Queue Wrapper.*/
     void Scheduler::create_orders()
+    {
       ROS_INFO("Scheduler: Creating orders...");
       Path p;
       p.len = 0;
       p.path = NULL;
-...
       Order d(4,0,2,t,p,end);
       Order e(5,0,4,t,p,end);
       ROS_INFO("Scheduler: Orders created. Adding to list");
     	unassignedOrders->insert(a);
     	unassignedOrders->insert(b);
     	unassignedOrders->insert(c);
     	unassignedOrders->insert(d);
     	unassignedOrders->insert(e);
       ROS_INFO("Scheduler: Orders initialized in list");
+    }
     /** @Brief: This is a confirmation that the task is complete.
         This function removes the order from assignedOrders. */
     void Scheduler::task_complete(Order o)
+    {
         for (unsigned int i=0; i<assignedOrders.size(); i++)
+    	{
     	    if (assignedOrders[i].getid()==o.getid())
+    		{
     			assignedOrders.erase(assignedOrders.begin()+i);
+    		}
+    	}
       ROS_INFO("Scheduler: Task id %d was completed", o.getid());
       for (unsigned int i=0; i<assignedOrders.size(); i++)
+      {
         if (assignedOrders[i].getid()==o.getid())
+        {
           assignedOrders.erase(assignedOrders.begin()+i);
+        }
+      }
+    }
     /** @Brief: This is a confirmation that the task failed.
         This function places the order back on the PQWrapper. */
     void Scheduler::task_failed(Order o)
+    {
       ROS_INFO("Scheduler: Task id %d was failed", o.getid());
     	task_complete(o);
     	unassignedOrders->insert(o);
+    }
     /** @Brief: Do a waiting dance. */
     void Scheduler::waiting_dance()
+    {
+    {
         ROS_INFO("Scheduler: TEEHEE i do a dance!");
+    }
     /** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */
-...
       return *best;
+    }
     void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
+    {
       if(msg->data.compare(0, 6, "FAILED") == 0)
+      {
         int order_id = atoi(msg->data.substr(7).c_str());
         for(int i=0; i<assignedOrders.size(); i++)
+        {
           if(assignedOrders[i].id == order_id)
+          {
             task_failed(assignedOrders[i]);
             break;
+          }
+        }
+      }
       else if(msg->data.compare(0, 7, "SUCCESS") == 0)
+      {
         int order_id = atoi(msg->data.substr(8).c_str());
         for(int i=0; i<assignedOrders.size(); i++)
+        {
           if(assignedOrders[i].id == order_id)
+          {
             task_complete(assignedOrders[i]);
             break;
+          }
+        }
+      }
       else if(msg->data.compare(0, 8, "GET_TASK") == 0)
+      {
         int robot = atoi(msg->data.substr(9).c_str());
         get_task(robot);
+      }
       else if(msg->data.compare(0, 8, "REGISTER") == 0)
+      {
         int id = robots.size() +1;
         Robot new_robot;
         new_robot.name = msg->data.substr(9);
         new_robot.topic = n.subscribe(new_robot.name + "_topic", 1000, msg_callback);
         robots.push_back(new_robot);
         std_msgs::String msg;
         std::stringstream ss;
         ss<<"REG_SUCCESS";
         msg.data = ss.str();
         sched_to_robot.publish(msg);
+      }
       else
+      {
         ROS_INFO("I got a bad message: %s", msg->data);
+      }
       return;
+    }
     /** @Brief: Continuously checks for waiting robots. If no robots are waiting,
         this function calls the waiting_dance() function. */
     void Scheduler::run()
+    {
       while(robots.size() < 3);
     	while (ok())
+    	{
         ROS_INFO("Scheduler running");
     		while(waitingRobots.empty() || unassignedOrders->arraySize()==0)
           waiting_dance();
     		(waitingRobots.front())->set_task(get_next_item());
         Order next = get_next_item();
         std_msgs::String msg;
         std::stringstream ss;
         ss<<"SET_TASK "<<next.id<<" "<<next.get_source()<<" "<<next.get_dest;
         msg.data = ss.str();
         sched_to_robot.publish(msg);
     		waitingRobots.pop();
+    	}
+    }

Also available in: Unified diff

Project

General

Profile

Colony » Colony Scout

Revision 3f72678f