Project

General

Profile

Revision 5d0687a9

ID5d0687a9991d806c94fc7c21405334e6717e3611

Added by Priya about 7 years ago

Small changes to wireless, and starting turning with line following.

View differences:

scout/libscout/src/behaviors/Scheduler.cpp
45 45
  p.path = NULL;
46 46
  Time t = ros::Time::now();
47 47
  Duration end(0);
48
	Order a(1,0,1,t,p,end);
49
  Order b(2,0,1,t,p,end);
50
  Order c(3,0,9,t,p,end);
48
  Duration five(5);
49
  Duration three(3);
50
  Duration ten(10);
51
	Order a(1,0,1,t+ten,p,end);
52
  Order b(2,0,1,t+five,p,end);
53
  Order c(3,0,9,t+three,p,end);
51 54
  Order d(4,0,2,t,p,end);
52 55
  Order e(5,0,4,t,p,end);
53 56
	
......
94 97
  double time = t.toSec();
95 98

  
96 99
  Order* best;
100
  int best_index;
97 101
  for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)
98 102
  {
99 103
    Order order = unassignedOrders->peek(i);
......
103 107
    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function
104 108
    {
105 109
      best = &order;
110
      best_index = i;
106 111
      time = end_time.toSec() + MAX_WAIT_TIME;
107 112
    }
108 113
  }
109
  return *best;
114

  
115
  Order ret;
116
  ret = unassignedOrders->remove(best_index);
117
  assignedOrders.push_back(ret);
118
  return ret;
110 119
}
111 120

  
112 121
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
......
137 146
  }
138 147
  else if(msg->data.compare(0, 8, "GET_TASK") == 0)
139 148
  {
149
    ROS_INFO("SCHEDULER: got get task message");
140 150
    int robot = atoi(msg->data.substr(9).c_str());
141 151
    get_task(robot);
142 152
  }
......
164 174
    ss<<"REG_SUCCESS "<<id;
165 175
    msg.data = ss.str();
166 176
    new_robot.topic.publish(msg);
167
    ros::spinOnce();
177
    spinOnce();
168 178

  
169 179
    ROS_INFO("Registration a success");
170 180

  
......
194 204
	{
195 205
    ROS_INFO("Scheduler running");
196 206
    spinOnce();
197
		while(waitingRobots.empty() || unassignedOrders->arraySize()==0) 
207
		while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())
208
    {
198 209
      waiting_dance();
210
      spinOnce();
211
    }
199 212
  
200 213
    ROS_INFO("SCHEDULER: Setting task");
201 214
    Order next = get_next_item();
......
207 220
    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
208 221
    msg.data = ss.str();
209 222
    r.topic.publish(msg);
223
  
224
    spinOnce();
210 225

  
211 226
    waitingRobots.front().sched_status = ORDERED_ROBOT;
212 227
		waitingRobots.pop();

Also available in: Unified diff