std::reverse(this->path_.begin(), this->path_.end());
}
-RRTS::RRTS() : gen_(std::random_device{}())
+RRTS::RRTS() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
{
this->nodes_.reserve(4000000);
this->steered_.reserve(1000);
this->nodes_.front().x(jvi["init"][0].asDouble());
this->nodes_.front().y(jvi["init"][1].asDouble());
this->nodes_.front().h(jvi["init"][2].asDouble());
- this->goal_.x(jvi["goal"][0].asDouble());
- this->goal_.y(jvi["goal"][1].asDouble());
- this->goal_.b(jvi["goal"][2].asDouble());
if (jvi["goal"].size() == 4) {
- this->goal_.e(jvi["goal"][3].asDouble());
+ this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+ jvi["goal"][1].asDouble(),
+ jvi["goal"][2].asDouble(),
+ jvi["goal"][3].asDouble());
} else {
- this->goal_.e(jvi["goal"][2].asDouble());
+ this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+ jvi["goal"][1].asDouble(),
+ jvi["goal"][2].asDouble(),
+ jvi["goal"][2].asDouble());
}
}
void
RRTS::reset()
{
- this->goal_ = RRTGoal();
+ this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
+ this->goal_.e());
this->path_.clear();
this->steered_.clear();
this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());