@@ -108,94 +108,51 @@ void RRT::renderParametersGui() {
108108 }
109109}
110110
111- // TODO: Since RRT*-like planners share the same iteration concept,
112- // solveConcurrently() function can be only defined one time
113- void RRT::solveConcurrently (std::shared_ptr<Vertex> start_point,
114- std::shared_ptr<Vertex> goal_point,
115- std::shared_ptr<MessageQueue<bool >> message_queue) {
116- // copy assignment
117- // thread-safe due to shared_ptrs
118- std::shared_ptr<Vertex> start_vertex = start_point;
119- std::shared_ptr<Vertex> goal_vertex = goal_point;
120- std::shared_ptr<MessageQueue<bool >> s_message_queue = message_queue;
121-
122- bool solved = false ;
123-
124- double cycle_duration = 1 ; // duration of a single simulation cycle in ms
125- // init stop watch
126- auto last_update = std::chrono::system_clock::now ();
127-
128- unsigned int iteration_number = 0u ;
129-
130- while (true ) {
131- // compute time difference to stop watch
132- long time_since_last_update =
133- std::chrono::duration_cast<std::chrono::milliseconds>(
134- std::chrono::system_clock::now () - last_update)
135- .count ();
136-
137- if (time_since_last_update >= cycle_duration) {
138- // //////////////////////////
139- // run the main algorithm //
140- // //////////////////////////
141- if (iteration_number < max_iterations_) {
142- std::shared_ptr<Vertex> x_rand = std::make_shared<Vertex>();
143- std::shared_ptr<Vertex> x_nearest = std::make_shared<Vertex>();
144- std::shared_ptr<Vertex> x_new = std::make_shared<Vertex>();
145-
146- sample (x_rand);
147- nearest (x_rand, x_nearest);
148-
149- // find the distance between x_rand and x_nearest
150- double d = distance (x_rand, x_nearest);
151-
152- // if this distance d > max_distance_, we need to find nearest state in
153- // the direction of x_rand
154- if (d > range_) {
155- interpolate (x_nearest, x_rand, range_ / d, x_new);
156- } else {
157- x_new->x = x_rand->x ;
158- x_new->y = x_rand->y ;
159- }
160-
161- if (!isCollision (x_nearest, x_new)) {
162- x_new->parent = x_nearest;
163-
164- std::unique_lock<std::mutex> lck (mutex_);
165- vertices_.emplace_back (x_new);
166- edges_.emplace_back (x_nearest, x_new);
167- lck.unlock ();
168-
169- if (inGoalRegion (x_new)) {
170- goal_vertex->parent = std::move (x_new);
171- solved = true ;
172- }
173- }
174-
175- iteration_number++;
176- } else {
177- std::cout << " Iterations number reach max limit. Planning stopped."
178- << ' \n '
179- solved = true ;
180- }
181- // //////////////////////////
182-
183- // reset stop watch for next cycle
184- last_update = std::chrono::system_clock::now ();
111+ void RRT::updatePlanner (bool &solved, Vertex &start, Vertex &goal) {
112+ std::unique_lock<std::mutex> iter_no_lck (iter_no_mutex_);
113+ bool running = (curr_iter_no_ < max_iterations_);
114+ iter_no_lck.unlock ();
115+
116+ if (running) {
117+ std::shared_ptr<Vertex> x_rand = std::make_shared<Vertex>();
118+ std::shared_ptr<Vertex> x_nearest = std::make_shared<Vertex>();
119+ std::shared_ptr<Vertex> x_new = std::make_shared<Vertex>();
120+
121+ sample (x_rand);
122+ nearest (x_rand, x_nearest);
123+
124+ // find the distance between x_rand and x_nearest
125+ double d = distance (x_rand, x_nearest);
126+
127+ // if this distance d > max_distance_, we need to find nearest state in
128+ // the direction of x_rand
129+ if (d > range_) {
130+ interpolate (x_nearest, x_rand, range_ / d, x_new);
131+ } else {
132+ x_new->x = x_rand->x ;
133+ x_new->y = x_rand->y ;
185134 }
186135
187- // sends an update method to the message queue using move semantics
188- auto ftr = std::async (std::launch::async, &MessageQueue<bool >::send,
189- s_message_queue, std::move (solved));
190- ftr.wait ();
136+ if (!isCollision (x_nearest, x_new)) {
137+ x_new->parent = x_nearest;
191138
192- if (solved) return ;
139+ std::unique_lock<std::mutex> lck (mutex_);
140+ vertices_.emplace_back (x_new);
141+ edges_.emplace_back (x_nearest, x_new);
142+ lck.unlock ();
193143
194- std::lock_guard<std::mutex> lock (mutex_);
195- if (is_stopped_) return ;
144+ if (inGoalRegion (x_new)) {
145+ goal.parent = std::move (x_new);
146+ solved = true ;
147+ }
148+ }
196149
197- // sleep at every iteration to reduce CPU usage
198- std::this_thread::sleep_for (std::chrono::milliseconds (1 ));
150+ iter_no_lck.lock ();
151+ curr_iter_no_++;
152+ iter_no_lck.unlock ();
153+ } else {
154+ std::cout << " Iterations number reach max limit. Planning stopped." ' \n '
155+ solved = true ;
199156 }
200157}
201158
0 commit comments