@@ -90,83 +90,15 @@ namespace sa {
9090 */
9191 struct MatrixEl {
9292 Vec2i pred;
93- double dist;
94- bool visi;
93+ double g_score; // best distance from start
94+ bool processed; // in closed set (already processed)
9595 MatrixEl () {
9696 pred = Vec2i::Create (-1 ,-1 );
97- dist = std::numeric_limits<double >::max ();
98- visi = false ;
97+ g_score = std::numeric_limits<double >::max ();
98+ processed = false ;
9999 }
100100 };
101101
102- void ComputePathWavefront (
103- nav_msgs::OccupancyGrid& rInput,
104- const Vec2i& rStart,
105- const Vec2i& rEnd,
106- std::list<Vec2i>& rOutPath) {
107- // ensure the new path is clear to avoid
108- // finding something that does not exists
109- // in the frontier discovery
110- rOutPath.clear ();
111-
112- // initialize distances and predecessors
113- // using struct with all elements to optimize
114- // cache hits
115- Matrix<MatrixEl> control (rInput.info .height , rInput.info .width );
116- control.clear (MatrixEl ());
117-
118- std::queue<Vec2i> to_visit;
119- to_visit.push (Vec2i::Create (rStart.x , rStart.y ));
120- control[rStart.y ][rStart.x ].visi = true ;
121-
122- Vec2i current;
123- Vec2i children;
124- bool found = false ;
125- int index;
126- int val;
127- MatrixEl* curel;
128-
129- while (to_visit.size () > 0 ) {
130- current = to_visit.front ();
131- to_visit.pop ();
132-
133- // stop condition
134- if (current == rEnd) {
135- found = true ;
136- break ;
137- }
138-
139- // iterate over children
140- for (int x = 0 ; x < 3 ; ++x) {
141- for (int y = 0 ; y < 3 ; ++y) {
142- if (x == 1 && y == 1 ) continue ;
143- children = Vec2i::Create (current.x - 1 + x, current.y - 1 + y);
144- // only add children that were not visited
145- if (IsInBounds (rInput, children) && control[children.y ][children.x ].visi == false ) {
146- index = children.y *rInput.info .width +children.x ;
147- val = rInput.data [index];
148- // check if the OCC is clean regarding data
149- if (val >= 0 && val < 90 ) {
150- curel = &(control[children.y ][children.x ]);
151- curel->visi = true ;
152- curel->pred = Vec2i::Create (current.x , current.y );
153- to_visit.push (Vec2i::Create (children.x , children.y ));
154- }
155- }
156- }
157- }
158- }
159-
160- // compute output path from search
161- if (found == true ) {
162- current = rEnd;
163- while (current != rStart) {
164- rOutPath.push_front (current);
165- current = control[current.y ][current.x ].pred ;
166- }
167- }
168- }
169-
170102 void ComputePath (
171103 nav_msgs::OccupancyGrid& rInput,
172104 const Vec2i& rStart,
@@ -181,12 +113,23 @@ namespace sa {
181113 // validate input bounds
182114 if (!IsInBounds (rInput, const_cast <Vec2i&>(rStart)) ||
183115 !IsInBounds (rInput, const_cast <Vec2i&>(rEnd))) {
116+ ROS_DEBUG (" [SearchAlgorithms] Start or end position out of bounds"
184117 return ;
185118 }
186119
187- // check if start and end are passable
188- if (rInput.data [rStart.y *rInput.info .width +rStart.x ] > 50 ||
189- rInput.data [rEnd.y *rInput.info .width +rEnd.x ] > 50 ) {
120+ // check if start and end are passable (use consistent threshold)
121+ int start_idx = rStart.y *rInput.info .width +rStart.x ;
122+ int end_idx = rEnd.y *rInput.info .width +rEnd.x ;
123+
124+ if (rInput.data [start_idx] > 50 || rInput.data [start_idx] < 0 ||
125+ rInput.data [end_idx] > 50 || rInput.data [end_idx] < 0 ) {
126+ ROS_DEBUG (" [SearchAlgorithms] Start (%d,%d) or end (%d,%d) position not passable. Start val: %d, End val: %d"
127+ rStart.x , rStart.y , rEnd.x , rEnd.y , rInput.data [start_idx], rInput.data [end_idx]);
128+ return ;
129+ }
130+
131+ // If start equals end, return immediately
132+ if (rStart == rEnd) {
190133 return ;
191134 }
192135
@@ -206,60 +149,105 @@ namespace sa {
206149
207150 // initialize search
208151 std::priority_queue<el> q;
209- q.push (el (Vec2i::Create (rStart.x , rStart.y ), 0.0 ));
210- control[rStart.y ][rStart.x ].dist = 0.0 ;
152+ double initial_f = sqrt (Distance (rStart, rEnd));
153+ q.push (el (Vec2i::Create (rStart.x , rStart.y ), initial_f));
154+ control[rStart.y ][rStart.x ].g_score = 0.0 ;
155+ control[rStart.y ][rStart.x ].pred = Vec2i::Create (-1 , -1 ); // No predecessor
156+
157+ // Limit iterations to prevent infinite loops
158+ int max_iterations = rInput.info .width * rInput.info .height ;
159+ int iterations = 0 ;
211160
212161 // do search
213- while (q.size () > 0 ) {
162+ while (q.size () > 0 && iterations < max_iterations) {
163+ iterations++;
164+
214165 current = q.top ().pos ;
215166 q.pop ();
216-
217- if (control[current.y ][current.x ].visi == true ) continue ;
218- control[current.y ][current.x ].visi = true ;
167+
168+ // Skip if already processed
169+ if (control[current.y ][current.x ].processed ) {
170+ continue ;
171+ }
172+
173+ // Mark as processed (closed set)
174+ control[current.y ][current.x ].processed = true ;
219175
220176 // stop condition
221177 if (current == rEnd) {
222178 found = true ;
223179 break ;
224180 }
225181
226- // iterate over children
182+ // iterate over children (8-directional movement)
227183 for (int x = 0 ; x < 3 ; ++x) {
228184 for (int y = 0 ; y < 3 ; ++y) {
229185 if (x == 1 && y == 1 ) continue ;
230186
231187 children = Vec2i::Create (current.x - 1 + x, current.y - 1 + y);
232188 if (IsInBounds (rInput, children)
233- && control[children.y ][children.x ].visi == false
234- && rInput.data [children.y *rInput.info .width +children.x ] <= 90 ) {
189+ && !control[children.y ][children.x ].processed ) {
235190
236- // compute actual distance (sqrt of squared distance) for correct A*
237- double edge_cost = sqrt (Distance (current, children));
238- dist = control[current.y ][current.x ].dist + edge_cost;
239- heuristic = sqrt (Distance (rEnd, children));
240- distance_metric = dist + heuristic;
191+ int child_idx = children.y *rInput.info .width +children.x ;
192+ int child_val = rInput.data [child_idx];
241193
242- // relax edge - only add to queue if we found a better path
243- if (control[children.y ][children.x ].dist > distance_metric) {
244- control[children.y ][children.x ].dist = distance_metric;
245- control[children.y ][children.x ].pred = Vec2i::Create (current.x , current.y );
194+ // Use consistent obstacle threshold (50) and check for unknown cells
195+ if (child_val <= 50 ) {
196+ // compute tentative g_score
197+ double edge_cost = sqrt (Distance (current, children));
198+ double tentative_g = control[current.y ][current.x ].g_score + edge_cost;
246199
247- // only add to queue when we update the distance
248- el element (children, distance_metric);
249- q.push (element);
250- }
200+ // If this path to neighbor is better than any previous one
201+ if (tentative_g < control[children.y ][children.x ].g_score ) {
202+ // Record the better path
203+ control[children.y ][children.x ].g_score = tentative_g;
204+ control[children.y ][children.x ].pred = Vec2i::Create (current.x , current.y );
205+
206+ // Add to priority queue with f-score
207+ heuristic = sqrt (Distance (rEnd, children));
208+ double f_score = tentative_g + heuristic;
209+ q.push (el (children, f_score));
210+ }
211+ }
251212 }
252213 }
253214 }
254215 }
255216
217+ if (iterations >= max_iterations) {
218+ ROS_WARN (" [SearchAlgorithms] A* search exceeded maximum iterations (%d)"
219+ }
220+
256221 // compute output path from search
257222 if (found) {
258223 current = rEnd;
259- while (current != rStart) {
224+ int path_length = 0 ;
225+ int max_path_length = rInput.info .width + rInput.info .height ; // Reasonable max path length
226+
227+ while (current != rStart && path_length < max_path_length) {
260228 rOutPath.push_front (current);
261- current = control[current.y ][current.x ].pred ;
229+ Vec2i next = control[current.y ][current.x ].pred ;
230+
231+ // Validate predecessor to prevent infinite loops
232+ if (next.x == -1 && next.y == -1 ) {
233+ ROS_WARN (" [SearchAlgorithms] Invalid predecessor found during path reconstruction"
234+ rOutPath.clear ();
235+ return ;
236+ }
237+
238+ current = next;
239+ path_length++;
240+ }
241+
242+ if (path_length >= max_path_length) {
243+ ROS_WARN (" [SearchAlgorithms] Path reconstruction exceeded maximum length"
244+ rOutPath.clear ();
245+ } else {
246+ ROS_DEBUG (" [SearchAlgorithms] Found path with %zu waypoints in %d iterations" size (), iterations);
262247 }
248+ } else {
249+ ROS_DEBUG (" [SearchAlgorithms] No path found from (%d,%d) to (%d,%d) after %d iterations"
250+ rStart.x , rStart.y , rEnd.x , rEnd.y , iterations);
263251 }
264252 }
265253
0 commit comments