Project

General

Profile

Revision 1905324e

ID1905324ede544b0057161ba7a6ff8f31be9ebc45
Parent 58371433
Child afa9104d

Added by Priya almost 12 years ago

Trying to make the warehouse robot drive around

View differences:

scout/libscout/src/behaviors/Scheduler.cpp
26 26
 */
27 27
void Scheduler::get_task(int robot)
28 28
{
29
  ROS_INFO("SCHEDULER: robots vector size is: %d", robots.size());
30 29
  Robot my_robot = robots[robot-1];
31
  ROS_INFO("SCHEDULER: My robot is %s", my_robot.name.c_str());
32 30
  if(my_robot.sched_status != WAITING_ROBOT)
33 31
  {
34 32
	  waitingRobots.push(my_robot);
35 33
    my_robot.sched_status = WAITING_ROBOT;
36
    ROS_INFO("SCHEDULER: Added to scheduler %d", my_robot.sched_status);
37 34
  }
38 35
}
39 36

  
......
48 45
  Duration five(5);
49 46
  Duration three(3);
50 47
  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);
54
  Order d(4,0,2,t,p,end);
55
  Order e(5,0,4,t,p,end);
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);
56 53
	
57 54
	unassignedOrders->insert(a);
58 55
	unassignedOrders->insert(b);
......
168 165
    new_robot.sched_status = NEW_ROBOT;
169 166
    robots.push_back(new_robot);
170 167

  
171
    ROS_INFO("Registration a robot %s %d", new_robot.name.c_str(), robots.size());
172 168
    std_msgs::String msg;
173 169
    std::stringstream ss;
174 170
    ss<<"REG_SUCCESS "<<id;
......
196 192
  ROS_INFO("I am a scheduler!. Now waiting for robots");
197 193
  while(robots.size() < 1 && ok())
198 194
  {
199
    ROS_INFO("Robots size: %d", robots.size());
200 195
    spinOnce();
201 196
  }
202 197
  ROS_INFO("SCHEDULER: main loop");
scout/libscout/src/behaviors/WH_Robot.cpp
2 2
#include "../helper_classes/Order.h"
3 3

  
4 4
/** @Brief: warehouse robot constructor **/
5
WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot")
5
WH_Robot::WH_Robot(std::string scoutname):line_follow(scoutname) 
6 6
{
7 7
  nav_map = new navigationMap(scoutname);
8 8
  curr_task = DEFAULT_TASK;
......
26 26
 *  @param the Path to follow
27 27
 */
28 28
void WH_Robot::follow_path(Path path_to_follow){
29
  ROS_INFO("Path length: %d", path_to_follow.len);
30 29
  for(int i=0; i<path_to_follow.len; i++)
31 30
  {
32 31
    Turn t = path_to_follow.path[i];
33
    ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state());
32
    ROS_INFO("making turn %d", t);
34 33
    nav_map->update_state(t);
35
    Duration time = nav_map->get_time_remaining();
36
    while(time.sec > 0)
37
      time = nav_map->get_time_remaining();
34
    switch(t)
35
    {
36
      case ISTRAIGHT:
37
        follow_line();
38
        break;
39
      case ILEFT:
40
        turn_left();
41
        follow_line();
42
        break;
43
      case IRIGHT:
44
        turn_right();
45
        follow_line();
46
        break;
47
      case IUTURN:
48
        u_turn();
49
        follow_line();
50
        break;
51
    }
38 52
  }  
39
  ROS_INFO("Path complete at state %d", (int)nav_map->get_state());
40 53
}
41 54

  
42 55
int WH_Robot::exec_task()
......
141 154
    spinOnce();
142 155
  }
143 156

  
157
  follow_line();
158

  
144 159
  while(ok())
145 160
  {
146 161
    ROS_INFO("WH_ROBOT: Running main loop");
scout/libscout/src/behaviors/WH_Robot.h
5 5
#define TASK_COMPLETED 0
6 6
#define TASK_FAILED -1
7 7

  
8
#include "line_follow.h"
8 9
#include "../Behavior.h"
9 10
#include "navigationMap.h"
10 11
#include "../helper_classes/Order.h"
11 12
#include <assert.h>
12 13
#include <stdlib.h>
13 14

  
14
class WH_Robot : Behavior{
15
class WH_Robot : line_follow{
15 16
        std::string name;
16 17
        int id;
17 18

  
......
35 36

  
36 37
        ros::Publisher robot_to_sched;
37 38
        ros::Subscriber sched_to_robot;
38

  
39 39
};
40 40

  
41 41
#endif
scout/libscout/src/behaviors/line_follow.cpp
36 36
    {
37 37
        double line_loc = linesensor->readline();
38 38

  
39
        cout << "line_loc: " << line_loc << endl;
40

  
41 39
        motor_l = -MOTOR_BASE + SCALE * line_loc;
42 40
        motor_r = -MOTOR_BASE - SCALE * line_loc;
43 41

  
44 42
        motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
45 43
    }
46 44
    halt();
45
    ROS_INFO("Intersection reached!");
47 46
}
48 47

  
49 48
void line_follow::turn_left()
......
56 55

  
57 56
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
58 57

  
59
    double line_loc = linesensor->readline();
60
    cout << "line_loc: " << line_loc << endl;
61 58
    if(first)
62 59
    {
63 60
        loop_rate->sleep();
......
84 81

  
85 82
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
86 83

  
87
    double line_loc = linesensor->readline();
88
    cout << "line_loc: " << line_loc << endl;
89 84
    if(first)
90 85
    {
91 86
        loop_rate->sleep();
......
97 92
  while(linesensor->readline());
98 93
}
99 94

  
100
void line_follow::run()
95
void line_follow::u_turn()
101 96
{
97
  turn_right();
102 98
  follow_line();
103 99
  turn_right();
100
}
101

  
102
void line_follow::run()
103
{
104 104
  follow_line();
105
  turn_left();
106 105
  follow_line();
107
  turn_right();
106
  /*
107
  follow_line();
108
  u_turn();
109
  follow_line();
110
  u_turn();
111
  follow_line();
112
  follow_line();
113
  u_turn();
108 114
  follow_line();
115
  */
109 116
}
scout/libscout/src/behaviors/line_follow.h
31 31
#define MOTOR_BASE 50
32 32
#define SCALE 20
33 33

  
34
class line_follow : Behavior
34
class line_follow : public Behavior
35 35
{
36 36
    public:
37 37
        line_follow(std::string scoutname) : Behavior(scoutname, "line_follow") {};
......
39 39
        /** Actually executes the behavior. */
40 40
        void run();
41 41

  
42
    private:
42
    protected:
43 43
        void turn_left();
44 44
        void turn_right();
45
        void u_turn();
45 46
        void follow_line();
46 47
        void halt();
47 48
};
scout/libscout/src/behaviors/navigationMap.h
51 51
#define IRIGHT		2
52 52
#define IUTURN		3
53 53

  
54
#define START_STATE 1
54
#define START_STATE 3
55 55

  
56 56
#define DEADEND 255
57 57
#define ARRAY_SIZE 3

Also available in: Unified diff