/ - Diff - Colony Scout - Robotics Club

       behavior_list.push_back((Behavior*)new Odometry(scoutname));
       behavior_list.push_back((Behavior*)new navigationMap(scoutname));
       behavior_list.push_back((Behavior*)new sim_line(scoutname));
     //  behavior_list.push_back((Behavior*)new Scheduler(scoutname));
       behavior_list.push_back((Behavior*)new Scheduler(scoutname));
       behavior_list.push_back((Behavior*)new WH_Robot(scoutname));
       return;
+    }

scout/libscout/src/BehaviorList.h
8	8	#include "behaviors/Odometry.h"
9	9	#include "behaviors/navigationMap.h"
10	10	#include "behaviors/Scheduler.h"
	11	#include "behaviors/WH_Robot.h"
11	12
12	13	class BehaviorList
13	14	{

     /** @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();
+    	}
+    }

     #include "../helper_classes/PQWrapper.h"
     #include "../helper_classes/Order.h"
     #include "WH_Robot.h"
     #include "../Behavior.h"
     #define NUM_TASKS 5
     typedef struct{
       std::string name;
       ros::Subscriber topic;
     } Robot;
     class Scheduler : Behavior {
       std::vector<WH_Robot*> robots;
       std::vector<Robot> robots;
       PQWrapper* unassignedOrders;
     	std::vector<Order> assignedOrders;
     	std::queue<WH_Robot*> waitingRobots;
     	std::queue<Robot> waitingRobots;
     	void create_orders();
     	void waiting_dance();
       void msg_callback(const std_msgs::String::ConstPtr& msg);
     public:
       Scheduler(std::string scoutname);
     	~Scheduler();
     	void get_task(WH_Robot* robot);
     	void get_task(int robot);
     	void task_complete(Order o);
     	void task_failed(Order o);
-...
     	Order get_next_item();
     	void run();
       ros::Publisher sched_to_robot;
     };
     #endif

     #include "WH_Robot.h"
     #include "Scheduler.h"
     #include "../helper_classes/Order.h"
     /** @Brief: warehouse robot constructor **/
     WH_Robot::WH_Robot(std::string scoutname, void* sched):Behavior(scoutname, "WH_Robot")
     WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot")
+    {
         ROS_INFO("WH_ROBOT: Creating a WH_Robot...");
         nav_map = new navigationMap(scoutname);
         curr_task = DEFAULT_TASK;
         scheduler = sched;
         name = scoutname;
         reg_failed = 1;
         robot_to_sched = n.advertise<std_msgs::String>(name + "_topic", 1000);
         sched_to_robot = n.subscribe("sched_to_robot", 1000, msg_callback);
         while(reg_failed)
+        {
           std_msgs::String msg;
           std::stringstream ss;
           ss << "REGISTER "<<name;
           msg.data = ss.str();
           robot_to_sched.publish(msg);
+        }
         ROS_INFO("WH_Robot: I am a created WH robot");
+    }
     WH_Robot::~WH_Robot()
-...
     int WH_Robot::exec_task()
+    {
         ROS_INFO("WH_robot: Executing a task");
         assert(curr_task != DEFAULT_TASK);
         //TODO: do task
         srand(0xDEADBEEF);
         int error = rand() % 12;
         if(error < 9) //Fail with 1/4 probability
+        {
           ROS_INFO("WH_robot: TASK COMPLETE!");
           return TASK_COMPLETED;
+        }
         else
+        {
           ROS_INFO("WH_robot: TASK FAILED!");
           return TASK_FAILED;
+        }
+    }
     void WH_Robot::run (){
         ((Scheduler*)scheduler)->get_task(this);
         while(curr_task == DEFAULT_TASK)
           continue;
         int error = exec_task();
         if(error == TASK_COMPLETED)
+        {
             ((Scheduler*)scheduler)->task_complete(*curr_task);
+        }
         else //error == TASK_FAILED
     void WH_Robot::msg_callback(const std_msgs::String::ConstPtr& msg)
+    {
       if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
+      {
         reg_failed = 0;
+      }
       else if(msg->data.compare(0, 8, "SET_TASK") == 0)
+      {
         char* string = msg->data.c_str();
         char* data;
         data = strtok(string, " ");
         int id = atoi(data);
         data = strtok(string, " ");
         Address source = atoi(data);
         data = strtok(string, " ");
         Address dest = atoi(data);
         Time start_time = ros::Time::now();
         Path path = NULL;
         Duration est_time(0);
         curr_task = new Order(id, source, dest, start_time, path, est_time);
+      }
       else
+      {
         ROS_INFO("I got a bad message: %s", msg->data);
+      }
+    }
     void WH_Robot::run ()
+    {
         while(ok())
+        {
             ((Scheduler*)scheduler)->task_failed(*curr_task);
             ROS_INFO("WH_ROBOT: Running main loop");
             std_msgs::String msg;
             std::stringstream ss;
             ss << "GET_TASK "<<id;
             msg.data = ss.str();
             robot_to_sched.publish(msg);
             while(curr_task == DEFAULT_TASK)
               continue;
             int error = exec_task();
             if(error == TASK_COMPLETED)
+            {
             ss << "SUCCESS "<<curr_task->id;
             msg.data = ss.str();
             robot_to_sched.publish(msg);
+            }
             else //error == TASK_FAILED
+            {
             ss << "FAILED "<<curr_task->id;
             msg.data = ss.str();
             robot_to_sched.publish(msg);
+           }
             delete curr_task;
             curr_task = DEFAULT_TASK;
+        }
         curr_task = DEFAULT_TASK;
+    }
     void WH_Robot::set_task(Order order)

     #include <stdlib.h>
     class WH_Robot : Behavior{
             std::string name;
             int id;
             int reg_failed;
             Order* curr_task;
             navigationMap* nav_map;
             void* scheduler;
             Duration get_worst_case_time(State start_state, State target_state);
             int exec_task();
             void msg_callback(const std_msgs::String::ConstPtr& msg);
         public:
             WH_Robot(std::string scoutname, void* sched);
             WH_Robot(std::string scoutname);
             ~WH_Robot();
             void run();
             void set_task(Order order);
             ros::Publisher robot_to_sched;
             ros::Subscriber sched_to_robot;
     };
     #endif

ID	3f72678f171cbcb5499524f6cbfe994f4a416364
Parent	e7b4f56d
Child	fa8e76a4

Project

General

Profile

Colony » Colony Scout

Revision 3f72678f