Project

General

Profile

Revision aa5e4ddc

IDaa5e4ddcdd38a8388d7157691894c250e3cb439d

Added by Leon over 6 years ago

Finishing BFS for behaviors navigationMap. Also commiting teleop skeleton code.

View differences:

scout/libscout/src/behaviors/navigationMap.cpp
14 14
   *     ---         ---        ---       ---
15 15
   */
16 16
  
17
    Edge* a1 = {MAKE_EDGE(ISTRAIGHT, 2, 10), 
18
                MAKE_EDGE(IRIGHT, 9, 40),
19
                MAKE_EDGE(IUTURN, DEADEND, 0)};
17
    Edge a1[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 2, 10), 
18
                            MAKE_EDGE(IRIGHT, 9, 40),
19
                            MAKE_EDGE(IUTURN, DEADEND, 0)};
20 20

  
21
    Edge* a2 = {MAKE_EDGE(ISTRAIGHT, 3, 10),
22
                MAKE_EDGE(IRIGHT, 10, 40),
23
                MAKE_EDGE(IUTURN, 5, 10)};
21
    Edge a2[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 3, 10),
22
                            MAKE_EDGE(IRIGHT, 10, 40),
23
                            MAKE_EDGE(IUTURN, 5, 10)};
24 24

  
25
    Edge* a3 = {MAKE_EDGE(ISTRAIGHT, 4, 10),
26
                MAKE_EDGE(IRIGHT, 11, 40),
27
                MAKE_EDGE(IUTURN, 6, 10)};
25
    Edge a3[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 4, 10),
26
                            MAKE_EDGE(IRIGHT, 11, 40),
27
                            MAKE_EDGE(IUTURN, 6, 10)};
28 28

  
29
    Edge* a4 = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
30
                MAKE_EDGE(IRIGHT, 12, 40),
31
                MAKE_EDGE(IUTURN, 7, 10)};
29
    Edge a4[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
30
                            MAKE_EDGE(IRIGHT, 12, 40),
31
                            MAKE_EDGE(IUTURN, 7, 10)};
32 32

  
33
    Edge* a5 = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
34
                MAKE_EDGE(ILEFT, 9, 40),
35
                MAKE_EDGE(IUTURN, 2, 10)};
33
    Edge a5[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
34
                            MAKE_EDGE(ILEFT, 9, 40),
35
                            MAKE_EDGE(IUTURN, 2, 10)};
36 36

  
37
    Edge* a6 = {MAKE_EDGE(ISTRAIGHT, 5, 10),
38
                MAKE_EGE(ILEFT, 10, 40),
39
                MAKE_EDGE(IUTURN, 3, 10)};
37
    Edge a6[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 5, 10),
38
                            MAKE_EDGE(ILEFT, 10, 40),
39
                            MAKE_EDGE(IUTURN, 3, 10)};
40 40

  
41
    Edge* a7 = {MAKE_EDGE(ISTRAIGHT, 6, 10),
42
                MAKE_EGE(ILEFT, 11, 40),
43
                MAKE_EDGE(IUTURN, 4, 10)};
41
    Edge a7[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 6, 10),
42
                            MAKE_EDGE(ILEFT, 11, 40),
43
                            MAKE_EDGE(IUTURN, 4, 10)};
44 44

  
45
    Edge* a8 = {MAKE_EDGE(ISTRAIGHT, 7, 10),
46
                MAKE_EGE(ILEFT, 12, 40),
47
                MAKE_EDGE(IUTURN, DEADEND, 0)};
45
    Edge a8[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 7, 10),
46
                            MAKE_EDGE(ILEFT, 12, 40),
47
                            MAKE_EDGE(IUTURN, DEADEND, 0)};
48 48

  
49
    Edge* a9 = {MAKE_EDGE(IRIGHT, 2, 10),
50
                MAKE_EGE(ILEFT, DEADEND, 0),
51
                MAKE_EDGE(IUTURN, 9, 40)};
49
    Edge a9[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 2, 10),
50
                            MAKE_EDGE(ILEFT, DEADEND, 0),
51
                            MAKE_EDGE(IUTURN, 9, 40)};
52 52

  
53
    Edge* a10 = {MAKE_EDGE(IRIGHT, 3, 10),
54
                MAKE_EGE(ILEFT, 5, 10),
55
                MAKE_EDGE(IUTURN, 10, 40)};
53
    Edge a10[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 3, 10),
54
                            MAKE_EDGE(ILEFT, 5, 10),
55
                            MAKE_EDGE(IUTURN, 10, 40)};
