#include <iostream>
#include <jsoncpp/json/json.h>
-#include "rrts.h"
+#include "rrtce.h"
std::chrono::high_resolution_clock::time_point TSTART_;
std::chrono::high_resolution_clock::time_point TEND_;
std::vector<RRTNode> final_path;
double final_path_cost = 9999;
+ unsigned int final_path_steps = 0;
+ double cost_from_orig_init = 0;
+ unsigned int steps_from_orig_init = 0;
RRTNode init_node;
init_node.x(jvi["init"][0].asDouble());
init_node.y(jvi["init"][1].asDouble());
init_node.h(jvi["init"][2].asDouble());
init_node.sp(2.7);
init_node.st(M_PI / 32); // only for sc2_4
+ RRTNode last_node(init_node);
+ RRTNode orig_init(init_node);
init_node.next();
+ steps_from_orig_init++;
+ {
+ double angl_diff = std::abs(last_node.h() - init_node.h());
+ if (angl_diff == 0)
+ cost_from_orig_init += sqrt(
+ pow(last_node.x() - init_node.x(), 2)
+ + pow(last_node.y() - init_node.y(), 2)
+ );
+ else
+ cost_from_orig_init += angl_diff * init_node.mtr();
+ }
RRTS rrts;
rrts.nodes().front().x(init_node.x());
while (rrts.next()) {}
TEND();
if (rrts.path().size() > 0) {
- if (cc(*rrts.path().back()) < final_path_cost) {
- final_path_cost = cc(*rrts.path().back());
+ if (
+ cc(*rrts.path().back())
+ + cost_from_orig_init
+ < final_path_cost
+ ) {
+ final_path_cost =
+ cc(*rrts.path().back())
+ + cost_from_orig_init
+ ;
+ final_path_steps = steps_from_orig_init;
final_path.clear();
for (auto n: rrts.path()) {
final_path.push_back(RRTNode());
}
}
}
-
- {
- jvo["time"] = TDIFF();
- }
+ int rcnt = 0;
+ jvo[rcnt++] = rrts.json();
+for (int i = 1; i < 5; i++) {
+ last_node = RRTNode(init_node);
+ init_node.next();
+ steps_from_orig_init++;
{
- jvo["iterations"] = rrts.icnt();
- }
+ double angl_diff = std::abs(last_node.h() - init_node.h());
+ if (angl_diff < 0.001)
+ cost_from_orig_init += sqrt(
+ pow(last_node.x() - init_node.x(), 2)
+ + pow(last_node.y() - init_node.y(), 2)
+ );
+ else
+ cost_from_orig_init += angl_diff * init_node.mtr();
+ }
+ RRTS rrts2;
+ rrts2.goals() = rrts.goals();
+ rrts2.obstacles() = rrts.obstacles();
+ rrts2.nodes().front().x(init_node.x());
+ rrts2.nodes().front().y(init_node.y());
+ rrts2.nodes().front().h(init_node.h());
{
- jvo["init"][0] = rrts.nodes().front().x();
- jvo["init"][1] = rrts.nodes().front().y();
- jvo["init"][2] = rrts.nodes().front().h();
- }
- {
- jvo["cost"] = final_path_cost;
+ double edist_init_goal = sqrt(
+ pow(
+ rrts2.nodes().front().x()
+ - rrts2.goals().front().x(),
+ 2
+ )
+ + pow(
+ rrts2.nodes().front().y()
+ - rrts2.goals().front().y(),
+ 2
+ )
+ );
+ rrts2.set_sample(
+ rrts2.nodes().front().x(), edist_init_goal,
+ rrts2.nodes().front().y(), edist_init_goal,
+ 0, 2 * M_PI
+ );
}
- {
- if (final_path.size() > 0) {
- jvo["goal"][0] = final_path.back().x();
- jvo["goal"][1] = final_path.back().y();
- jvo["goal"][2] = final_path.back().h();
- }
- unsigned int cu = 0;
- unsigned int co = 0;
- unsigned int pcnt = 0;
- for (auto n: final_path) {
- jvo["path"][pcnt][0] = n.x();
- jvo["path"][pcnt][1] = n.y();
- jvo["path"][pcnt][2] = n.h();
- if (n.t(RRTNodeType::cusp))
- cu++;
- if (n.t(RRTNodeType::connected))
- co++;
- pcnt++;
+ TSTART();
+ while (rrts2.next()) {}
+ TEND();
+ if (rrts2.path().size() > 0) {
+ if (
+ cc(*rrts2.path().back())
+ + cost_from_orig_init
+ < final_path_cost
+ ) {
+ final_path_cost =
+ cc(*rrts2.path().back())
+ + cost_from_orig_init
+ ;
+ final_path_steps = steps_from_orig_init;
+ final_path.clear();
+ for (auto n: rrts2.path()) {
+ final_path.push_back(RRTNode());
+ final_path.back().x(n->x());
+ final_path.back().y(n->y());
+ final_path.back().h(n->h());
+ }
}
- jvo["cusps-in-path"] = cu;
- jvo["connecteds-in-path"] = co;
}
+ jvo[rcnt++] = rrts2.json();
+}
{
- unsigned int gcnt = 0;
- for (auto g: rrts.goals()) {
- jvo["goals"][gcnt][0] = g.x();
- jvo["goals"][gcnt][1] = g.y();
- jvo["goals"][gcnt][2] = g.h();
- gcnt++;
+ jvo[rcnt]["obst"] = jvo[rcnt - 1]["obst"];
+ jvo[rcnt]["cost"] = final_path_cost;
+ unsigned int ncnt = 0;
+ for (unsigned int i = 0; i < final_path_steps; i++) {
+ jvo[rcnt]["path"][ncnt][0] = orig_init.x();
+ jvo[rcnt]["path"][ncnt][1] = orig_init.y();
+ jvo[rcnt]["path"][ncnt][2] = orig_init.h();
+ orig_init.next();
+ ncnt++;
}
- }
- {
- unsigned int ocnt = 0;
- for (auto o: rrts.obstacles()) {
- unsigned int ccnt = 0;
- for (auto c: o.poly()) {
- jvo["obst"][ocnt][ccnt][0] = std::get<0>(c);
- jvo["obst"][ocnt][ccnt][1] = std::get<1>(c);
- ccnt++;
- }
- ocnt++;
+ for (auto n: final_path) {
+ jvo[rcnt]["path"][ncnt][0] = n.x();
+ jvo[rcnt]["path"][ncnt][1] = n.y();
+ jvo[rcnt]["path"][ncnt][2] = n.h();
+ ncnt++;
}
}
- {
- jvo["nodes"] = (unsigned int) rrts.nodes().size();
- }
- //{
- // unsigned int ncnt = 0;
- // for (auto n: rrts.nodes()) {
- // jvo["nodes_x"][ncnt] = n.x();
- // jvo["nodes_y"][ncnt] = n.y();
- // //jvo["nodes_h"][ncnt] = n.h();
- // ncnt++;
- // }
- //}
std::cout << jvo << std::endl;
return 0;
}