Entry here means the range of poses in the format {x, y, b, e}, where
(x, y) are the cartesian coordinates, b is begin of the heading range
and e is the end of the heading range.
bool connect();
void rewire();
public:
bool connect();
void rewire();
public:
+ /// ---
+ struct { double x; double y; double b; double e; } entry;
+ bool entry_set = false;
+ /// ---
/*! \brief Initialize RRT algorithm if needed.
*/
virtual void init();
/*! \brief Initialize RRT algorithm if needed.
*/
virtual void init();
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->nodes().front().x(jvi["init"][0].asDouble());
this->nodes().front().y(jvi["init"][1].asDouble());
this->nodes().front().h(jvi["init"][2].asDouble());
+ {
+ if (jvi["entry"] != Json::nullValue) {
+ this->entry_set = true;
+ this->entry.x = jvi["entry"][0].asDouble();
+ this->entry.y = jvi["entry"][1].asDouble();
+ this->entry.b = jvi["entry"][2].asDouble();
+ this->entry.e = jvi["entry"][3].asDouble();
+ } else {
+ this->entry.x = 0.0;
+ this->entry.y = 0.0;
+ this->entry.b = 0.0;
+ this->entry.e = 0.0;
+ }
+ }
{
RRTNode tmp_node;
RRTNode* gp = nullptr;
{
RRTNode tmp_node;
RRTNode* gp = nullptr;
+ if (jvi["entry"] != Json::nullValue) {
+ this->entry_set = true;
+ this->entry.x = jvi["entry"][0].asDouble();
+ this->entry.y = jvi["entry"][1].asDouble();
+ this->entry.b = jvi["entry"][2].asDouble();
+ this->entry.e = jvi["entry"][3].asDouble();
+ tmp_node.x(this->entry.x);
+ tmp_node.y(this->entry.y);
+ tmp_node.h((this->entry.b + this->entry.e) / 2.0);
+ this->goals().push_back(tmp_node);
+ this->goals().back().p(gp);
+ gp = &this->goals().back();
+ }
for (auto g: jvi["goals"]) {
tmp_node.x(g[0].asDouble());
tmp_node.y(g[1].asDouble());
for (auto g: jvi["goals"]) {
tmp_node.x(g[0].asDouble());
tmp_node.y(g[1].asDouble());