Project

General

Profile

Revision 7ac5e9bc

ID7ac5e9bc2e048962b82aab3a995270bab56d816a

Added by Priya about 6 years ago

ROS scheduler/whrobot behavior needs to be debugged

View differences:

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"
scout/libscout/src/BehaviorList.cpp
6 6
  behavior_list.push_back((Behavior*)new draw_ccw_circle(scoutname));
7 7
  behavior_list.push_back((Behavior*)new Odometry(scoutname));
8 8
  behavior_list.push_back((Behavior*)new navigationMap(scoutname));
9
  behavior_list.push_back((Behavior*)new sim_line(scoutname));
9
  //behavior_list.push_back((Behavior*)new sim_line(scoutname));
10 10
  behavior_list.push_back((Behavior*)new Scheduler(scoutname));
11 11
  behavior_list.push_back((Behavior*)new WH_Robot(scoutname));
12 12
  return;
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
	}
scout/libscout/src/behaviors/Scheduler.h
9 9

  
10 10
typedef struct{
11 11
  std::string name;
12
  ros::Subscriber topic;
12
  ros::Publisher topic;
13 13
} Robot;
14 14

  
15 15
class Scheduler : Behavior {
......
38 38
	
39 39
	void run();
40 40

  
41
  ros::Publisher sched_to_robot;
41
  ros::Subscriber robot_to_sched;
42 42
    
43 43
};
44 44
#endif
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
    ROS_INFO("WH_ROBOT: Creating a WH_Robot...");
8 7
    nav_map = new navigationMap(scoutname);
9 8
    curr_task = DEFAULT_TASK;
10 9
    name = scoutname;
11 10
    reg_failed = 1;
12 11

  
13
    robot_to_sched = n.advertise<std_msgs::String>(name + "_topic", 1000);
14
    sched_to_robot = n.subscribe("sched_to_robot", 1000, msg_callback);
15

  
16
    while(reg_failed)
17
    {
18
      std_msgs::String msg;
19
      std::stringstream ss;
20
      ss << "REGISTER "<<name;
21
      msg.data = ss.str();
22
      robot_to_sched.publish(msg);
23
    }
24

  
25
    ROS_INFO("WH_Robot: I am a created WH robot");
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");
26 15
}
27 16

  
28 17
WH_Robot::~WH_Robot()
......
54 43
    }
55 44
}
56 45

  
57
void WH_Robot::msg_callback(const std_msgs::String::ConstPtr& msg)
46
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
58 47
{
59 48
  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
60 49
  {
50
    id = atoi(msg->data.substr(12).c_str());
61 51
    reg_failed = 0;
62 52
  }
63 53
  else if(msg->data.compare(0, 8, "SET_TASK") == 0)
64 54
  {
65
    char* string = msg->data.c_str();
55
    char* string = (char*)msg->data.c_str();
66 56
    char* data;
67 57
    data = strtok(string, " ");
68
    int id = atoi(data);
58
    int order_id = atoi(data);
69 59
    data = strtok(string, " ");
70 60
    Address source = atoi(data);
71 61
    data = strtok(string, " ");
72 62
    Address dest = atoi(data);
73 63
    Time start_time = ros::Time::now();
74
    Path path = NULL;
64
    Path path;
65
    path.len = 0;
66
    path.path = NULL;
75 67
    Duration est_time(0);
76 68

  
77
    curr_task = new Order(id, source, dest, start_time, path, est_time);
69
    curr_task = new Order(order_id, source, dest, start_time, path, est_time);
78 70
  }
79 71
  else
80 72
  {
81
    ROS_INFO("I got a bad message: %s", msg->data);
73
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
82 74
  }
83 75
}
84 76

  
85 77
void WH_Robot::run ()
86 78
{
79
    ROS_INFO("I am a robot. Registering with scheduler...");
80
    while(reg_failed)
81
    {
82
      std_msgs::String msg;
83
      std::stringstream ss;
84
      ss << "REGISTER "<<name;
85
      msg.data = ss.str();
86
      robot_to_sched.publish(msg);
87

  
88
      spinOnce();
89
    }
87 90

  
88 91
    while(ok())
89 92
    {
......
96 99
        msg.data = ss.str();
97 100
        robot_to_sched.publish(msg);
98 101

  
102
        spinOnce();
99 103

  
100 104
        while(curr_task == DEFAULT_TASK)
101 105
          continue;
......
104 108
        {
105 109

  
106 110

  
107
        ss << "SUCCESS "<<curr_task->id;
111
        ss << "SUCCESS "<<curr_task->getid();
108 112
        msg.data = ss.str();
109 113
        robot_to_sched.publish(msg);
110 114

  
......
114 118
        {
115 119
 
116 120

  
117
        ss << "FAILED "<<curr_task->id;
121
        ss << "FAILED "<<curr_task->getid();
118 122
        msg.data = ss.str();
119 123
        robot_to_sched.publish(msg);
120 124

  
scout/libscout/src/behaviors/WH_Robot.h
23 23
        Duration get_worst_case_time(State start_state, State target_state);
24 24
        int exec_task();
25 25

  
26
        void msg_callback(const std_msgs::String::ConstPtr& msg);
26
        void robot_callback(const std_msgs::String::ConstPtr& msg);
27 27

  
28 28
    public:
29 29
        WH_Robot(std::string scoutname);

Also available in: Unified diff