[ROS] Move Base (5) Global Planner Code 분석

global_planner

nav_core의 base_global_planner의 plugin이다. class 정의를 살펴보면 다음과 같다.

class GlobalPlanner : public nav_core::BaseGlobalPlanner {
    public:
        GlobalPlanner();
        GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
        ~GlobalPlanner();
        void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
        void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
        bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                      std::vector<geometry_msgs::PoseStamped>& plan);
        bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance,
                      std::vector<geometry_msgs::PoseStamped>& plan);
        bool computePotential(const geometry_msgs::Point& world_point);
        bool getPlanFromPotential(double start_x, double start_y, double end_x, double end_y,
                                  const geometry_msgs::PoseStamped& goal,
                                  std::vector<geometry_msgs::PoseStamped>& plan);
        double getPointPotential(const geometry_msgs::Point& world_point);
        bool validPointPotential(const geometry_msgs::Point& world_point);
        bool validPointPotential(const geometry_msgs::Point& world_point, double tolerance);
        void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
        bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp);

    protected:
        costmap_2d::Costmap2D* costmap_;
        std::string frame_id_;
        ros::Publisher plan_pub_;
        bool initialized_, allow_unknown_;
    private:
        void mapToWorld(double mx, double my, double& wx, double& wy);
        bool worldToMap(double wx, double wy, double& mx, double& my);
        void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my);
        void publishPotential(float* potential);
        double planner_window_x_, planner_window_y_, default_tolerance_;
        boost::mutex mutex_;
        ros::ServiceServer make_plan_srv_;
        PotentialCalculator* p_calc_;
        Expander* planner_;
        Traceback* path_maker_;
        OrientationFilter* orientation_filter_;
        bool publish_potential_;
        ros::Publisher potential_pub_;
        int publish_scale_;
        void outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value);
        float* potential_array_;
        unsigned int start_x_, start_y_, end_x_, end_y_;
        bool old_navfn_behavior_;
        float convert_offset_;
        bool outline_map_;
        dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig> *dsrv_;
        void reconfigureCB(global_planner::GlobalPlannerConfig &config, uint32_t level);
};

여기서 봐야 중점으로 봐야하는건 makePlan을 할 때와, initialize에서 어떤 걸 하는지 중점으로 확인한다.

GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);

initialize에 필요한 인자들을 받는 constructor이다. 받은뒤 initialize를 호출한다.

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)

frameId를 costmap의 frameId로 사용하는 initialize방법이다. costmap에서 frameId를 가져온다.

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id)

initialize를 하는 함수이다.

• initialize flow
  • if (!initialized_)
    • initialize는 한번만 필요하다.
  • unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY()
    • costmap의 Cellsize를 가져온다.
  • if (!old_navfn_behavior_)
    • old_navfn_behavior_ parameter에 따라 convert_offset_값을 설정한다.
  • if (use_quadratic)
    • use_quadratic 에 따라 Calculator 종류를 설정한다.
  • if (use_dijkstra)
    • use_dijkstra가 true이면 dijkstar expansion(planner)를 생성한다.
    • false이면 A* expansion(planner)를 생성한다.
  • if (use_grid_path)
    • use_grid_path가 true면 GridPath로 path_maker를 설정한다.
    • false이면 GradientPath로 path_maker를 설정한다.
  • orientation_filter_ = new OrientationFilter()
    • orientation filter 생성
  • 나머지는 자잘한 설정들이다. (보면 안다.)

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,

std::vector<geometry_msgs::PoseStamped> &plan)

기존 base_global_planner에서 정의된 인터페이스로, tolerance가 정의 되어 있지 않아 Default tolerance로 makePlane을 하게 된다.

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,

double tolerance, std::vector<geometry_msgs::PoseStamped> &plan)

plan을 하기 위한 준비를 하고, getPlanFromPotential을 호출하는 함수이다.

• make plan flow
  • boost::mutex::scoped_lock lock(mutex_)
    • plan을 하는 동안 변수들을 건들게 되니까 mutex를 잠그고 시작한다.
  • if (!initialized_)
    • planner가 initialized됐는지 확인한다.
  • plan.clear()
    • plan을 초기화 한다.(vector 초기화)
  • if (goal.header.frame_id != global_frame)
    • goal의 좌표가 global_frame에 기준한 좌표로 들어왔는지 확인한다.
      • 이건 여기서 알아서 처리해도 좋을거 같은데...
  • if (start.header.frame_id != global_frame)
    • start의 좌표가 global_frame에 기준한 좌표로 들어왔는지 확인한다.
      • 이것도 마찬가지...
  • if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i))
    • meter 단위로 들어온 x, y 좌표를 costmap의 pixel좌표로 transform한다.
  • if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i))
    • goal에대해서도 똑같이 해준다.
  • clearRobotCell(start, start_x_i, start_y_i)
    • 로봇이 있는곳은 obstacle이 아니니까 cell을 초기화 해준다.
  • int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY()
    • costmap 크기를 가져온다.
  • p_calc_->setSize(nx, ny); planner_->setSize(nx, ny); path_maker_->setSize(nx, ny); potential_array_ = new float[nx * ny]
    • plan에 필요한 사이즈를 초기화 한다.
  • if (outline_map_)
    • 맵에 테두리를 그리는 옵션이 들어가면 테두리를 그린다.
  • bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y, nx * ny * 2, potential_array_)
    • A* 또는 Dijkstar로 goal point까지의 potential을 계산한다.
  • if (!old_navfn_behavior_)
    • goal point의 Cell을 초기화 한다.
  • if (publish_potential_)
    • potential map을 publish한다.
  • if (found_legal)
    • 위에 조건들이 부합하면 planner에 plan을 요청하는 함수로 책임을 위임한다.
  • orientation_filter_->processPath(start, plan)
    • orientation filter로 로봇이 plan을 따라갈 때의 orientation을 수정한다.
  • publishPlan(plan)
    • visualization을 위해 plan을 publish한다.

bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,

const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan)

expander(planner)를 호출하는 함수이다.

• get plan from potential flow
  • if (!initialized_)
    • planner가 initialized됐는지 확인한다.
  • plan.clear()
    • plan을 초기화 한다.
    • 앞에서 많이 초기화 하고 주는데 혹시 몰라서 또 해주는 거 같다.
  • path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)
    • path를 생성한다.
  • for (int i = path.size() - 1; i >= 0; i--)
    • 생성된 path들을 pixel단위에서 meter단위로 변경한다.

Expander

global planner에선 실질적으로 meter단위에서 pixel단위로 변환한뒤 Expender가 start point부터 end point까지의 potential을 생성하고, traceback이 potential을 기반으로 path를 만들어내게 된다.

expander의 class interface는 다음과 같다.

class Expander {
    public:
        Expander(PotentialCalculator* p_calc, int nx, int ny) :
                unknown_(true), lethal_cost_(253), neutral_cost_(50), factor_(3.0), p_calc_(p_calc) {
            setSize(nx, ny);
        }
        virtual ~Expander() {}
        virtual bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
                                        int cycles, float* potential) = 0;
        virtual void setSize(int nx, int ny) {
            nx_ = nx;
            ny_ = ny;
            ns_ = nx * ny;
        }
        void setLethalCost(unsigned char lethal_cost) {
            lethal_cost_ = lethal_cost;
        }
        void setNeutralCost(unsigned char neutral_cost) {
            neutral_cost_ = neutral_cost;
        }
        void setFactor(float factor) {
            factor_ = factor;
        }
        void setHasUnknown(bool unknown) {
            unknown_ = unknown;
        }
        void clearEndpoint(unsigned char* costs, float* potential, int gx, int gy, int s){
            int startCell = toIndex(gx, gy);
            for(int i=-s;i<=s;i++){
            for(int j=-s;j<=s;j++){
                int n = startCell+i+nx_*j;
                if(potential[n]<POT_HIGH)
                    continue;
                float c = costs[n]+neutral_cost_;
                float pot = p_calc_->calculatePotential(potential, c, n);
                potential[n] = pot;
            }
            }
        }
    protected:
        inline int toIndex(int x, int y) {
            return x + nx_ * y;
        }
        int nx_, ny_, ns_; /**< size of grid, in pixels */
        bool unknown_;
        unsigned char lethal_cost_, neutral_cost_;
        int cells_visited_;
        float factor_;
        PotentialCalculator* p_calc_;

};

• virtual bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, float* potential)
  • start부터 goal까지의 potential을 계산한다.
• virtual void setSize(int nx, int ny)
  • map 크기를 설정한다.
• void clearEndpoint(unsigned char* costs, float* potential, int gx, int gy, int s)
  • goal point를 clear한다.
• inline int toIndex(int x, int y)
  • potential map에서 x,y의 index를 얻는 함수이다.(2차원 map → 1차원)

Expander는 2개의 class로 정의 되어있다.(Dijkstar , A*) 다음과 같다.

Dijkstar(Expander)

• start에서 goal을 찾아갈 때 Dijkstar algorithm을 사용한다.
• 내부 구현은 Dijkstar 그대로이다.

A* (Expander)

• start에서 goal을 찾아갈 때 A* algorithm을 사용한다.
• 내부 구현은 A* 그대로다.

Traceback

global planner에선 실질적으로 meter단위에서 pixel단위로 변환, Expander의 potential 생성을 하고 실질적인 path를 생성하는건 Traceback이 담당하게 된다. traceback의 class interface는 다음과 같다.

class Traceback {
    public:
        Traceback(PotentialCalculator* p_calc) : p_calc_(p_calc) {}
        virtual ~Traceback() {}
        virtual bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) = 0;
        virtual void setSize(int xs, int ys) {
            xs_ = xs;
            ys_ = ys;
        }
        inline int getIndex(int x, int y) {
            return x + y * xs_;
        }
        void setLethalCost(unsigned char lethal_cost) {
            lethal_cost_ = lethal_cost;
        }
    protected:
        int xs_, ys_;
        unsigned char lethal_cost_;
        PotentialCalculator* p_calc_;
}

• p_calc
  • Potential을 계산할 때 사용하는 potential calcuator이다.
• getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path)
  • potential map이 주어졌을 때, path를 생성하는 함수이다.
• setSize(int xs, int ys)
  • path를 찾기 위한 map의 크기를 세팅한다.
• getIndex(int x, int y)
  • potential map에서 x,y의 index를 얻는 함수이다.(2차원 map → 1차원)
• setLethalCost(unsigned char lethal_cost)
  • lethalcost를 설정한다.

GridPath(Traceback)

cell 단위로 grid한 path가 나오는 traceback이다. goal point에서 start로 역으로 오면서 path를 그리게 된다.

bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path)

• 최소 값을 찾으며 역행해서 해당하는 cell index를 path에 넣어준다.

GradientPath(Traceback)

path가 연속적으로 (스무스하게)나오는 trace back이다. GridPath와 마찬가지로 goal point에서 start로 ㅇ역으로 오면서 path를 그린다.

bool GradientPath::getPath(float* potential, double start_x, double start_y, double goal_x, double goal_y, std::vector<std::pair<float, float> >& path)

• 최소 값을 찾으며 역행하여 path를 생성한다.