/ - Diff - Colony Scout - Robotics Club

scout/libscout/src/Behavior.h
38	38	#define _BEHAVIOR_H_
39	39
40	40	#include <ros/ros.h>
	41	#include <std_msgs/String.h>
41	42
42	43	#include "MotorControl.h"
43	44	#include "HeadlightControl.h"

       behavior_list.push_back((Behavior*)new draw_ccw_circle(scoutname));
       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 sim_line(scoutname));
       behavior_list.push_back((Behavior*)new Scheduler(scoutname));
       behavior_list.push_back((Behavior*)new WH_Robot(scoutname));
       return;

     /** @Brief: Initialize data structures for the scheduler. */
     Scheduler::Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler")
+    {
       ROS_INFO("Scheduler: Creating order PQ...");
       unassignedOrders = new PQWrapper(NUM_TASKS);
       ROS_INFO("Scheduler: PQ orders initialized. ");
       sched_to_robot = n.advertise("sched_to_robot", 1000);
       robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
     	create_orders();
       ROS_INFO("Scheduler: I am initialized!");
+    }
     /** @Brief: Free allocatetd memory. */
     Scheduler::~Scheduler()
+    {
       while(robots.size() != 0)
         delete robots.at(0);
       delete unassignedOrders;
+    }
-...
     void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
+    {
       ROS_INFO("I got a message: %s", msg->data.c_str());
       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++)
         for(unsigned int i=0; i<assignedOrders.size(); i++)
+        {
           if(assignedOrders[i].id == order_id)
           if(assignedOrders[i].getid() == 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++)
         for(unsigned int i=0; i<assignedOrders.size(); i++)
+        {
           if(assignedOrders[i].id == order_id)
           if(assignedOrders[i].getid() == order_id)
+          {
             task_complete(assignedOrders[i]);
             break;
-...
+      }
       else if(msg->data.compare(0, 8, "REGISTER") == 0)
+      {
         string robot_name = msg->data.substr(9);
         for(unsigned int i=0; i<robots.size(); i++)
+        {
           if(robots[i].name.compare(robot_name) == 0)
             return;
+        }
         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);
         new_robot.name = robot_name;
         new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
         robots.push_back(new_robot);
         ROS_INFO("Registration a robot %s", new_robot.name.c_str());
         std_msgs::String msg;
         std::stringstream ss;
         ss<<"REG_SUCCESS";
         ss<<"REG_SUCCESS "<<id;
         msg.data = ss.str();
         sched_to_robot.publish(msg);
         new_robot.topic.publish(msg);
         ROS_INFO("Registration a success");
+      }
       else
+      {
         ROS_INFO("I got a bad message: %s", msg->data);
         ROS_INFO("I got a bad message: %s", msg->data.c_str());
+      }
       return;
-...
         this function calls the waiting_dance() function. */
     void Scheduler::run()
+    {
       while(robots.size() < 3);
       ROS_INFO("I am a scheduler!. Now waiting for robots");
       while(robots.size() < 3)
         spinOnce();
     	while (ok())
+    	{
         ROS_INFO("Scheduler running");
         spinOnce();
     		while(waitingRobots.empty() || unassignedOrders->arraySize()==0)
           waiting_dance();
         Order next = get_next_item();
         Robot r = waitingRobots.front();
         std_msgs::String msg;
         std::stringstream ss;
         ss<<"SET_TASK "<<next.id<<" "<<next.get_source()<<" "<<next.get_dest;
         ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
         msg.data = ss.str();
         sched_to_robot.publish(msg);
         r.topic.publish(msg);
     		waitingRobots.pop();
+    	}

     typedef struct{
       std::string name;
       ros::Subscriber topic;
       ros::Publisher topic;
     } Robot;
     class Scheduler : Behavior {
-...
     	void run();
       ros::Publisher sched_to_robot;
       ros::Subscriber robot_to_sched;
     };
     #endif

     /** @Brief: warehouse robot constructor **/
     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;
         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");
         robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000);
         sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this);
         ROS_INFO("A WH_Robot has been created");
+    }
     WH_Robot::~WH_Robot()
-...
+        }
+    }
     void WH_Robot::msg_callback(const std_msgs::String::ConstPtr& msg)
     void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
+    {
       if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
+      {
         id = atoi(msg->data.substr(12).c_str());
         reg_failed = 0;
+      }
       else if(msg->data.compare(0, 8, "SET_TASK") == 0)
+      {
         char* string = msg->data.c_str();
         char* string = (char*)msg->data.c_str();
         char* data;
         data = strtok(string, " ");
         int id = atoi(data);
         int order_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;
         Path path;
         path.len = 0;
         path.path = NULL;
         Duration est_time(0);
         curr_task = new Order(id, source, dest, start_time, path, est_time);
         curr_task = new Order(order_id, source, dest, start_time, path, est_time);
+      }
       else
+      {
         ROS_INFO("I got a bad message: %s", msg->data);
         ROS_INFO("I got a bad message: %s", msg->data.c_str());
+      }
+    }
     void WH_Robot::run ()
+    {
         ROS_INFO("I am a robot. Registering with scheduler...");
         while(reg_failed)
+        {
           std_msgs::String msg;
           std::stringstream ss;
           ss << "REGISTER "<<name;
           msg.data = ss.str();
           robot_to_sched.publish(msg);
           spinOnce();
+        }
         while(ok())
+        {
-...
             msg.data = ss.str();
             robot_to_sched.publish(msg);
             spinOnce();
             while(curr_task == DEFAULT_TASK)
               continue;
-...
+            {
             ss << "SUCCESS "<<curr_task->id;
             ss << "SUCCESS "<<curr_task->getid();
             msg.data = ss.str();
             robot_to_sched.publish(msg);
-...
+            {
             ss << "FAILED "<<curr_task->id;
             ss << "FAILED "<<curr_task->getid();
             msg.data = ss.str();
             robot_to_sched.publish(msg);

             Duration get_worst_case_time(State start_state, State target_state);
             int exec_task();
             void msg_callback(const std_msgs::String::ConstPtr& msg);
             void robot_callback(const std_msgs::String::ConstPtr& msg);
         public:
             WH_Robot(std::string scoutname);

ID	7ac5e9bc2e048962b82aab3a995270bab56d816a
Parent	5bd763bc
Child	ebb19fe0

Project

General

Profile

Colony » Colony Scout

Revision 7ac5e9bc