Revision ede03b64
| ID | ede03b64ba90685e63e150fc90adfe8e62bb3161 |
Hello, this is James, I updated the navigation map code to better support the getting of the current state and the getting of the estimated time until the robot arrives at its destination. Also there is a TODO that says that we need to find the actual speed of the robot.
| scout/libscout/src/behaviors/navigationMap.cpp | ||
|---|---|---|
| 74 | 74 |
map.pushback(a10); |
| 75 | 75 |
map.pushback(a11); |
| 76 | 76 |
map.pushback(a12); |
| 77 |
|
|
| 77 |
|
|
| 78 |
curr_state = START_STATE; |
|
| 79 |
eta = 99999; |
|
| 78 | 80 |
} |
| 79 | 81 |
|
| 80 | 82 |
/** @brief Goes through and frees all allocated memory */ |
| ... | ... | |
| 93 | 95 |
|
| 94 | 96 |
} |
| 95 | 97 |
|
| 98 |
navigationMap::update_state(Turn turn_made) |
|
| 99 |
{
|
|
| 100 |
Edge* possible_edges = get_outbound_edges(current_state); |
|
| 101 |
int arr_size = sizeof(possible_edges)/sizeof(Edge); |
|
| 102 |
for(i=0;i<arr_size;i++) |
|
| 103 |
{
|
|
| 104 |
//sets the current state to the state associated with the turn made |
|
| 105 |
if(GET_EDGE_DIR(possible_edges[i]) == turn_made) |
|
| 106 |
{
|
|
| 107 |
int speed = 10000;//its over 9000 |
|
| 108 |
current_state = GET_EDGE_STATE(possible_edges[i]); |
|
| 109 |
//TODO: get actual speed |
|
| 110 |
arrival_time = ros::Time::now() + |
|
| 111 |
GET_EDGE_DIST(possible_edges[i])/speed; |
|
| 112 |
return current_state; |
|
| 113 |
} |
|
| 114 |
} |
|
| 115 |
return -1;//failure to succeed |
|
| 116 |
} |
|
| 117 |
|
|
| 118 |
navigationMap::get_eta() |
|
| 119 |
{
|
|
| 120 |
return arrival_time; |
|
| 121 |
} |
|
| 122 |
|
|
| 123 |
navigationMap::get_time_remaining() |
|
| 124 |
{
|
|
| 125 |
return (arrival_time - ros::Time::now()); |
|
| 126 |
} |
|
| 127 |
|
|
| 96 | 128 |
navigationMap::get_state() |
| 97 | 129 |
{
|
| 98 | 130 |
return curr_state; |
| scout/libscout/src/behaviors/navigationMap.h | ||
|---|---|---|
| 25 | 25 |
typedef int Edge; |
| 26 | 26 |
typedef int State; |
| 27 | 27 |
typedef int Turn; |
| 28 |
typedef ros::Time Time; |
|
| 28 | 29 |
|
| 29 | 30 |
class navigationMap : Behavior |
| 30 | 31 |
{
|
| ... | ... | |
| 33 | 34 |
~navigationMap(); |
| 34 | 35 |
|
| 35 | 36 |
void run(); |
| 37 |
|
|
| 38 |
State update_state(Turn turn_made); |
|
| 39 |
|
|
| 40 |
Time get_eta(); |
|
| 41 |
Time get_time_remaining(); |
|
| 36 | 42 |
|
| 37 | 43 |
State get_state(); |
| 38 | 44 |
Turn* shortest_path(State target_state); |
| ... | ... | |
| 42 | 48 |
private: |
| 43 | 49 |
vector <Edge*> map; |
| 44 | 50 |
State curr_state; |
| 51 |
Time arrival_time; |
|
| 45 | 52 |
}; |
| 46 | 53 |
#endif |
Also available in: Unified diff