Project

General

Profile

Revision 58c19c15

ID58c19c15b13ea81473e724302b2c64e055aaac82
Parent afa9104d
Child da08d580, 11aa087a

Added by Priya almost 12 years ago

better warehouse image (corresponds with lines now) and demo. Now only thing left is to implement home behavior.

View differences:

scout/libscout/src/behaviors/Scheduler.cpp
45 45
  Duration five(5);
46 46
  Duration three(3);
47 47
  Duration ten(10);
48
	Order a(1,0,2,t+ten,p,end);
49
  Order b(2,0,4,t+five,p,end);
50
  Order c(3,0,5,t+three,p,end);
51
  Order d(4,0,6,t,p,end);
52
  Order e(5,0,7,t,p,end);
48
	Order a(1,0,13,t+ten,p,end);
49
  Order b(2,0,14,t+five,p,end);
50
  Order c(3,0,15,t+three,p,end);
51
  Order d(4,0,16,t,p,end);
53 52
	
54 53
	unassignedOrders->insert(a);
55 54
	unassignedOrders->insert(b);
56 55
	unassignedOrders->insert(c);
57 56
	unassignedOrders->insert(d);
58
	unassignedOrders->insert(e);
59 57
}
60 58

  
61 59
/** @Brief: This is a confirmation that the task is complete. 
scout/libscout/src/behaviors/WH_Robot.cpp
8 8
  curr_task = DEFAULT_TASK;
9 9
  name = scoutname;
10 10
  reg_failed = 1;
11
  srand(0xDEADBEEF);
11 12
}
12 13

  
13 14
WH_Robot::~WH_Robot()
......
29 30
  for(int i=0; i<path_to_follow.len; i++)
30 31
  {
31 32
    Turn t = path_to_follow.path[i];
33
    unsigned int curr_state = nav_map->get_state();
34

  
32 35
    ROS_INFO("making turn %d", t);
36

  
33 37
    nav_map->update_state(t);
34 38
    switch(t)
35 39
    {
......
42 46
        follow_line();
43 47
        break;
44 48
      case IRIGHT:
49
        if(9 <= curr_state && curr_state <= 12)
50
        {
51
          turn_straight();
52
          follow_line();
53
        }
45 54
        turn_right();
46 55
        follow_line();
56
        if(curr_state <= 4)
57
        {
58
          turn_straight();
59
          follow_line();
60
        }
47 61
        break;
48 62
      case IUTURN:
49 63
        u_turn();
50 64
        follow_line();
51 65
        break;
66
      case ISPOTTURN:
67
        spot_turn();
68
        follow_line();
69
        break;
52 70
    }
53 71
  }  
54 72
}
55 73

  
56 74
int WH_Robot::exec_task()
57 75
{
58
  ROS_INFO("WH_robot: Executing a task");
59 76
  assert(curr_task != DEFAULT_TASK);
60 77
  
61 78
  // remember where you started doing this task so we know where to return
62 79
  State home_state = nav_map->get_state();
63
  ROS_INFO("WH_robot: starting from home state %d", (int)home_state);
64
  ROS_INFO("WH_robot: aiming for state %d", (int)curr_task->get_dest());
65 80
  Path new_path = nav_map->shortest_path(curr_task->get_dest());
66 81
  curr_task->set_path(new_path);
67 82

  
......
71 86

  
72 87
  int did_task_complete;
73 88

  
74
  srand(0xDEADBEEF);
75 89
  int error = rand() % 12;
76
  if(error < 6) //Fail with 1/2 probability
90
  if(error < 9) //Fail with 1/4 probability
77 91
  {
78 92
    ROS_INFO("WH_robot: TASK COMPLETE!");
79 93
    did_task_complete = TASK_COMPLETED;
......
84 98
    did_task_complete = TASK_FAILED;
85 99
  }
86 100

  
87
  ROS_INFO("WH_robot: Now I am attempting to go home from state %d to state %d", 
88
        (int)nav_map->get_state(), (int)home_state);
89

  
90 101
  Path return_path = nav_map->shortest_path(home_state);
91 102
  curr_task->set_path(return_path);
92 103
  
......
97 108

  
98 109
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
99 110
{
100
  std::cout << "robot callback called." << std::endl;
101 111
  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
102 112
  {
103 113
    id = atoi(msg->data.substr(12).c_str());
......
105 115
  }
106 116
  else if(msg->data.compare(0, 8, "SET_TASK") == 0)
107 117
  {
108
    ROS_INFO("WH_ROBOT: Setting robot task");
109 118
    char* string = (char*)msg->data.c_str();
110
    ROS_INFO("Set task string is: %s", string);
111 119
    char* data;
112 120
    data = strtok(string, " ");
121

  
113 122
    data = strtok(NULL, " ");
114 123
    int order_id = atoi(data);
115
    ROS_INFO("order Id: %d with string %s", order_id, data);
116 124

  
117 125
    data = strtok(NULL, " ");
118 126
    Address source = atoi(data);
119
    ROS_INFO("source: %d with string %s", source, data);
120 127

  
121 128
    data = strtok(NULL, " ");
122 129
    Address dest = atoi(data);
123
    ROS_INFO("dest: %d with string %s", dest, data);
124 130

  
125 131
    Time start_time = ros::Time::now();
126 132
    Path path;
scout/libscout/src/behaviors/line_follow.cpp
79 79

  
80 80
    line_loc = linesensor->readline();
81 81
  }
82
  while(!(-.5 < line_loc && line_loc < .5));
82
  while(!(-1 < line_loc && line_loc < 1));
83 83
}
84 84

  
85 85
void line_follow::halt()
scout/libscout/src/behaviors/navigationMap.cpp
63 63
  
64 64
    Edge* a1 = new Edge[ARRAY_SIZE];
65 65
    a1[0] = MAKE_EDGE(ISTRAIGHT, 2, 10);
66
    a1[1] = MAKE_EDGE(IRIGHT, 9, 40);
66
    a1[1] = MAKE_EDGE(IRIGHT, 13, 40);
67 67
    a1[2] = MAKE_EDGE(IUTURN, DEADEND, 0);
68 68

  
69 69
    Edge* a2 = new Edge[ARRAY_SIZE]; 
70 70
    a2[0] = MAKE_EDGE(ISTRAIGHT, 3, 10);
71
    a2[1] = MAKE_EDGE(IRIGHT, 10, 40);
71
    a2[1] = MAKE_EDGE(IRIGHT, 14, 40);
72 72
    a2[2] = MAKE_EDGE(IUTURN, 5, 10);
73 73

  
74 74
    Edge* a3 = new Edge[ARRAY_SIZE]; 
75 75
    a3[0] = MAKE_EDGE(ISTRAIGHT, 4, 10);
76
    a3[1] = MAKE_EDGE(IRIGHT, 11, 40);
76
    a3[1] = MAKE_EDGE(IRIGHT, 15, 40);
77 77
    a3[2] = MAKE_EDGE(IUTURN, 6, 10);
78 78

  
79 79
    Edge* a4 = new Edge[ARRAY_SIZE]; 
80 80
    a4[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0);
81
    a4[1] = MAKE_EDGE(IRIGHT, 12, 40);
81
    a4[1] = MAKE_EDGE(IRIGHT, 16, 40);
82 82
    a4[2] = MAKE_EDGE(IUTURN, 7, 10);
83 83

  
84 84
    Edge* a5 = new Edge[ARRAY_SIZE];
85 85
    a5[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0);
86
    a5[1] = MAKE_EDGE(ILEFT, 9, 40);
86
    a5[1] = MAKE_EDGE(ILEFT, 13, 40);
87 87
    a5[2] = MAKE_EDGE(IUTURN, 2, 10);
88 88

  
89 89
    Edge* a6 = new Edge[ARRAY_SIZE];
90 90
    a6[0] = MAKE_EDGE(ISTRAIGHT, 5, 0);
91
    a6[1] = MAKE_EDGE(ILEFT, 10, 40);
91
    a6[1] = MAKE_EDGE(ILEFT, 14, 40);
92 92
    a6[2] = MAKE_EDGE(IUTURN, 3, 10);
93 93

  
94 94
    Edge* a7 = new Edge[ARRAY_SIZE];
95 95
    a7[0] = MAKE_EDGE(ISTRAIGHT, 6, 0);
96
    a7[1] = MAKE_EDGE(ILEFT, 11, 40);
96
    a7[1] = MAKE_EDGE(ILEFT, 15, 40);
97 97
    a7[2] = MAKE_EDGE(IUTURN, 4, 10);
98 98

  
99 99
    Edge* a8 = new Edge[ARRAY_SIZE];
100 100
    a8[0] = MAKE_EDGE(ISTRAIGHT, 7, 0);
101
    a8[1] = MAKE_EDGE(ILEFT, 12, 40);
101
    a8[1] = MAKE_EDGE(ILEFT, 16, 40);
102 102
    a8[2] = MAKE_EDGE(IUTURN, DEADEND, 10);
103 103

  
104 104
    Edge* a9 = new Edge[ARRAY_SIZE];
105 105
    a9[0] = MAKE_EDGE(IRIGHT, 2, 10);
106 106
    a9[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
107
    a9[2] = MAKE_EDGE(IUTURN, 9, 40);
107
    a9[2] = MAKE_EDGE(ISPOTTURN, 13, 40);
108 108

  
109 109
    Edge* a10 = new Edge[ARRAY_SIZE];
110 110
    a10[0] = MAKE_EDGE(IRIGHT, 3, 10);
111 111
    a10[1] = MAKE_EDGE(ILEFT, 5, 10);
112
    a10[2] = MAKE_EDGE(IUTURN, 10, 40);
112
    a10[2] = MAKE_EDGE(ISPOTTURN, 14, 40);
113 113

  
114 114
    Edge* a11 = new Edge[ARRAY_SIZE];
115 115
    a11[0] = MAKE_EDGE(IRIGHT, 4, 10);
116 116
    a11[1] = MAKE_EDGE(ILEFT, 6, 10);
117
    a11[2] = MAKE_EDGE(IUTURN, 11, 40);
117
    a11[2] = MAKE_EDGE(ISPOTTURN, 15, 40);
118 118

  
119 119
    Edge* a12 = new Edge[ARRAY_SIZE];
120 120
    a12[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
121 121
    a12[1] = MAKE_EDGE(ILEFT, 7, 10);
122
    a12[2] = MAKE_EDGE(IUTURN, 12, 40);
122
    a12[2] = MAKE_EDGE(ISPOTTURN, 16, 40);
123

  
124
    Edge* a13 = new Edge[ARRAY_SIZE];
125
    a13[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
126
    a13[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
127
    a13[2] = MAKE_EDGE(ISPOTTURN, 9, 40);
128

  
129
    Edge* a14 = new Edge[ARRAY_SIZE];
130
    a14[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
131
    a14[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
132
    a14[2] = MAKE_EDGE(ISPOTTURN, 10, 40);
133

  
134
    Edge* a15 = new Edge[ARRAY_SIZE];
135
    a15[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
136
    a15[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
137
    a15[2] = MAKE_EDGE(ISPOTTURN, 11, 40);
138

  
139
    Edge* a16 = new Edge[ARRAY_SIZE];
140
    a16[0] = MAKE_EDGE(IRIGHT, DEADEND, 0);
141
    a16[1] = MAKE_EDGE(ILEFT, DEADEND, 0);
142
    a16[2] = MAKE_EDGE(ISPOTTURN, 12, 40);
143

  
123 144

  
124 145
    map.push_back(a1);
125 146
    map.push_back(a2);
......
133 154
    map.push_back(a10);
134 155
    map.push_back(a11);
135 156
    map.push_back(a12);
157
    map.push_back(a13);
158
    map.push_back(a14);
159
    map.push_back(a15);
160
    map.push_back(a16);
136 161

  
137 162
    curr_state = START_STATE;
138 163
    arrival_time = ros::TIME_MAX;
......
154 179
void navigationMap::run()
155 180
{
156 181
  Duration t;
157
  ROS_INFO("My state is: %d\n", curr_state);
158
  //Straight, straight, right, left, straight 5
159
  update_state(ISTRAIGHT);
160
  ROS_INFO("My state is: %d\n", curr_state);
161
  t = get_time_remaining();
162
  while(t.sec > 0)
163
    t = get_time_remaining();
164
  update_state(ISTRAIGHT);
165
  ROS_INFO("My state is: %d\n", curr_state);
166
  t = get_time_remaining();
167
  while(t.sec > 0)
168
    t = get_time_remaining();
169
  update_state(IRIGHT);
170
  ROS_INFO("My state is: %d\n", curr_state);
171
  t = get_time_remaining();
172
  while(t.sec > 0)
173
    t = get_time_remaining();
174
  update_state(ILEFT);
175
  ROS_INFO("My state is: %d\n", curr_state);
176
  t = get_time_remaining();
177
  while(t.sec > 0)
178
    t = get_time_remaining();
179
  update_state(ISTRAIGHT);
180
  ROS_INFO("My state is: %d\n", curr_state);
181
  t = get_time_remaining();
182
  while(t.sec > 0)
183
    t = get_time_remaining();
184
  ROS_INFO("Traveled route!\n");
185 182

  
186
  ROS_INFO("Going to state 6\n");
187
  Path path = shortest_path(6);
183
  ROS_INFO("Going to state 16\n");
184
  Path path = shortest_path(16);
188 185
  if(path.path == NULL)
186
  {
187
    ROS_WARN("There is no path to state 16");
189 188
    return;
189
  }
190 190

  
191
  ROS_INFO("Worst case time to 6 is %d", get_worst_case_time(curr_state, 6).sec);
191
  ROS_INFO("Worst case time to 16 is %d", get_worst_case_time(curr_state, 6).sec);
192 192

  
193 193
  for(int i=0; i<path.len; i++)
194 194
  {
......
199 199
      t = get_time_remaining();
200 200
    ROS_INFO("Now at state %d\n", curr_state);
201 201
  }
202

  
202 203
  ROS_INFO("Traveled route!\n");
203 204

  
204 205
  while(ok())
......
217 218
    //sets the current state to the state associated with the turn made
218 219
    if(GET_EDGE_DIR(possible_edges[i]) == turn_made)
219 220
    {
220
      int speed = 10000;//its over 9000
221
      curr_state = GET_EDGE_STATE(possible_edges[i]);
222 221
      //TODO: get actual speed
222
      int speed = 10;
223
      curr_state = GET_EDGE_STATE(possible_edges[i]);
223 224
      Duration travel_time(GET_EDGE_DIST(possible_edges[i])/speed);
224 225
      arrival_time = Time::now() + travel_time;
225 226
      return curr_state;
scout/libscout/src/behaviors/navigationMap.h
47 47

  
48 48
/** Turn defintions */
49 49
#define ISTRAIGHT	0
50
#define ILEFT		1
50
#define ILEFT		  1
51 51
#define IRIGHT		2
52 52
#define IUTURN		3
53
#define ISPOTTURN 4
53 54

  
54 55
#define START_STATE 3
55 56

  
56 57
#define DEADEND 255
57 58
#define ARRAY_SIZE 3
58
#define MAX_NODES 12
59
#define MAX_NODES 16
59 60

  
60 61
#define SPEED 10 //distance_unit per s
61 62
#define DROPOFF_TIME 10 //seconds
......
64 65
#define WAIT_TIME (TURN_TIME * NUM_ROBOTS) //Seconds
65 66
 
