}
void
-RRTNode::p(RRTNode& p)
+RRTNode::p(RRTNode& p, bool connecting_goal)
{
- if (this != &p) {
- this->_p = &p;
+ assert(this != &p);
+ if (!connecting_goal) {
+ assert(!(std::abs(p.x() - this->x()) < 1e-3
+ && std::abs(p.y() - this->y()) < 1e-3
+ && std::abs(p.h() - this->h()) < 1e-3));
}
+ this->_p = &p;
+ this->p_is_cusp(this->would_be_cusp_if_parent(p));
+ this->cusp_cnt(p);
+}
+
+void
+RRTNode::p(RRTNode& p)
+{
+ return this->p(p, false);
}
unsigned int
-RRTNode::cusp() const
+RRTNode::cusp_cnt() const
{
- return this->_cusp;
+ return this->_cusp_cnt;
}
void
-RRTNode::cusp(RRTNode const& p)
+RRTNode::cusp_cnt(RRTNode const& p)
{
- this->_cusp = p.cusp();
- if (this->sp() != p.sp() || this->sp() == 0.0) {
- this->_cusp++;
+ this->_cusp_cnt = p.cusp_cnt();
+ if (this->_p_is_cusp) {
+ this->_cusp_cnt++;
}
}
this->_segment_type = st;
}
+bool
+RRTNode::p_is_cusp(void) const
+{
+ return this->_p_is_cusp;
+}
+
+void
+RRTNode::p_is_cusp(bool isit)
+{
+ this->_p_is_cusp = isit;
+}
+
+bool
+RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
+{
+ if (std::abs(p.sp()) < 1e-3) {
+ // It should not be possible to have two zero speed nodes next
+ // to each other. In practice, this sometimes happens.
+ //assert(std::abs(this->sp()) > 1e-3);
+ if (p.p()) {
+ if (sgn(p.p()->sp()) != sgn(this->sp())) {
+ return true;
+ } else {
+ return false;
+ }
+ } else {
+ return true; // only root has no parent and it is cusp
+ }
+ } else {
+ if (std::abs(this->sp()) < 1e-3) {
+ return false; // this is cusp, not the parent
+ } else if (sgn(p.sp()) != sgn(this->sp())) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+}
+
bool
RRTNode::operator==(RRTNode const& n)
{
RRTNode* t = &this->_nodes.back();
t->p(*f);
t->c(this->cost_build(*f, *t));
- t->cusp(*f);
this->_steered.erase(this->_steered.begin());
f = t;
}
{
RRTNode* f = this->_nn;
RRTNode* t = &this->_steered.front();
+ // Require the steer method to return first node equal to nn:
+ assert(std::abs(t->x() - f->x()) < 1e-3
+ && std::abs(t->y() - f->y()) < 1e-3
+ && std::abs(t->h() - f->h()) < 1e-3);
+ this->_steered.erase(this->_steered.begin());
+ t = &this->_steered.front();
+ assert(!(std::abs(t->x() - f->x()) < 1e-3
+ && std::abs(t->y() - f->y()) < 1e-3
+ && std::abs(t->h() - f->h()) < 1e-3));
#if USE_RRTS
double cost = f->cc() + this->cost_build(*f, *t);
for (auto n: this->nv_) {
t = &this->_nodes.back();
t->p(*f);
t->c(this->cost_build(*f, *t));
- t->cusp(*f);
this->_steered.erase(this->_steered.begin());
return true;
}
this->_bc.set_pose_to(p);
}
+void
+RRTS::set_bc_to_become(std::string what)
+{
+ this->_bc.become(what);
+}
+
RRTGoal const&
RRTS::goal(void) const
{
}
void
-RRTS::start(double x, double y, double h)
+RRTS::set_init_pose_to(Pose const& p)
{
- this->_nodes.front().x(x);
- this->_nodes.front().y(y);
- this->_nodes.front().h(h);
+ this->_nodes.front().x(p.x());
+ this->_nodes.front().y(p.y());
+ this->_nodes.front().h(p.h());
}
std::vector<Pose>
double
RRTS::last_path_cost(void) const
{
- if (this->_last_path.size() == 0) {
- return 999.9;
- } else {
- return this->_last_path.back().cc();
+ if (this->_logged_paths.size() == 0) {
+ return 0.0;
}
+ assert(this->_logged_paths.back().size() > 0);
+ return this->_logged_paths.back().back().cc();
}
double
}
Json::Value
-RRTS::json() const
+RRTS::json(void) const
{
Json::Value jvo;
- unsigned int i = 0;
- for (auto n: this->_path) { // TODO because path() has just x, y, h
+ unsigned int i = 0, j = 0;
+ for (auto path: this->_logged_paths) {
+ i = 0;
+ for (auto n: path) {
+ jvo["paths"][j][i][0] = n.x();
+ jvo["paths"][j][i][1] = n.y();
+ jvo["paths"][j][i][2] = n.h();
+ jvo["paths"][j][i][3] = n.sp();
+ jvo["paths"][j][i][4] = n.st();
+ jvo["paths"][j][i][5] = n.p_is_cusp();
+ i++;
+ }
+ jvo["costs"][j] = path.back().cc();
+ j++;
+ }
+ i = 0;
+ for (auto n: this->_path) {
+ jvo["paths"][j][i][0] = n->x();
+ jvo["paths"][j][i][1] = n->y();
+ jvo["paths"][j][i][2] = n->h();
+ jvo["paths"][j][i][3] = n->sp();
+ jvo["paths"][j][i][4] = n->st();
+ jvo["paths"][j][i][5] = n->p_is_cusp();
jvo["path"][i][0] = n->x();
jvo["path"][i][1] = n->y();
jvo["path"][i][2] = n->h();
jvo["path"][i][3] = n->sp();
jvo["path"][i][4] = n->st();
+ jvo["path"][i][5] = n->p_is_cusp();
i++;
}
- jvo["goal_cc"] = this->_goal.cc();
+ jvo["costs"][j] = this->_path.back()->cc();
+ j++;
+ jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
+ jvo["cost"] = this->path_cost();
jvo["time"] = this->scnt();
return jvo;
}
{
assert(jvi["init"] != Json::nullValue);
assert(jvi["goal"] != Json::nullValue);
- this->start(
+ this->set_init_pose_to(Pose(
jvi["init"][0].asDouble(),
jvi["init"][1].asDouble(),
- jvi["init"][2].asDouble());
+ jvi["init"][2].asDouble()));
if (jvi["goal"].size() == 4) {
this->goal(
jvi["goal"][0].asDouble(),
double d1 = this->cost_search(this->_nodes.front(), rs);
double d2 = this->cost_search(rs, this->_goal);
if (this->last_path_cost() != 0.0 && d1 + d2 > this->last_path_cost()) {
- rs = this->_last_path[rand() % this->_last_path.size()];
+ auto& last_path = this->_logged_paths.back();
+ rs = last_path[rand() % last_path.size()];
}
}
#endif
this->join_steered(&this->_nodes.back());
RRTNode* just_added = &this->_nodes.back();
bool gf = false;
- while (ss > 0 && just_added->p() != nullptr) {
+ while (ss > 0 && just_added != nullptr) {
this->steer(*just_added, this->_goal);
if (this->collide_steered()) {
ss--;
just_added = just_added->p();
continue;
}
+ // The first of steered is the same as just_added.
+ this->_steered.erase(this->_steered.begin());
this->join_steered(just_added);
bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
bool gd = this->goal_drivable_from(this->_nodes.back());
double ncc = this->_nodes.back().cc() + nc;
if (this->_goal.p() == nullptr
|| ncc < this->_goal.cc()) {
- this->_goal.p(this->_nodes.back());
+ this->_goal.p(this->_nodes.back(), true);
this->_goal.c(nc);
gf = true;
}
{
if (this->path_cost() != 0.0
&& this->path_cost() < this->last_path_cost()) {
- this->_last_path.clear();
+ this->_logged_paths.push_back(std::vector<RRTNode>());
+ auto& last_path = this->_logged_paths.back();
+ last_path.reserve(this->_path.size());
+ RRTNode* p = nullptr;
for (auto n: this->_path) {
- this->_last_path.push_back(*n);
- // FIXME _last_path nodes' pointers to parents
+ last_path.push_back(*n);
+ if (p != nullptr) {
+ last_path.back().p(*p);
+ }
+ p = &last_path.back();
+ }
+ // Test that last path cost matches.
+ auto last_path_cost = last_path.back().cc();
+ for (unsigned int i = 1; i < last_path.size(); i++) {
+ last_path[i].c(this->cost_build(
+ last_path[i - 1],
+ last_path[i]));
}
+ assert(last_path_cost == last_path.back().cc());
}
this->_goal = RRTGoal(
this->_goal.x(),
projects / hubacji1 / iamcar2.git / blobdiff