Fix car dimension settings in test template

[hubacji1/iamcar2.git] / rrts / src / rrts.cc

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 4f131145c63ae19c45cdf7cd5c38a7334b04ccc0..7cd4684deadee7ab413b97553309b178becd3678 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -279,6 +279,12 @@ RRTS::set_bc_pose_to(Pose const& p)
        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
 {
@@ -328,11 +334,11 @@ RRTS::scnt() 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>
@@ -418,10 +424,10 @@ RRTS::json(Json::Value jvi)
 {
        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(),