PlannerRRT_SE2_TPS.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "nav-precomp.h"  // Precomp header

#include <mrpt/nav/planners/PlannerRRT_SE2_TPS.h>
#include <mrpt/nav/tpspace/CPTG_DiffDrive_CollisionGridBased.h>
#include <mrpt/utils/CTicTac.h>
#include <mrpt/random.h>
#include <mrpt/system/filesystem.h>

using namespace mrpt::nav;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace std;

MRPT_TODO("Optimize getNearestNode() with KD-tree!")

PlannerRRT_SE2_TPS::PlannerRRT_SE2_TPS() : m_initialized(false) {}
/** Load all params from a config file source */
void PlannerRRT_SE2_TPS::loadConfig(
        const mrpt::utils::CConfigFileBase& ini, const std::string& sSect)
{
        PlannerTPS_VirtualBase::internal_loadConfig_PTG(ini, sSect);
}

/** Must be called after setting all params (see `loadConfig()`) and before
 * calling `solve()` */
void PlannerRRT_SE2_TPS::initialize()
{
        PlannerTPS_VirtualBase::internal_initialize_PTG();

        m_initialized = true;
}

/** The main API entry point: tries to find a planned path from 'goal' to
 * 'target' */
void PlannerRRT_SE2_TPS::solve(
        const PlannerRRT_SE2_TPS::TPlannerInput& pi,
        PlannerRRT_SE2_TPS::TPlannerResult& result)
{
        mrpt::utils::CTimeLoggerEntry tle(m_timelogger, "PT_RRT::solve");

        // Sanity checks:
        ASSERTMSG_(m_initialized, "initialize() must be called before!");

        // Calc maximum vehicle shape radius:
        double max_veh_radius = 0.;
        for (const auto& ptg : m_PTGs)
                mrpt::utils::keep_max(max_veh_radius, ptg->getMaxRobotRadius());

        // [Algo `tp_space_rrt`: Line 1]: Init tree adding the initial pose
        if (result.move_tree.getAllNodes().empty())
        {
                result.move_tree.root = 0;
                result.move_tree.insertNode(
                        result.move_tree.root, TNodeSE2_TP(pi.start_pose));
        }

        mrpt::utils::CTicTac working_time;
        working_time.Tic();
        size_t rrt_iter_counter = 0;

        size_t SAVE_3D_TREE_LOG_DECIMATION_CNT = 0;
        static size_t SAVE_LOG_SOLVE_COUNT = 0;
        SAVE_LOG_SOLVE_COUNT++;

        // Keep track of the best solution so far:
        // By reusing the contents of "result" we make the algorithm re-callable
        // ("any-time" algorithm) to refine results

        // [Algo `tp_space_rrt`: Line 2]: Iterate
        // ------------------------------------------
        for (;;)
        {
                // Check end conditions:
                const double elap_tim = working_time.Tac();
                if ((end_criteria.maxComputationTime > 0 &&
                         elap_tim > end_criteria.maxComputationTime)  // Max comp time
                        ||
                        (result.goal_distance < end_criteria.acceptedDistToTarget &&
                         elap_tim >=
                                 end_criteria
                                         .minComputationTime)  // Reach closer than this to target
                        )
                {
                        break;
                }

                // [Algo `tp_space_rrt`: Line 3]: sample random state (with goal
                // biasing)
                // -----------------------------------------
                node_pose_t x_rand;
                // bool rand_is_target=false;
                if (mrpt::random::getRandomGenerator().drawUniform(0.0, 1.0) <
                        params.goalBias)
                {
                        x_rand = pi.goal_pose;
                        // rand_is_target=true;
                }
                else
                {
                        // Sample uniform:
                        for (int i = 0; i < node_pose_t::static_size; i++)
                                x_rand[i] = mrpt::random::getRandomGenerator().drawUniform(
                                        pi.world_bbox_min[i], pi.world_bbox_max[i]);
                }
                const CPose2D x_rand_pose(x_rand);

                // [Algo `tp_space_rrt`: Line 4]: Init empty solution set
                // -----------------------------------------
                typedef std::map<double, TMoveEdgeSE2_TP> sorted_solution_list_t;
                sorted_solution_list_t candidate_new_nodes;  // Map: cost -> info. Pick
                // begin() to select the
                // lowest-cose one.

                const PoseDistanceMetric<TNodeSE2>
                        distance_evaluator_se2;  // Plain distances in SE(2), not along PTGs
                bool is_new_best_solution = false;  // Just for logging purposes

                //#define DO_LOG_TXTS
                std::string sLogTxt;

                // [Algo `tp_space_rrt`: Line 5]: For each PTG
                // -----------------------------------------
                const size_t nPTGs = m_PTGs.size();
                for (size_t idxPTG = 0; idxPTG < nPTGs; ++idxPTG)
                {
                        rrt_iter_counter++;

                        // [Algo `tp_space_rrt`: Line 5]: Search nearest neig. to x_rand
                        // -----------------------------------------------
                        const PoseDistanceMetric<TNodeSE2_TP> distance_evaluator(
                                *m_PTGs[idxPTG]);

                        const TNodeSE2_TP query_node(x_rand);

                        m_timelogger.enter("TMoveTree::getNearestNode");
                        mrpt::utils::TNodeID x_nearest_id =
                                result.move_tree.getNearestNode(query_node, distance_evaluator);
                        m_timelogger.leave("TMoveTree::getNearestNode");

                        if (x_nearest_id == INVALID_NODEID)
                        {
                                // We can't find any close node, at least with this PTG's paths:
                                // skip

                                // Before that, save log:
                                if (params.save_3d_log_freq > 0 &&
                                        (++SAVE_3D_TREE_LOG_DECIMATION_CNT >=
                                         params.save_3d_log_freq))
                                {
                                        SAVE_3D_TREE_LOG_DECIMATION_CNT =
                                                0;  // Reset decimation counter
                                        TRenderPlannedPathOptions render_options;
                                        render_options.highlight_path_to_node_id =
                                                result.best_goal_node_id;
                                        render_options.highlight_last_added_edge = false;
                                        render_options.x_rand_pose = &x_rand_pose;
                                        render_options.log_msg = "SKIP: Can't find any close node";
                                        render_options.log_msg_position = mrpt::math::TPoint3D(
                                                pi.world_bbox_min.x, pi.world_bbox_min.y, 0);
                                        render_options.ground_xy_grid_frequency = 1.0;

                                        mrpt::opengl::COpenGLScene scene;
                                        renderMoveTree(scene, pi, result, render_options);
                                        mrpt::system::createDirectory("./rrt_log_trees");
                                        scene.saveToFile(
                                                mrpt::format(
                                                        "./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",
                                                        static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),
                                                        static_cast<unsigned int>(rrt_iter_counter)));
                                }

                                continue;  // Skip
                        }

                        const TNodeSE2_TP& x_nearest_node =
                                result.move_tree.getAllNodes().find(x_nearest_id)->second;

                        // [Algo `tp_space_rrt`: Line 6]: Relative target
                        // -----------------------------------------------
                        const CPose2D x_nearest_pose(x_nearest_node.state);
                        const CPose2D x_rand_rel = x_rand_pose - x_nearest_pose;

                        // [Algo `tp_space_rrt`: Line 7]: Relative target in TP-Space
                        // ------------------------------------------------------------
                        const double D_max =
                                std::min(params.maxLength, m_PTGs[idxPTG]->getRefDistance());

                        double d_rand;  // Coordinates in TP-space
                        int k_rand;  // k_rand is the index of target_alpha in PTGs
                        // corresponding to a specific d_rand
                        // bool tp_point_is_exact =
                        m_PTGs[idxPTG]->inverseMap_WS2TP(
                                x_rand_rel.x(), x_rand_rel.y(), k_rand, d_rand);
                        d_rand *=
                                m_PTGs[idxPTG]
                                        ->getRefDistance();  // distance to target, in "real meters"

                        float d_free;
                        // bool local_obs_ok = false; // Just for 3D log files: indicates
                        // whether obstacle points have been recomputed

                        // [Algo `tp_space_rrt`: Line 8]: TP-Obstacles
                        // ------------------------------------------------------------
                        // Transform obstacles as seen from x_nearest_node -> TP_obstacles
                        double TP_Obstacles_k_rand = .0;  // vector<double> TP_Obstacles;
                        const double MAX_DIST_FOR_OBSTACLES =
                                1.5 * m_PTGs[idxPTG]->getRefDistance();  // Maximum Euclidean
                        // distance (radius)
                        // for considering
                        // obstacles around the
                        // current robot pose

                        ASSERT_ABOVE_(
                                m_PTGs[idxPTG]->getRefDistance(),
                                1.1 * max_veh_radius);  // Make sure the PTG covers at least a
                        // bit more than the vehicle shape!!
                        // (should be much, much higher)

                        {
                                CTimeLoggerEntry tle(
                                        m_timelogger, "PT_RRT::solve.changeCoordinatesReference");
                                transformPointcloudWithSquareClipping(
                                        pi.obstacles_points, m_local_obs,
                                        CPose2D(x_nearest_node.state), MAX_DIST_FOR_OBSTACLES);
                                // local_obs_ok=true;
                        }
                        {
                                CTimeLoggerEntry tle(
                                        m_timelogger, "PT_RRT::solve.SpaceTransformer");
                                spaceTransformerOneDirectionOnly(
                                        k_rand, m_local_obs, m_PTGs[idxPTG].get(),
                                        MAX_DIST_FOR_OBSTACLES, TP_Obstacles_k_rand);
                        }

                        // directions k_rand in TP_obstacles[k_rand] = d_free
                        // this is the collision free distance to the TP_target
                        d_free = TP_Obstacles_k_rand;  // TP_Obstacles[k_rand];

                        // [Algo `tp_space_rrt`: Line 10]: d_new
                        // ------------------------------------------------------------
                        double d_new = std::min(
                                D_max,
                                d_rand);  // distance of the new candidate state in TP-space

// mrpt::poses::CPose2D *log_new_state_ptr=NULL; // For graphical logs only

#ifdef DO_LOG_TXTS
                        sLogTxt += mrpt::format(
                                "tp_idx=%u tp_exact=%c\n d_free: %f d_rand=%f d_new=%f\n",
                                static_cast<unsigned int>(idxPTG),
                                tp_point_is_exact ? 'Y' : 'N', d_free, d_rand, d_new);
                        sLogTxt += mrpt::format(
                                " nearest:%s\n", x_nearest_pose.asString().c_str());
#endif

                        // [Algo `tp_space_rrt`: Line 13]: Do we have free space?
                        // ------------------------------------------------------------
                        if (d_free >= d_new)
                        {
                                // [Algo `tp_space_rrt`: Line 14]: PTG function
                                // ------------------------------------------------------------
                                // given d_rand and k_rand provides x,y,phi of the point in
                                // c-space
                                uint32_t nStep;
                                m_PTGs[idxPTG]->getPathStepForDist(k_rand, d_new, nStep);

                                mrpt::math::TPose2D rel_pose;
                                m_PTGs[idxPTG]->getPathPose(k_rand, nStep, rel_pose);

                                mrpt::math::wrapToPiInPlace(rel_pose.phi);  // wrap to [-pi,pi]
                                // -->avoid out of
                                // bounds errors

                                // [Algo `tp_space_rrt`: Line 15]: pose composition
                                // ------------------------------------------------------------
                                const mrpt::poses::CPose2D new_state_rel(rel_pose);
                                mrpt::poses::CPose2D new_state =
                                        x_nearest_pose + new_state_rel;  // compose the new_motion
                                // as the last nmotion and
                                // the new state
                                // log_new_state_ptr = &new_state;

                                // Check whether there's already a too-close node around:
                                // --------------------------------------------------------
                                bool accept_this_node = true;

                                // Is this a potential solution
                                const double goal_dist =
                                        new_state.distance2DTo(pi.goal_pose.x, pi.goal_pose.y);
                                const double goal_ang = std::abs(
                                        mrpt::math::angDistance(new_state.phi(), pi.goal_pose.phi));
                                const bool is_acceptable_goal =
                                        (goal_dist < end_criteria.acceptedDistToTarget) &&
                                        (goal_ang < end_criteria.acceptedAngToTarget);

                                mrpt::utils::TNodeID new_nearest_id = INVALID_NODEID;
                                if (!is_acceptable_goal)  // Only check for nearby nodes if this
                                // is not a solution!
                                {
                                        double new_nearest_dist;
                                        const TNodeSE2 new_state_node(new_state);

                                        m_timelogger.enter("TMoveTree::getNearestNode");
                                        new_nearest_id = result.move_tree.getNearestNode(
                                                new_state_node, distance_evaluator_se2,
                                                &new_nearest_dist, &result.acceptable_goal_node_ids);
                                        m_timelogger.leave("TMoveTree::getNearestNode");

                                        if (new_nearest_id != INVALID_NODEID)
                                        {
                                                // Also check angular distance:
                                                const double new_nearest_ang = std::abs(
                                                        mrpt::math::angDistance(
                                                                new_state.phi(), result.move_tree.getAllNodes()
                                                                                                         .find(new_nearest_id)
                                                                                                         ->second.state.phi));
                                                accept_this_node =
                                                        (new_nearest_dist >=
                                                                 params.minDistanceBetweenNewNodes ||
                                                         new_nearest_ang >= params.minAngBetweenNewNodes);
                                        }
                                }

                                if (!accept_this_node)
                                {
#ifdef DO_LOG_TXTS
                                        if (new_nearest_id != INVALID_NODEID)
                                        {
                                                sLogTxt += mrpt::format(
                                                        " -> new node NOT accepted for closeness to: %s\n",
                                                        result.move_tree.getAllNodes()
                                                                .find(new_nearest_id)
                                                                ->second.state.asString()
                                                                .c_str());
                                        }
#endif
                                        continue;  // Too close node, skip!
                                }

                                // [Algo `tp_space_rrt`: Line 16]: Add to candidate solution set
                                // ------------------------------------------------------------
                                // Create "movement" (tree edge) object:
                                TMoveEdgeSE2_TP new_edge(
                                        x_nearest_id, mrpt::math::TPose2D(new_state));

                                new_edge.cost = d_new;
                                new_edge.ptg_index = idxPTG;
                                new_edge.ptg_K = k_rand;
                                new_edge.ptg_dist = d_new;

                                candidate_new_nodes[new_edge.cost] = new_edge;

                        }  // end if the path is obstacle free
                        else
                        {
#ifdef DO_LOG_TXTS
                                sLogTxt += mrpt::format(" -> d_free NOT < d_rand\n");
#endif
                        }

                }  // end for idxPTG

                // [Algo `tp_space_rrt`: Line 19]: Any solution found?
                // ------------------------------------------------------------
                if (!candidate_new_nodes.empty())
                {
                        const TMoveEdgeSE2_TP& best_edge =
                                candidate_new_nodes.begin()->second;
                        const TNodeSE2_TP new_state_node(best_edge.end_state);

                        // Insert into the tree:
                        const mrpt::utils::TNodeID new_child_id =
                                result.move_tree.getNextFreeNodeID();
                        result.move_tree.insertNodeAndEdge(
                                best_edge.parent_id, new_child_id, new_state_node, best_edge);

                        // Distance to goal:
                        const double goal_dist =
                                mrpt::poses::CPose2D(best_edge.end_state)
                                        .distance2DTo(pi.goal_pose.x, pi.goal_pose.y);
                        const double goal_ang = std::abs(
                                mrpt::math::angDistance(
                                        best_edge.end_state.phi, pi.goal_pose.phi));

                        const bool is_acceptable_goal =
                                (goal_dist < end_criteria.acceptedDistToTarget) &&
                                (goal_ang < end_criteria.acceptedAngToTarget);

                        if (is_acceptable_goal)
                                result.acceptable_goal_node_ids.insert(new_child_id);

                        // Total path length:
                        double this_path_cost = std::numeric_limits<double>::max();
                        if (is_acceptable_goal)  // Don't waste time computing path length
                        // if it doesn't matter anyway
                        {
                                TMoveTreeSE2_TP::path_t candidate_solution_path;
                                result.move_tree.backtrackPath(
                                        new_child_id, candidate_solution_path);
                                this_path_cost = 0;
                                for (TMoveTreeSE2_TP::path_t::const_iterator it =
                                                 candidate_solution_path.begin();
                                         it != candidate_solution_path.end(); ++it)
                                        if (it->edge_to_parent)
                                                this_path_cost += it->edge_to_parent->cost;
                        }

                        // Check if this should be the new optimal path:
                        if (is_acceptable_goal && this_path_cost < result.path_cost)
                        {
                                result.goal_distance = goal_dist;
                                result.path_cost = this_path_cost;

                                result.best_goal_node_id = new_child_id;
                                is_new_best_solution = true;
                        }
                }  // end if any candidate found

                //  Graphical logging, if enabled:
                // ------------------------------------------------------
                if (params.save_3d_log_freq > 0 &&
                        (++SAVE_3D_TREE_LOG_DECIMATION_CNT >= params.save_3d_log_freq ||
                         is_new_best_solution))
                {
                        CTimeLoggerEntry tle(
                                m_timelogger, "PT_RRT::solve.generate_log_files");
                        SAVE_3D_TREE_LOG_DECIMATION_CNT = 0;  // Reset decimation counter

                        // Render & save to file:
                        TRenderPlannedPathOptions render_options;
                        render_options.highlight_path_to_node_id = result.best_goal_node_id;
                        render_options.x_rand_pose = &x_rand_pose;
                        // render_options.x_nearest_pose = &x_nearest_pose;
                        // if (local_obs_ok) render_options.local_obs_from_nearest_pose =
                        // &m_local_obs;
                        // render_options.new_state = log_new_state_ptr;
                        render_options.highlight_last_added_edge = true;
                        render_options.ground_xy_grid_frequency = 1.0;

                        render_options.log_msg = sLogTxt;
                        render_options.log_msg_position = mrpt::math::TPoint3D(
                                pi.world_bbox_min.x, pi.world_bbox_min.y, 0);

                        mrpt::opengl::COpenGLScene scene;
                        renderMoveTree(scene, pi, result, render_options);

                        mrpt::system::createDirectory("./rrt_log_trees");
                        scene.saveToFile(
                                mrpt::format(
                                        "./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",
                                        static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),
                                        static_cast<unsigned int>(rrt_iter_counter)));
                }

        }  // end loop until end conditions

        // [Algo `tp_space_rrt`: Line 17]: Tree back trace
        // ------------------------------------------------------------
        result.success = (result.goal_distance < end_criteria.acceptedDistToTarget);
        result.computation_time = working_time.Tac();

}  // end solve()