Revision cccc25c9
ID | cccc25c99e4cea26a0a2c76d7ab7121512d7b3d2 |
Made nav map into an actual workable behaviorgit statusgit status
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