56 56

  
57
    Edge* a11 = {MAKE_EDGE(IRIGHT, 4, 10),
58
                MAKE_EGE(ILEFT, 6, 10),
59
                MAKE_EDGE(IUTURN, 11, 40)};
57
    Edge a11[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 4, 10),
58
                            MAKE_EDGE(ILEFT, 6, 10),
59
                            MAKE_EDGE(IUTURN, 11, 40)};
60 60

  
61
    Edge* a12 = {MAKE_EDGE(IRIGHT, DEADEND, 0),
62
                MAKE_EGE(ILEFT, 7, 10),
63
                MAKE_EDGE(IUTURN, 12, 40)};
61
    Edge a12[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, DEADEND, 0),
62
                            MAKE_EDGE(ILEFT, 7, 10),
63
                            MAKE_EDGE(IUTURN, 12, 40)};
64 64
      
65 65
    map.pushback(a1);
66 66
    map.pushback(a2);
......
82 82
{
83 83
    while(!map.empty())
84 84
    {
85
      Edge** e = pop_back();
86
      for(int i=0; i<ARRAY_SIZE; i++)
87
      {
88
        delete e[i];
89
      }
85
      Edge* e = map.pop_back();
86
      delete e;
90 87
    }
91 88
    return;
92 89
}
......
106 103
    // BFS algorithm
107 104
    State curr_state = get_state();
108 105
    char visited[MAX_NODES+1]; 
109
    //FAKE QUEUE OH NO 
110
    //TODO: real implementation
111
    Queue queue;
112
    queue.enqueue(curr_state);
106
    queue<State> q;
107
    q.push(curr_state);
108
    // not zero = visited, zero = unvisited, negative = start state
109
    visited[curr_state] = -1;
113 110

  
114
    // 1 == visited, 0 == not visited
115
    visited[curr_state] = 1;
116

  
117
    while (!queue.is_empty)
111
    while (!q.is_empty)
118 112
    {
119
        State state = queue.dequeue();
113
        State state = q.pop();
120 114
        if (state == target_state)
121 115
        {   
122
            //TODO: implement moves_list
123
            State i;
124

  
125
            while(i != curr_state)
116
            Turn moves_list[MAX_NODES];
117
            int j = 0; // counter
118
            for(State child = state; visited[child] >= 0; 
119
                    child = visited[child]) //while not start state
126 120
            {
127
                i = visited[i];
128

  
121
                State parent = visited[child];
122
                Edge* edges = get_outbound_edges(parent);
123
                for (int i = 0; i < ARRAY_SIZE; i++)
124
                {
125
                    if (GET_EDGE_STATE(edges[i]) == child)
126
                    {
127
                        moves_list[j] = GET_EDGE_DIR(edges[i]);
128
                        j++;
129
                        break;
130
                    }
131
                }
129 132
            }
130 133
            return moves_list;
131 134
        }
......
136 139
            State new_state = GET_EDGE_STATE(edges[i]);
137 140
            if (!visited[new_state]) 
138 141
            {
142
                // set visited to the parent of the new state
139 143
                visited[new_state] = state;
140
                queue.enqueue(new_state);
144
                q.push(new_state);
141 145
            }
142 146
        }
143 147
    }
144
}
145

  
146
navigationMap::make_edge(Turn dir, State state, int dist)
147
{
148
    Edge* e = new Edge;
149
    e->direction = dir;
150
    e->state = state;
151
    e->distance = dist;
152
    return e;
148
    //oops, no way to get to target from state
149
    return NULL;
153 150
}
scout/libscout/src/behaviors/navigationMap.h
2 2
#define _NAVIGATION_MAP_
3 3

  
4 4
#include <cstdlib>
5
#include "linieDrive.h" // Get turn Macros
5
#include <queue>
6
#include "lineDrive.h" // Get turn Macros
6 7

  
7 8
#define START_STATE 1
8 9

  
......
20 21

  
21 22
#define MAKE_EDGE(dir, state, dist) \
22 23
SET_EDGE_DIR(dir)+SET_EDGE_STATE(state)+SET_EDGE_DIST(dist)
23

  
24
    
24 25
typedef int Edge;
25 26
typedef int State;
26 27
typedef int Turn;
......
41 42
    private:
42 43
        vector <Edge*> map;
43 44
        State curr_state;
44

  
45
        Edge* make_edge(Turn dir, State state, int dist);
46
        
