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; |
Also available in: Unified diff