Project

General

Profile

Revision 5d0687a9

ID5d0687a9991d806c94fc7c21405334e6717e3611

Added by Priya about 12 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