47 45
};
48 46
#endif
scout/scoutsim/CMakeLists.txt
36 36
#rosbuild_add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)
37 37
#rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
38 38
#rosbuild_add_executable(mimic tutorials/mimic.cpp)
39
rosbuild_add_executable(scout_teleop src/scout_teleop.cpp)
scout/scoutsim/src/scout_teleop.cpp
1
#include <ros/ros.h>
2
#include <motors/set_motors.h>
3
#include <signal.h>
4
#include <termios.h>
5
#include <stdio.h>
6

  
7
// TODO: add this to a constants.h
8
#define QUEUE_SIZE 10
9

  
10
#define KEYCODE_R 0x43 
11
#define KEYCODE_L 0x44
12
#define KEYCODE_U 0x41
13
#define KEYCODE_D 0x42
14
#define KEYCODE_Q 0x71
15
#define KEYCODE_S 0x73
16

  
17
class TeleopScout
18
{
19
public:
20
    TeleopScout();
21
    void keyLoop();
22

  
23
private:
24
    ros::NodeHandle nh_;
25
    bool fl_set_, fr_set_, bl_set_, br_set_;
26
    int fl_speed_, fr_speed_, bl_speed_, br_speed_;
27
    char last_key;
28
    ros::Publisher motors_pub_;
29
};
30

  
31
TeleopScout::TeleopScout()
32
{
33
    fl_set_ = fr_set_ = bl_set_ = br_set_ = true;
34
    fl_speed_ = fr_speed_ = bl_speed_ = br_speed_ = 0;
35
    motors_pub_ = nh_.advertise<motors::set_motors>(
36
                    "scout1/set_motors", QUEUE_SIZE);
37
}
38

  
39
int kfd = 0;
40
struct termios cooked, raw;
41

  
42
void quit(int sig)
43
{
44
    tcsetattr(kfd, TCSANOW, &cooked);
45
    ros::shutdown();
46
    exit(0);
47
}
48

  
49

  
50
int main(int argc, char** argv)
51
{
52
  ros::init(argc, argv, "teleop_scout");
53
  TeleopScout teleop_scout;
54

  
55
  signal(SIGINT,quit);
56

  
57
  teleop_scout.keyLoop();
58
  
59
  return(0);
60
}
61

  
62

  
63
void TeleopScout::keyLoop()
64
{
65
  char c;
66
  bool dirty=false;
67

  
68
  // get the console in raw mode                                                              
69
  tcgetattr(kfd, &cooked);
70
  memcpy(&raw, &cooked, sizeof(struct termios));
71
  raw.c_lflag &=~ (ICANON | ECHO);
72
  // Setting a new line, then end of file                         
73
  raw.c_cc[VEOL] = 1;
74
  raw.c_cc[VEOF] = 2;
75
  tcsetattr(kfd, TCSANOW, &raw);
76

  
77
  puts("Reading from keyboard");
78
  puts("---------------------------");
79
  puts("Use arrow keys to move the scout.");
80

  
81

  
82
  for(;;)
83
  {
84
    ROS_INFO("loop pass engaged");
85
    // get the next event from the keyboard  
86
    if(read(kfd, &c, 1) < 0)
87
    {
88
      perror("read():");
89
      exit(-1);
90
    }
91

  
92
    //ROS_DEBUG("value: 0x%02X\n", c);
93
    //ROS_INFO("value: 0x%02X\n", c);
94
    switch(c)
95
    {
96
      case KEYCODE_L:
97
        ROS_INFO("LEFT");
98
        fl_speed_ = bl_speed_ = 200;
99
        fr_speed_ = br_speed_ = -200;
100
        dirty = true;
101
        break;
102
      case KEYCODE_R:
103
        ROS_INFO("RIGHT");
104
        fl_speed_ = bl_speed_ = -200;
105
        fr_speed_ = br_speed_ = 200;
106
        dirty = true;
107
        break;
108
      case KEYCODE_U:
109
        ROS_INFO("UP");
110
        fl_speed_ = bl_speed_ = 200;
111
        fr_speed_ = br_speed_ = 200;
112
        dirty = true;
113
        break;
114
      case KEYCODE_D:
115
        ROS_INFO("DOWN");
116
        fl_speed_ = bl_speed_ = -200;
117
        fr_speed_ = br_speed_ = -200;
118
        dirty = true;
119
        break;
120
      default:
121
        ROS_INFO("test");
122
        fl_speed_ = bl_speed_ = 0;
123
        fr_speed_ = br_speed_ = 0;
124
        dirty = true;
125
        break;
126
    }
127
   
128
    motors::set_motors msg;
129
    msg.fl_set = fl_set_;
130
    msg.fr_set = fr_set_;
131
    msg.bl_set = bl_set_;
132
    msg.br_set = br_set_;
133
    msg.fl_speed = fl_speed_;
134
    msg.fr_speed = fr_speed_;
135
    msg.bl_speed = bl_speed_;
136
    msg.br_speed = br_speed_;
137
    
138
    if(dirty == true)
139
    {
140
      if (last_key != c)
141
      {
142
        motors_pub_.publish(msg);
143
      }
144
      dirty=false;
145
    }
146
    last_key = c;
147
  }
148

  
149
  return;
150
}
151

  
152

  
153

  
scout/scoutsim/src/turtle_teleop.cpp
1
#include <ros/ros.h>
2
#include <signal.h>
3
#include <termios.h>
4
#include <stdio.h>
5
#include "constants.h"
6

  
7
#define KEYCODE_R 0x43 
8
#define KEYCODE_L 0x44
9
#define KEYCODE_U 0x41
10
#define KEYCODE_D 0x42
11
#define KEYCODE_Q 0x71
12

  
13
class TeleopTurtle
14
{
15
public:
16
  TeleopTurtle();
17
  void keyLoop();
18

  
19
private:
20

  
21
  
22
  ros::NodeHandle nh_;
23
  double linear_, angular_, l_scale_, a_scale_;
24
  ros::Publisher vel_pub_;
25
  
26
};
27

  
28
TeleopTurtle::TeleopTurtle():
29
  linear_(0),
