Project

General

Profile

Revision ad5b5826

IDad5b58260593f9a00266e17ccdb1aaed3d72460e

Added by Priya almost 12 years ago

some fixes to whrobot and scheduler

View differences:

scout/libscout/src/behaviors/Scheduler.cpp
8 8
{
9 9
  unassignedOrders = new PQWrapper(NUM_TASKS);
10 10

  
11
  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
12

  
13 11
	create_orders();
14 12
}
15 13

  
......
28 26
 */
29 27
void Scheduler::get_task(int robot)
30 28
{
31
	waitingRobots.push(robots[robot-1]);
29
  ROS_INFO("SCHEDULER: robots vector size is: %d", robots.size());
30
  Robot my_robot = robots[robot-1];
31
  ROS_INFO("SCHEDULER: My robot is %s", my_robot.name.c_str());
32
  if(my_robot.sched_status != WAITING_ROBOT)
33
  {
34
	  waitingRobots.push(my_robot);
35
    my_robot.sched_status = WAITING_ROBOT;
36
    ROS_INFO("SCHEDULER: Added to scheduler %d", my_robot.sched_status);
37
  }
32 38
}
33 39

  
34 40
/** @Brief: Statically add orders to the Priority Queue Wrapper.*/
35 41
void Scheduler::create_orders()
36 42
{
37
  ROS_INFO("Scheduler: Creating orders...");
38

  
39 43
  Path p;
40 44
  p.len = 0;
41 45
  p.path = NULL;
......
47 51
  Order d(4,0,2,t,p,end);
48 52
  Order e(5,0,4,t,p,end);
49 53
	
50
  ROS_INFO("Scheduler: Orders created. Adding to list");
51

  
52 54
	unassignedOrders->insert(a);
53 55
	unassignedOrders->insert(b);
54 56
	unassignedOrders->insert(c);
55 57
	unassignedOrders->insert(d);
56 58
	unassignedOrders->insert(e);
57

  
58
  ROS_INFO("Scheduler: Orders initialized in list");
59 59
}
60 60

  
61 61
/** @Brief: This is a confirmation that the task is complete. 
......
84 84
/** @Brief: Do a waiting dance. */
85 85
void Scheduler::waiting_dance()
86 86
{ 
87
    ROS_INFO("Scheduler: TEEHEE i do a dance!");
87
    //ROS_INFO("Scheduler: TEEHEE i do a dance!");
88 88
}
89 89

  
90 90
/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */
......
111 111

  
112 112
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
113 113
{
114
  ROS_INFO("I got a message: %s", msg->data.c_str());
115 114
  if(msg->data.compare(0, 6, "FAILED") == 0)
116 115
  {
117 116
    int order_id = atoi(msg->data.substr(7).c_str());
......
146 145
    string robot_name = msg->data.substr(9);
147 146
    for(unsigned int i=0; i<robots.size(); i++)
148 147
    {
149
      if(robots[i].name.compare(robot_name) == 0){
148
      if(robots[i].name.compare(robot_name) == 0)
149
      { 
150 150
        return;
151 151
      }
152 152
    }
......
155 155
    Robot new_robot;
156 156
    new_robot.name = robot_name;
157 157
    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
158
    new_robot.sched_status = NEW_ROBOT;
158 159
    robots.push_back(new_robot);
159 160

  
160
    ROS_INFO("Registration a robot %s", new_robot.name.c_str());
161
    ROS_INFO("Registration a robot %s %d", new_robot.name.c_str(), robots.size());
161 162
    std_msgs::String msg;
162 163
    std::stringstream ss;
163 164
    ss<<"REG_SUCCESS "<<id;
164 165
    msg.data = ss.str();
165 166
    new_robot.topic.publish(msg);
167
    ros::spinOnce();
168

  
166 169
    ROS_INFO("Registration a success");
170

  
171
    assert(robots.size() > 0);
167 172
  }
168 173
  else
169 174
  {
......
177 182
    this function calls the waiting_dance() function. */
178 183
void Scheduler::run()
179 184
{
185
  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
180 186
  ROS_INFO("I am a scheduler!. Now waiting for robots");
181
  while(robots.size() < 3)
187
  while(robots.size() < 1 && ok())
188
  {
189
    ROS_INFO("Robots size: %d", robots.size());
182 190
    spinOnce();
183

  
191
  }
192
  ROS_INFO("SCHEDULER: main loop");
184 193
	while (ok())
185 194
	{
186 195
    ROS_INFO("Scheduler running");
187 196
    spinOnce();
188 197
		while(waitingRobots.empty() || unassignedOrders->arraySize()==0) 
189 198
      waiting_dance();
190

  
191

  
199
  
200
    ROS_INFO("SCHEDULER: Setting task");
192 201
    Order next = get_next_item();
193 202
    Robot r = waitingRobots.front();
194 203

  
204
    ROS_INFO("SCHEDULER: Setting task %d to robot %s", next.getid(), r.name.c_str());
195 205
    std_msgs::String msg;
196 206
    std::stringstream ss;
197 207
    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
198 208
    msg.data = ss.str();
199 209
    r.topic.publish(msg);
200 210

  
211
    waitingRobots.front().sched_status = ORDERED_ROBOT;
201 212
		waitingRobots.pop();
202 213
	}
203 214
}
scout/libscout/src/behaviors/Scheduler.h
6 6
#include "../Behavior.h"
7 7

  
8 8
#define NUM_TASKS 5
9
#define WAITING_ROBOT 1
10
#define NEW_ROBOT 2
11
#define ORDERED_ROBOT 3
9 12

  
10 13
typedef struct{
11 14
  std::string name;
12 15
  ros::Publisher topic;
16
  int sched_status;
13 17
} Robot;
14 18

  
15 19
class Scheduler : Behavior {
16 20
  std::vector<Robot> robots;
21
	std::queue<Robot> waitingRobots;
22

  
17 23
  PQWrapper* unassignedOrders;
18 24
	std::vector<Order> assignedOrders;
19
	std::queue<Robot> waitingRobots;
20 25

  
21 26
	void create_orders();
22 27

  
scout/libscout/src/behaviors/WH_Robot.cpp
4 4
/** @Brief: warehouse robot constructor **/
5 5
WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot")
6 6
{
7
    nav_map = new navigationMap(scoutname);
8
    curr_task = DEFAULT_TASK;
9
    name = scoutname;
10
    reg_failed = 1;
11

  
12
    robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000);
13
    sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this);
14
    ROS_INFO("A WH_Robot has been created");
7
  nav_map = new navigationMap(scoutname);
8
  curr_task = DEFAULT_TASK;
9
  name = scoutname;
10
  reg_failed = 1;
15 11
}
16 12

  
17 13
WH_Robot::~WH_Robot()
18 14
{
19
    delete(nav_map);
15
  delete(nav_map);
20 16
}
21 17

  
22 18
Duration WH_Robot::get_worst_case_time(State start_state, State target_state)
......
26 22

  
27 23
int WH_Robot::exec_task()
28 24
{
29
    ROS_INFO("WH_robot: Executing a task");
30
    assert(curr_task != DEFAULT_TASK);
31
    //TODO: do task
32
    srand(0xDEADBEEF);
33
    int error = rand() % 12;
34
    if(error < 9) //Fail with 1/4 probability
35
    {
36
      ROS_INFO("WH_robot: TASK COMPLETE!");
37
      return TASK_COMPLETED;
38
    }
39
    else
40
    {
41
      ROS_INFO("WH_robot: TASK FAILED!");
42
      return TASK_FAILED;
43
    }
25
  ROS_INFO("WH_robot: Executing a task");
26
  assert(curr_task != DEFAULT_TASK);
27
  //TODO: do task
28
  srand(0xDEADBEEF);
29
  int error = rand() % 12;
30
  if(error < 9) //Fail with 1/4 probability
31
  {
32
    ROS_INFO("WH_robot: TASK COMPLETE!");
33
    return TASK_COMPLETED;
34
  }
35
  else
36
  {
37
    ROS_INFO("WH_robot: TASK FAILED!");
38
    return TASK_FAILED;
39
  }
44 40
}
45 41

  
46 42
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
47 43
{
44
  std::cout << "robot callback called." << std::endl;
48 45
  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
49 46
  {
50 47
    id = atoi(msg->data.substr(12).c_str());
......
52 49
  }
53 50
  else if(msg->data.compare(0, 8, "SET_TASK") == 0)
54 51
  {
52
    ROS_INFO("WH_ROBOT: Setting robot task");
55 53
    char* string = (char*)msg->data.c_str();
56 54
    char* data;
57 55
    data = strtok(string, " ");
......
76 74

  
77 75
void WH_Robot::run ()
78 76
{
79
    ROS_INFO("I am a robot. Registering with scheduler...");
80
    while(reg_failed)
81
    {
77
  ROS_INFO("A WH_Robot has been created");
78

  
79
  robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000);
80
  sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this);
81

  
82
  ROS_INFO("I am a robot. Registering with scheduler...");
83
  while(reg_failed && ok())
84
  {
85
    std_msgs::String msg;
86
    std::stringstream ss;
87
    ss << "REGISTER "<<name;
88
    msg.data = ss.str();
89
    robot_to_sched.publish(msg);
90

  
91
    spinOnce();
92
  }
93

  
94
  while(ok())
95
  {
96
    ROS_INFO("WH_ROBOT: Running main loop");
97

  
98

  
82 99
      std_msgs::String msg;
83 100
      std::stringstream ss;
84
      ss << "REGISTER "<<name;
101
      ss << "GET_TASK "<<id;
85 102
      msg.data = ss.str();
86 103
      robot_to_sched.publish(msg);
87 104

  
105
    while(curr_task == DEFAULT_TASK && ok()){
88 106
      spinOnce();
89 107
    }
90 108

  
91
    while(ok())
109
    int error = exec_task();
110
    if(error == TASK_COMPLETED)
92 111
    {
93
        ROS_INFO("WH_ROBOT: Running main loop");
94

  
95

  
96
        std_msgs::String msg;
97
        std::stringstream ss;
98
        ss << "GET_TASK "<<id;
99
        msg.data = ss.str();
100
        robot_to_sched.publish(msg);
101

  
102
        spinOnce();
103

  
104
        while(curr_task == DEFAULT_TASK){
105
          continue;
106
	}
107
	
108
        int error = exec_task();
109
        if(error == TASK_COMPLETED)
110
        {
111

  
112

  
113
        ss << "SUCCESS "<<curr_task->getid();
114
        msg.data = ss.str();
115
        robot_to_sched.publish(msg);
116

  
117

  
118
        }
119
        else //error == TASK_FAILED
120
        {
121
 
122

  
123
        ss << "FAILED "<<curr_task->getid();
124
        msg.data = ss.str();
125
        robot_to_sched.publish(msg);
126

  
127

  
128
       }
129
        delete curr_task;
130
        curr_task = DEFAULT_TASK;
131

  
112
      std_msgs::String msg;
113
      std::stringstream ss;
114
      ss << "SUCCESS "<<curr_task->getid();
115
      msg.data = ss.str();
116
      robot_to_sched.publish(msg);
132 117
    }
118
    else //error == TASK_FAILED
119
    {
120
      std_msgs::String msg;
121
      std::stringstream ss;
122
      ss << "FAILED "<<curr_task->getid();
123
      msg.data = ss.str();
124
      robot_to_sched.publish(msg);
125
    }
126
    delete curr_task;
127
    curr_task = DEFAULT_TASK;
128

  
129
  }
133 130
}
134 131

  
135 132
void WH_Robot::set_task(Order order)
136 133
{
137
    curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 
138
        order.get_start_time(), order.get_path(), order.get_est_time()); 
139
    return;
134
  curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 
135
      order.get_start_time(), order.get_path(), order.get_est_time()); 
136
  return;
140 137
}
141 138

  

Also available in: Unified diff