Project

General

Profile

Revision cccc25c9

IDcccc25c99e4cea26a0a2c76d7ab7121512d7b3d2

Added by Priya about 12 years ago

Made nav map into an actual workable behaviorgit statusgit status

View differences:

scout/libscout/CMakeLists.txt
30 30
#target_link_libraries(example ${PROJECT_NAME})
31 31

  
32 32
set(MAIN_FILES src/Behavior.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp)
33
set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp)
33
set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp)
34 34
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/EncodersControl.cpp)
35 35

  
36 36
rosbuild_add_executable(libscout ${MAIN_FILES} ${BEHAVIOR_FILES} ${CONTROL_CLASSES})
scout/libscout/src/Behavior.h
47 47
#include "EncodersControl.h"
48 48
#include "constants.h"
49 49

  
50
typedef ros::Time Time;
51
typedef ros::Duration Duration;
52

  
50 53
class Behavior
51 54
{
52 55
    public:
scout/libscout/src/BehaviorList.cpp
5 5
  behavior_list.push_back((Behavior*)new draw_cw_circle(scoutname));
6 6
  behavior_list.push_back((Behavior*)new draw_ccw_circle(scoutname));
7 7
  behavior_list.push_back((Behavior*)new Odometry(scoutname));
8
  behavior_list.push_back((Behavior*)new navigationMap(scoutname));
8 9
  return;
9 10
}
10 11

  
scout/libscout/src/BehaviorList.h
5 5
#include "behaviors/draw_cw_circle.h"
6 6
#include "behaviors/draw_ccw_circle.h"
7 7
#include "behaviors/Odometry.h"
8
#include "behaviors/navigationMap.h"
8 9

  
9 10
class BehaviorList
10 11
{
scout/libscout/src/behaviors/navigationMap.cpp
1 1
#include "navigationMap.h"
2 2

  
3
using namespace std;
4

  
3 5
/** @brief Initializes the map */
4
navigationMap::navigationMap(std::string scoutname) : Behavior(scoutname, "navigationMap")
6
navigationMap::navigationMap(string scoutname) : Behavior(scoutname, "navigationMap")
5 7
{
6 8
  /** Initialize Map 
7 9
   *
......
88 90
    map.push_back(a12);
89 91

  
90 92
    curr_state = START_STATE;
91
    eta = 99999;
93
    arrival_time = ros::TIME_MAX;
92 94
}
93 95

  
94 96
/** @brief Goes through and frees all allocated memory */
......
103 105
  return;
104 106
}
105 107

  
106
navigationMap::run()
108
void navigationMap::run()
107 109
{
108

  
110
  Duration t;
111
  ROS_INFO("My state is: %d\n", curr_state);
112
  //Straight, straight, right, left, straight 5
113
  update_state(ISTRAIGHT);
114
  ROS_INFO("My state is: %d\n", curr_state);
115
  t = get_time_remaining();
116
  while(t.toSec() > 0)
117
    t = get_time_remaining();
118
  update_state(ISTRAIGHT);
119
  ROS_INFO("My state is: %d\n", curr_state);
120
  t = get_time_remaining();
121
  while(t.toSec() > 0)
122
    t = get_time_remaining();
123
  update_state(IRIGHT);
124
  ROS_INFO("My state is: %d\n", curr_state);
125
  t = get_time_remaining();
126
  while(t.toSec() > 0)
127
    t = get_time_remaining();
128
  update_state(ILEFT);
129
  ROS_INFO("My state is: %d\n", curr_state);
130
  t = get_time_remaining();
131
  while(t.toSec() > 0)
132
    t = get_time_remaining();
133
  update_state(ISTRAIGHT);
134
  ROS_INFO("My state is: %d\n", curr_state);
135
  t = get_time_remaining();
136
  while(t.toSec() > 0)
137
    t = get_time_remaining();
138
  ROS_INFO("Traveled route!\n");
139
  while(ok())
140
    continue;
109 141
}
110 142

  
111
navigationMap::update_state(Turn turn_made)
143
State navigationMap::update_state(Turn turn_made)
112 144
{
113 145
  Edge* possible_edges = get_outbound_edges(curr_state);
114 146
  int arr_size = sizeof(possible_edges)/sizeof(Edge);
......
120 152
      int speed = 10000;//its over 9000
121 153
      curr_state = GET_EDGE_STATE(possible_edges[i]);
122 154
      //TODO: get actual speed
123
      arrival_time = ros::Time::now() + 
124
        GET_EDGE_DIST(possible_edges[i])/speed;
155
      Duration travel_time(GET_EDGE_DIST(possible_edges[i])/speed);
156
      arrival_time = Time::now() + travel_time;
125 157
      return curr_state;
126 158
    }
127 159
  }
128 160
  return -1;//failure to succeed
129 161
}
130 162

  
131
navigationMap::get_eta()
163
Time navigationMap::get_eta()
132 164
{
133 165
  return arrival_time;
134 166
}
135 167

  
136
navigationMap::get_time_remaining()
168
Duration navigationMap::get_time_remaining()
137 169
{
138
  return (arrival_time - ros::Time::now());
170
  return (arrival_time - Time::now());
139 171
}
140 172

  
141
navigationMap::get_state()
173
State navigationMap::get_state()
142 174
{
143 175
  return curr_state;
144 176
}
145 177

  
146
navigationMap::get_outbound_edges(State state)
178
Edge* navigationMap::get_outbound_edges(State state)
147 179
{
148
  return map(state-1); 
180
  return map.at(state-1); 
149 181
}
150 182

  
151 183
Path navigationMap::shortest_path(State target_state)
scout/libscout/src/behaviors/navigationMap.h
3 3

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

  
9
#define ISTRAIGHT	0
10
#define ILEFT		1
11
#define IRIGHT		2
12
#define IUTURN		3
7 13

  
8 14
#define START_STATE 1
9 15

  
......
25 31
typedef int Edge;
26 32
typedef int State;
27 33
typedef int Turn;
28
typedef ros::Time Time;
29 34

  
30 35
typedef struct{
31 36
  int len;
......
43 48
    State update_state(Turn turn_made);
44 49

  
45 50
    Time get_eta();
46
    Time get_time_remaining();
51
    Duration get_time_remaining();
47 52

  
48 53
    State get_state();
49 54
    Path shortest_path(State target_state);
......
51 56
    Edge* get_outbound_edges(State state);        
52 57

  
53 58
  private:
54
    vector <Edge*> map;
59
    std::vector <Edge*> map;
55 60
    State curr_state;
56 61
    Time arrival_time;
57 62
};

Also available in: Unified diff