66 67
/** used to extract information from an encoded Edge */
67
#define GET_EDGE_DIR(edge) ((edge)&0x3)
68
#define GET_EDGE_STATE(edge) (((edge)>>2)&0xFF)
69
#define GET_EDGE_DIST(edge) (((edge)>>10)&0x3FFFFF)
68
#define GET_EDGE_DIR(edge) ((edge)&0xF)
69
#define GET_EDGE_STATE(edge) (((edge)>>4)&0xFF)
70
#define GET_EDGE_DIST(edge) (((edge)>>12)&0xFFFFF)
70 71

  
71 72
/** used to change or build an Edge's information */
72
#define SET_EDGE_DIR(dir) ((dir)&0x3)
73
#define SET_EDGE_STATE(state) (((state)&0xFF)<<2)
74
#define SET_EDGE_DIST(dist) (((dist)&0x3FFFFF)<<10)
73
#define SET_EDGE_DIR(dir) ((dir)&0xF)
74
#define SET_EDGE_STATE(state) (((state)&0xFF)<<4)
75
#define SET_EDGE_DIST(dist) (((dist)&0xFFFFF)<<12)
75 76

  
76 77
#define MAKE_EDGE(dir, state, dist) \
77 78
  SET_EDGE_DIR(dir)+SET_EDGE_STATE(state)+SET_EDGE_DIST(dist)
......
102 103
   *      |           |          |         |
103 104
   *     9|         10|        11|       12|
104 105
   *      |           |          |         |
105
   *     ---         ---        ---       ---
106
   *     ---13       ---14      ---15     ---16
106 107
   */
107 108

  
108 109
  public:

Also available in: Unified diff