Project

General

Profile

Revision 7ac5e9bc

ID7ac5e9bc2e048962b82aab3a995270bab56d816a

Added by Priya over 6 years ago

ROS scheduler/whrobot behavior needs to be debugged

View differences:

scout/libscout/src/behaviors/Scheduler.cpp
6 6
/** @Brief: Initialize data structures for the scheduler. */
7 7
Scheduler::Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler")
8 8
{
9

  
10
  ROS_INFO("Scheduler: Creating order PQ...");
11

  
12 9
  unassignedOrders = new PQWrapper(NUM_TASKS);
13 10

  
14
  ROS_INFO("Scheduler: PQ orders initialized. ");
15

  
16
  sched_to_robot = n.advertise("sched_to_robot", 1000);
11
  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
17 12

  
18 13
	create_orders();
19
  ROS_INFO("Scheduler: I am initialized!");
20 14
}
21 15

  
22 16
/** @Brief: Free allocatetd memory. */
23 17
Scheduler::~Scheduler()
24 18
{
25
  while(robots.size() != 0)
26
    delete robots.at(0);
27
   
28 19
  delete unassignedOrders;
29 20
}
30 21

  
......
120 111

  
121 112
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
122 113
{
114
  ROS_INFO("I got a message: %s", msg->data.c_str());
123 115
  if(msg->data.compare(0, 6, "FAILED") == 0)
124 116
  {
125 117
    int order_id = atoi(msg->data.substr(7).c_str());
126
    for(int i=0; i<assignedOrders.size(); i++)
118
    for(unsigned int i=0; i<assignedOrders.size(); i++)
127 119
    {
128
      if(assignedOrders[i].id == order_id)
120
      if(assignedOrders[i].getid() == order_id)
129 121
      {
130 122
        task_failed(assignedOrders[i]);
131 123
        break;
......
135 127
  else if(msg->data.compare(0, 7, "SUCCESS") == 0)
136 128
  {
137 129
    int order_id = atoi(msg->data.substr(8).c_str());
138
    for(int i=0; i<assignedOrders.size(); i++)
130
    for(unsigned int i=0; i<assignedOrders.size(); i++)
139 131
    {
140
      if(assignedOrders[i].id == order_id)
132
      if(assignedOrders[i].getid() == order_id)
141 133
      {
142 134
        task_complete(assignedOrders[i]);
143 135
        break;
......
151 143
  }
152 144
  else if(msg->data.compare(0, 8, "REGISTER") == 0)
153 145
  {
146
    string robot_name = msg->data.substr(9);
147
    for(unsigned int i=0; i<robots.size(); i++)
148
    {
149
      if(robots[i].name.compare(robot_name) == 0)
150
        return;
151
    }
152

  
154 153
    int id = robots.size() +1;
155 154
    Robot new_robot;
156
    new_robot.name = msg->data.substr(9);
157
    new_robot.topic = n.subscribe(new_robot.name + "_topic", 1000, msg_callback);
155
    new_robot.name = robot_name;
156
    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
158 157
    robots.push_back(new_robot);
159 158

  
159
    ROS_INFO("Registration a robot %s", new_robot.name.c_str());
160 160
    std_msgs::String msg;
161 161
    std::stringstream ss;
162
    ss<<"REG_SUCCESS";
162
    ss<<"REG_SUCCESS "<<id;
163 163
    msg.data = ss.str();
164
    sched_to_robot.publish(msg);
165

  
164
    new_robot.topic.publish(msg);
165
    ROS_INFO("Registration a success");
166 166
  }
167 167
  else
168 168
  {
169
    ROS_INFO("I got a bad message: %s", msg->data);
169
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
170 170
  }
171 171

  
172 172
  return;
......
176 176
    this function calls the waiting_dance() function. */
177 177
void Scheduler::run()
178 178
{
179
  while(robots.size() < 3);
179
  ROS_INFO("I am a scheduler!. Now waiting for robots");
180
  while(robots.size() < 3)
181
    spinOnce();
180 182

  
181 183
	while (ok())
182 184
	{
183 185
    ROS_INFO("Scheduler running");
186
    spinOnce();
184 187
		while(waitingRobots.empty() || unassignedOrders->arraySize()==0) 
185 188
      waiting_dance();
186 189

  
187 190

  
188 191
    Order next = get_next_item();
192
    Robot r = waitingRobots.front();
193

  
189 194
    std_msgs::String msg;
190 195
    std::stringstream ss;
191
    ss<<"SET_TASK "<<next.id<<" "<<next.get_source()<<" "<<next.get_dest;
196
    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
192 197
    msg.data = ss.str();
193
    sched_to_robot.publish(msg);
198
    r.topic.publish(msg);
194 199

  
195 200
		waitingRobots.pop();
196 201
	}

Also available in: Unified diff