planner_core.h

Go to the documentation of this file.

#ifndef _PLANNERCORE_H
#define _PLANNERCORE_H
/*********************************************************************
 *
 *
 *
 *********************************************************************/
#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
#include <vector>
#include <nav_core/base_global_planner.h>
#include <nav_msgs/GetPlan.h>

#include <tuple>
#include <vector>
#include "dynamicvoronoi.h"

#include <voronoi_planner/VoronoiPlannerConfig.h>

namespace voronoi_planner {

class VoronoiPlanner : public nav_core::BaseGlobalPlanner {
    public:
        VoronoiPlanner();

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

        ~VoronoiPlanner();

        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 findPath(std::vector<std::pair<float, float> > *path,
                      int init_x, int init_y,
                      int goal_x, int goal_y,
                      DynamicVoronoi *voronoi,
                      bool check_is_voronoi_cell,
                      bool stop_at_voronoi );

        void smoothPath(std::vector<std::pair<float, float> > *path);


        void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);

        bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp);

        void publishVoronoiGrid(DynamicVoronoi *voronoi);

    protected:

        costmap_2d::Costmap2D* costmap_;
        std::string frame_id_;
        ros::Publisher plan_pub_;
        bool initialized_;

        bool publish_voronoi_grid_;
        ros::Publisher voronoi_grid_pub_;

        bool smooth_path_;
        float weight_data_;
        float weight_smooth_;



    private:
        void mapToWorld(double mx, double my, double& wx, double& wy);
        bool worldToMap(double wx, double wy, double& mx, double& my);
        void clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my);

        double planner_window_x_, planner_window_y_, default_tolerance_;
        std::string tf_prefix_;
        boost::mutex mutex_;
        ros::ServiceServer make_plan_srv_;

        DynamicVoronoi voronoi_;

        void outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value);
        unsigned char* cost_array_;
        unsigned int start_x_, start_y_, end_x_, end_y_;


        dynamic_reconfigure::Server<voronoi_planner::VoronoiPlannerConfig> *dsrv_;
        void reconfigureCB(voronoi_planner::VoronoiPlannerConfig &config, uint32_t level);
        void costmapUpdateCallback(const map_msgs::OccupancyGridUpdate::ConstPtr& msg);

};

} //end namespace voronoi_planner

#endif