30
  angular_(0),
31
  l_scale_(2.0),
32
  a_scale_(2.0)
33
{
34
  nh_.param("scale_angular", a_scale_, a_scale_);
35
  nh_.param("scale_linear", l_scale_, l_scale_);
36

  
37
  vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
38
}
39

  
40
int kfd = 0;
41
struct termios cooked, raw;
42

  
43
void quit(int sig)
44
{
45
  tcsetattr(kfd, TCSANOW, &cooked);
46
  ros::shutdown();
47
  exit(0);
48
}
49

  
50

  
51
int main(int argc, char** argv)
52
{
53
  ros::init(argc, argv, "teleop_turtle");
54
  TeleopTurtle teleop_turtle;
55

  
56
  signal(SIGINT,quit);
57

  
58
  teleop_turtle.keyLoop();
59
  
60
  return(0);
61
}
62

  
63

  
64
void TeleopTurtle::keyLoop()
65
{
66
  char c;
67
  bool dirty=false;
68

  
69

  
70
  // get the console in raw mode                                                              
71
  tcgetattr(kfd, &cooked);
72
  memcpy(&raw, &cooked, sizeof(struct termios));
73
  raw.c_lflag &=~ (ICANON | ECHO);
74
  // Setting a new line, then end of file                         
75
  raw.c_cc[VEOL] = 1;
76
  raw.c_cc[VEOF] = 2;
77
  tcsetattr(kfd, TCSANOW, &raw);
78

  
79
  puts("Reading from keyboard");
80
  puts("---------------------------");
81
  puts("Use arrow keys to move the turtle.");
82

  
83

  
84
  for(;;)
85
  {
86
    // get the next event from the keyboard  
87
    if(read(kfd, &c, 1) < 0)
88
    {
89
      perror("read():");
90
      exit(-1);
91
    }
92

  
93
    linear_=angular_=0;
94
    ROS_DEBUG("value: 0x%02X\n", c);
95
  
96
    switch(c)
97
    {
98
      case KEYCODE_L:
99
        ROS_DEBUG("LEFT");
100
        angular_ = 1.0;
101
        dirty = true;
102
        break;
103
      case KEYCODE_R:
104
        ROS_DEBUG("RIGHT");
105
        angular_ = -1.0;
106
        dirty = true;
107
        break;
108
      case KEYCODE_U:
109
        ROS_DEBUG("UP");
110
        linear_ = 1.0;
111
        dirty = true;
112
        break;
113
      case KEYCODE_D:
114
        ROS_DEBUG("DOWN");
115
        linear_ = -1.0;
116
        dirty = true;
117
        break;
118
    }
119
   
120

  
121
    turtlesim::Velocity vel;
122
    vel.angular = a_scale_*angular_;
123
    vel.linear = l_scale_*linear_;
124
    if(dirty ==true)
125
    {
126
      vel_pub_.publish(vel);    
127
      dirty=false;
128
    }
129
  }
130

  
131

  
132
  return;
133
}
134

  
135

  
136

  

Also available in: Unified diff