curved_path_extractor.H

Go to the documentation of this file.

#ifndef CAJUN_CURVED_PATH_EXTRACTOR_H
#define CAJUN_CURVED_PATH_EXTRACTOR_H

#include "path_extractor.H"
#include "obstacle_free.H"

#include "uc_planner_param.H"
#include <queue>

namespace cajun
{

class curved_path_extractor_t : public path_extractor_t
{

public:
        curved_path_extractor_t (double cell_size_, obstacle_free_t *of_);

private:
        static const unsigned MAX_CHILDREN = 3;

        struct ep_node_t // extraction path node
        {
                double x;
                double y;
                double orient;
                unsigned id; // its id in n possible steps from parent node
                unsigned depth;

                double cost_to_goal; // cost from point to goal
                double cost_from_start; // cost from start to this point
                double total_cost; // total cost

                ep_node_t *parent_node;
                ep_node_t* child_node[MAX_CHILDREN];

        };

        struct cmp_node_t
        {
                bool operator () (ep_node_t const *n1_, ep_node_t const *n2_)
                        { return n1_->total_cost > n2_->total_cost; }
        };
        std::priority_queue<ep_node_t *, std::vector<ep_node_t *>, cmp_node_t> m_work_list;

        obstacle_free_t *m_obstacle_free;

        double m_start_x;
        double m_start_y;

        // steps in which orientation for child nodes needs to be explored
        std::vector <double> m_orient_step;

        // Dicstance between parent and child node
        double m_dis_step_size;

        const uc_planner_param_t *m_uc_param;

        bool extract (double tstamp_, path_t &path_, path_t &detail_path_,
                      double orient_);

        void clear_work_list ();

        ep_node_t* bsd_search (path_t &path_);

        void create_start_node (ep_node_t *start_node_,
                                path_t const &detail_path_, double orient_);

        void create_ith_child (ep_node_t *parent_node, unsigned child_id, ep_node_t* &child_node);


        bool stop_exploring (ep_node_t *node_);


        bool through_bad_part (ep_node_t const *node_);

        bool point_over_bad_part (double x_, double y_);

        bool outside_zone (ep_node_t const *node_);

        bool reached_goal (ep_node_t const *node);

        void form_new_node (ep_node_t *start_node_,
                            ep_node_t *new_node_,
                            unsigned child_id_);

        void extract_path (ep_node_t *start_node, path_t &path_);

        double get_cost_at_xy (double x_, double y_);

        double orientation_to_goal (ep_node_t const *node_);

        bool approaching_wrong (ep_node_t const *start_node_);
        void write_to_file (FILE *fp, ep_node_t *node1, ep_node_t *node2);

        unsigned m_depth;
        unsigned m_count;

};

};

#endif

---