#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/Point32.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/GetMap.h>
#include <geometry_msgs/Pose2D.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Float64.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "base_msgs/global_position.h"
// #include "space_key/space_key.h"

#include "map/map.h"
#include "slam/slam.h"
#include "common/common.h"
#include "space_lib/space_lib.h"
#include "real_match_2d/real_match_2d.h"
#include "ceres_matcher/ceres_scan_matcher_2d.h"

#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))

namespace
{

    using namespace hlzn_slam;
    using namespace hlzn_slam::common;

    std::unique_ptr<ros::NodeHandle> nh_;
    ros::Publisher map_pub_, package_init_pub_, point_cloud_pub_, show_laser_pub_;

    /*tf相关*/
    std::unique_ptr<tf::TransformBroadcaster> trans_tfb_;
    std::unique_ptr<tf::TransformListener> tf_;
    ros::Duration transform_tolerance_;
    tf::Transform tf_transform_;

    std::string odom_frame_id_ = "odom";
    std::string base_frame_id_ = "base_link";
    std::string global_frame_id_ = "map";
    /*******/

    /*match*/
    ros::Publisher map_pose_pub_, cost_pub_;
    std::vector<ros::Subscriber> subs_;
    /*******/

    std::unique_ptr<slam::Slam> slam_ptr_ = nullptr;

    std::map<std::string, Eigen::Matrix<float, 4, 4>> transforms_;

    static PointCloud __transformScanData2PointCloudFilter(const sensor_msgs::LaserScanConstPtr &msg, const Eigen::Matrix<float, 4, 4> &transform,
                                                           const float step_angle);

    static bool __getMapInfo(const std::string &map_path, const std::string &yaml_path, Eigen::MatrixXf &map,
                             float &resolution, float &x_origin, float &y_origin);

    static void __locationMsgsCallback(const base_msgs::global_positionConstPtr &msg);
    static void __pubMapPose(const float &x, const float &y, const float &yaw);
    static void __pubCost(const float &cost);
    static void __shutDown();
    static void __start();

    static float probability_plus_;
    static float probability_up_;
    static float search_range_;
    static float search_angle_;
    static float window_size_;
    static float achieve_range_;
    static float achieve_angle_;
    static float submap_size_;
    static float submap_resolution_;

    static bool use_towermatch_;
    static bool use_odom_;
    static bool debug_;

    static float first_search_range_;
    static float first_search_angle_;

    static Json::Value __readLaserConfig()
    {
        std::string path = space::home() + "configure/laser_config.json";

        Json::Value c = space::readConfig(path);
        Json::Value laser;

        space::jsonValueTo(c, laser);

        return laser;
    }

    static bool __readLocationConfig()
    {
        std::string path = space::home() + "configure/location_config.json";

        Json::Value c = space::readConfig(path);
        Json::Value compair = c;

        space::jsonValueTo(c["probability_plus"], probability_plus_) ? false : (c["probability_plus"] = probability_plus_ = 0.001);
        space::jsonValueTo(c["probability_up"], probability_up_) ? false : (c["probability_up"] = probability_up_ = 0.2);
        space::jsonValueTo(c["search_range"], search_range_) ? false : (c["search_range"] = search_range_ = 0.3);
        space::jsonValueTo(c["search_angle"], search_angle_) ? false : (c["search_angle"] = search_angle_ = 0.1);
        space::jsonValueTo(c["window_size"], window_size_) ? false : (c["window_size"] = window_size_ = 20);
        space::jsonValueTo(c["achieve_range"], achieve_range_) ? false : (c["achieve_range"] = achieve_range_ = 0.5);
        space::jsonValueTo(c["achieve_angle"], achieve_angle_) ? false : (c["achieve_angle"] = achieve_angle_ = 0.4);
        space::jsonValueTo(c["submap_size"], submap_size_) ? false : (c["submap_size"] = submap_size_ = 15.0);
        space::jsonValueTo(c["submap_resolution"], submap_resolution_) ? false : (c["submap_resolution"] = submap_resolution_ = 0.05);
        space::jsonValueTo(c["use_towermatch"], use_towermatch_) ? false : (c["use_towermatch"] = use_towermatch_ = true);
        space::jsonValueTo(c["use_odom"], use_odom_) ? false : (c["use_odom"] = use_odom_ = true);
        space::jsonValueTo(c["debug"], debug_) ? false : (c["debug"] = debug_ = false);

        LOG(INFO) << use_odom_;

        space::jsonValueTo(c["first_search_range"], first_search_range_) ? false : (c["first_search_range"] = first_search_range_ = 5.0);
        space::jsonValueTo(c["first_search_angle"], first_search_angle_) ? false : (c["first_search_angle"] = first_search_angle_ = M_PI);

        if (compair != c)
        {
            space::writeConfig(path, c);
        }
        return true;
    }

    static bool __CacluLaserToBaseLinkTransform()
    {
        transforms_.clear();

        Json::Value lasers = __readLaserConfig();

        for (int i = 0; i < lasers.size(); i++)
        {
            float tf_x, tf_y, tf_z, tf_roll, tf_pitch, tf_yaw;
            std::string name;

            bool ok = space::jsonValueTo(lasers[i]["tf_x"], tf_x) &&
                      space::jsonValueTo(lasers[i]["tf_y"], tf_y) &&
                      space::jsonValueTo(lasers[i]["tf_z"], tf_z) &&
                      space::jsonValueTo(lasers[i]["tf_roll"], tf_roll) &&
                      space::jsonValueTo(lasers[i]["tf_pitch"], tf_pitch) &&
                      space::jsonValueTo(lasers[i]["tf_yaw"], tf_yaw) &&
                      space::jsonValueTo(lasers[i]["name"], name);
            if (ok)
            {
                Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(tf_roll, Eigen::Vector3d::UnitX()));
                Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(tf_pitch, Eigen::Vector3d::UnitY()));
                Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(tf_yaw, Eigen::Vector3d::UnitZ()));

                Eigen::Matrix3f rotation_matrix;
                Eigen::Matrix<float, 4, 4> transform;

                rotation_matrix = (yawAngle * pitchAngle * rollAngle).cast<float>();
                transform.rightCols<1>() << tf_x, tf_y, tf_z, 1.0;
                transform.block<3, 3>(0, 0, 3, 3) = rotation_matrix;
                transform.bottomRows<1>() << 0, 0, 0, 1.0;

                transforms_.emplace(std::make_pair(name, transform));
            }
        }
        if (!transforms_.empty())
        {
            return true;
        }
        return false;
    }

    static void __mapPub(std::string map_file, std::string config_path)
    {
        static nav_msgs::GetMap::Response map_res;
        cv::Mat image;
        float x_origin, y_origin, resolution, occupied_thresh, free_thresh;

        image = cv::imread(map_file, cv::IMREAD_GRAYSCALE);

        tf::Quaternion q;
        q.setRPY(0, 0, 0);

        Json::Value c = space::readConfig(config_path);

        bool ok = space::jsonValueTo(c["x_origin"], x_origin) &&
                  space::jsonValueTo(c["y_origin"], y_origin) &&
                  space::jsonValueTo(c["resolution"], resolution) &&
                  space::jsonValueTo(c["occupied_thresh"], occupied_thresh) &&
                  space::jsonValueTo(c["free_thresh"], free_thresh);

        map_res.map.info.map_load_time = ros::Time::now();
        map_res.map.header.frame_id = "map";
        map_res.map.header.stamp = ros::Time::now();
        map_res.map.info.width = image.cols;
        map_res.map.info.height = image.rows;
        map_res.map.info.resolution = resolution;
        map_res.map.info.origin.position.x = -x_origin;
        map_res.map.info.origin.position.y = -y_origin;
        map_res.map.info.origin.position.z = 0.0;
        map_res.map.info.origin.orientation.x = q.x();
        map_res.map.info.origin.orientation.y = q.y();
        map_res.map.info.origin.orientation.z = q.z();
        map_res.map.info.origin.orientation.w = q.w();
        map_res.map.data.resize(map_res.map.info.width * map_res.map.info.height);

        unsigned char *p;
        unsigned char value;
        unsigned char pix = 0;

        for (size_t x = 0; x < image.rows; ++x)
        {
            p = image.ptr<uchar>(x);
            for (size_t y = 0; y < image.cols; ++y)
            {
                pix = *(p + y);
                float cm = float(pix) / 255.f;

                if (cm < 1 - occupied_thresh)
                {
                    value = (1 - cm) * 255;
                }
                else if (cm > 1 - free_thresh)
                {
                    value = 0;
                }
                else
                {
                    value = 0;
                }

                map_res.map.data[MAP_IDX(map_res.map.info.width, y, map_res.map.info.height - x - 1)] = value;
            }
        }

        LOG(INFO) << "Read a " << map_res.map.info.width << " * " << map_res.map.info.height
                  << " @ " << map_res.map.info.resolution << " Map!";
        map_pub_.publish(map_res.map);
    }

    static PointCloud __transformScanData2PointCloud(const sensor_msgs::LaserScanConstPtr &msg, const Eigen::Matrix<float, 4, 4> &transform)
    {
        PointCloud point_cloud;
        float theta = msg->angle_max;
        float offset = msg->angle_max + msg->angle_min;

        for (const float &range : msg->ranges)
        {
            theta -= msg->angle_increment;

            if (range > msg->range_max)
                continue;
            if (range < msg->range_min)
                continue;
            if (theta < msg->angle_min)
                continue;
            if (theta > msg->angle_max)
                continue;
            if (std::isnan(range))
                continue;

            const Eigen::Matrix<float, 4, 1> point(range * cos(theta - offset),
                                                   -range * sin(theta - offset),
                                                   0.0,
                                                   1.0);
            // 将基于传感器坐标系的数据转到基于base_link坐标系
            Eigen::Matrix<float, 4, 1> base = transform * point;

            point_cloud.data.push_back(
                Eigen::Vector3f(base(0), base(1), base(2)));
        }
        point_cloud.time = std::chrono::system_clock::now();
        return point_cloud;
    }

    static common::laserScan __transformROSScanData2CommonScan(const sensor_msgs::LaserScanConstPtr &msg)
    {
        laserScan scan;

        scan.angle_max = msg->angle_max;
        scan.angle_min = msg->angle_min;
        scan.range_max = msg->range_max;
        scan.range_min = msg->range_min;
        scan.ranges = msg->ranges;
        scan.angle_increment = msg->angle_increment;

        return scan;
    }

    static PointCloud __transformScanData2PointCloudFilter(const sensor_msgs::LaserScanConstPtr &msg, const Eigen::Matrix<float, 4, 4> &transform)
    {
        static sensor_msgs::LaserScan last_msg = *msg;
        PointCloud point_cloud;
        float theta = msg->angle_max;
        float offset = msg->angle_max + msg->angle_min;

        for (int i = 0; i < msg->ranges.size(); i++)
        {
            float range = msg->ranges[i];
            float range_last = last_msg.ranges[i];

            theta -= msg->angle_increment;
            if (range > msg->range_max)
                continue;
            if (range < msg->range_min)
                continue;
            if (theta < msg->angle_min)
                continue;
            if (theta > msg->angle_max)
                continue;
            if (std::isnan(range))
                continue;

            if (std::isnan(range_last))
            {
                float dist = hypot((range_last - range) * cos(theta - offset),
                                   (range_last - range) * sin(theta - offset));
                float max = (msg->header.stamp - last_msg.header.stamp).toSec() * 0.75;

                if (dist > max)
                {
                    continue;
                }
            }

            const Eigen::Matrix<float, 4, 1> point(range * cos(theta - offset),
                                                   -range * sin(theta - offset),
                                                   0.0,
                                                   1.0);
            // 将基于传感器坐标系的数据转到基于base_link坐标系
            Eigen::Matrix<float, 4, 1> base = transform * point;

            point_cloud.data.push_back(
                Eigen::Vector3f(base(0), base(1), base(2)));
        }

        point_cloud.time = std::chrono::system_clock::now();
        last_msg = *msg;
        return point_cloud;
    }

    static PointCloud __transformScanData2PointCloudFilter(const sensor_msgs::LaserScanConstPtr &msg, const Eigen::Matrix<float, 4, 4> &transform, const float step_angle)
    {
        PointCloud point_cloud;
        float theta = msg->angle_max;
        float offset = msg->angle_max + msg->angle_min;
        float step = 0;

        for (int i = 0; i < msg->ranges.size(); i++)
        {
            float range = msg->ranges[i];

            theta -= msg->angle_increment;
            step += msg->angle_increment;
            if (step < step_angle)
                continue;
            if (range > msg->range_max)
                continue;
            if (range < msg->range_min)
                continue;
            if (theta < msg->angle_min)
                continue;
            if (theta > msg->angle_max)
                continue;
            if (std::isnan(range))
                continue;

            const Eigen::Matrix<float, 4, 1> point(range * cos(theta - offset),
                                                   -range * sin(theta - offset),
                                                   0.0,
                                                   1.0);
            // 将基于传感器坐标系的数据转到基于base_link坐标系
            Eigen::Matrix<float, 4, 1> base = transform * point;

            point_cloud.data.push_back(
                Eigen::Vector3f(base(0), base(1), base(2)));
            step = 0;
        }

        point_cloud.time = std::chrono::system_clock::now();
        return point_cloud;
    }

    static void __tfMapToOdom(const float &x, const float &y, const float &yaw, ros::Time stamp)
    {
        tf::Stamped<tf::Pose> odom_to_map;
        ros::Time copy_stamp = stamp;

        transform_tolerance_.fromSec(0.03);
        try
        {
            /*tmp_tf,从map原点看base_link的位置*/
            tf::Transform tmp_tf(tf::createQuaternionFromYaw(yaw),
                                 tf::Vector3(x, y, 0.0));
        }
        catch (tf::TransformException)
        {
            // LOG(INFO)<<"Location Failed to subtract base to odom transform";
            return;
        }

        tf_transform_ = tf::Transform(tf::createQuaternionFromYaw(yaw), tf::Vector3(x, y, 0.0));

        ros::Time transform_expiration = (copy_stamp + transform_tolerance_);

        /*发布tf转换关系，latest_tf_.inverse() 得到的是从map原点看odom原点的位置，也就是 odom->map的转换关系*/
        tf::StampedTransform tmp_tf_stamped(tf_transform_.inverse(),
                                            transform_expiration,
                                            base_frame_id_, global_frame_id_);

        trans_tfb_->sendTransform(tmp_tf_stamped);
    }

    common::PointCloud __transformPointCloud(
        const common::PointCloud &point_cloud, const common::Rigid2f &transform)
    {
        common::PointCloud trans_cloud;

        trans_cloud.data.resize(point_cloud.data.size());

        for (int i = 0; i < point_cloud.data.size(); i++)
        {
            Eigen::Vector3f trans;
            common::Rigid2f::Vector vector = point_cloud.data[i].head<2>();

            vector = transform * vector;
            trans = Eigen::Vector3f(vector(0), vector(1), 0.0);
            trans_cloud.data[i] = trans;
        }
        trans_cloud.time = point_cloud.time;

        return trans_cloud;
    }

    static void __laserDataCallback(const sensor_msgs::LaserScanConstPtr &msg)
    {
        // ROS_WARN("space_slam: __laserDataCallback()  get in !!");
        if (slam_ptr_ == nullptr)
        {
            return;
        }
        std::map<std::string, Eigen::Matrix<float, 4, 4>>::iterator it =
            transforms_.find(msg->header.frame_id);

        if (it == transforms_.end())
        {
            return;
        }
        common::PointCloud point_cloud = __transformScanData2PointCloud(msg, it->second);
        common::laserScan scan = __transformROSScanData2CommonScan(msg);

        slam_ptr_->addPointCloud(point_cloud, scan, it->second);

        if (debug_)
        {
            sensor_msgs::PointCloud laser_point_cloud;

            for (const Eigen::Vector3f &point : point_cloud.data)
            {
                geometry_msgs::Point32 tmp;
                tmp.x = point(0);
                tmp.y = point(1);
                tmp.z = 0;
                laser_point_cloud.points.push_back(tmp);
            }

            laser_point_cloud.header.stamp = ros::Time::now();
            laser_point_cloud.header.frame_id = "base_link";

            point_cloud_pub_.publish(laser_point_cloud);
        }

        common::TimeRigid2f time_pose = slam_ptr_->getCurrentPose();
        std::chrono::duration<float> dur = std::chrono::system_clock::now() - time_pose.stamp;

        if (dur.count() < 0.1)
        { // 100ms
            __pubMapPose(time_pose.pose.translation()(0), time_pose.pose.translation()(1),
                         time_pose.pose.rotation().angle());
            __tfMapToOdom(time_pose.pose.translation()(0), time_pose.pose.translation()(1),
                          time_pose.pose.rotation().angle(), msg->header.stamp);
            __pubCost(time_pose.cost);
        }

        { // show cloud
            PointCloud show_cloud = __transformScanData2PointCloudFilter(msg, it->second, 0.017);
            sensor_msgs::PointCloud show_cloud_ros;

            show_cloud = __transformPointCloud(show_cloud, time_pose.pose);
            for (const Eigen::Vector3f &point : show_cloud.data)
            {
                geometry_msgs::Point32 tmp;
                tmp.x = point(0);
                tmp.y = point(1);
                tmp.z = 0;
                show_cloud_ros.points.push_back(tmp);
            }

            show_cloud_ros.header.stamp = ros::Time::now();
            show_cloud_ros.header.frame_id = "map";

            show_laser_pub_.publish(show_cloud_ros);
        }
    }

    static void __odomDataCallback(const nav_msgs::OdometryConstPtr &msg)
    {
        if (slam_ptr_ != nullptr)
        {
            float x = msg->pose.pose.position.x;
            float y = msg->pose.pose.position.y;
            float yaw = tf::getYaw(msg->pose.pose.orientation);

            float vx = msg->twist.twist.linear.x;
            float angular = msg->twist.twist.angular.z;

            slam_ptr_->addOdom2DVelocity(vx, angular);
            slam_ptr_->addOdom2D(x, y, yaw);
        }
    }

    static void __locationMsgsCallback(const base_msgs::global_positionConstPtr &msg)
    {
        enum LOCMODE
        {
            QUIT = 0,
            FAST,
            EXACT,
            CALIBRATION
        };
        bool fast_init = false;

        __shutDown();
        if (msg->mode == QUIT)
        {
            return;
        }
        else if (msg->mode == FAST)
        {
            fast_init = true;
        }
        else
        {
            fast_init = false;
        }
        Eigen::MatrixXf map;
        float resolution, x_origin, y_origin;
        std::string map_path = msg->map_path + ".png";
        std::string yaml_path = msg->yaml_path + ".json";

        __readLocationConfig();
        __mapPub(map_path, yaml_path);
        if (__getMapInfo(map_path, yaml_path, map, resolution, x_origin, y_origin))
        {
            std::unique_ptr<hlzn_slam::Map> global_map =
                std::unique_ptr<hlzn_slam::Map>(
                    new Map(map, resolution, x_origin, y_origin));

            slam_ptr_ = std::unique_ptr<slam::Slam>(
                new slam::Slam(std::move(global_map)));
            slam_ptr_->setParam(search_range_,
                                search_angle_,
                                use_towermatch_,
                                probability_plus_,
                                probability_up_,
                                window_size_,
                                achieve_range_,
                                achieve_angle_,
                                submap_size_,
                                submap_resolution_,
                                use_odom_);
            slam_ptr_->init(common::Rigid2f(
                                {msg->init_position.pose.position.x, msg->init_position.pose.position.y},
                                tf::getYaw(msg->init_position.pose.orientation)),
                            fast_init, first_search_range_, first_search_angle_);
        }
        else
        {
            LOG(INFO) << "[LOCATION]: read map error!";
            return;
        }
        __start();
    }

    static void __pubMapPose(const float &x, const float &y, const float &yaw)
    {
        geometry_msgs::Pose2D map_pose;

        map_pose.x = x;
        map_pose.y = y;
        map_pose.theta = yaw;

        map_pose_pub_.publish(map_pose);
    }

    static void __pubCost(const float &cost)
    {
        std_msgs::Float64 cost_data;

        cost_data.data = cost;

        cost_pub_.publish(cost_data);
    }

    static bool __ReadMapInfo(const std::string &path,
                              float &resolution, float &x_origin, float &y_origin, float &occupied_thresh, float &free_thresh)
    {
        Json::Value c = space::readConfig(path);

        bool ok = space::jsonValueTo(c["x_origin"], x_origin) &&
                  space::jsonValueTo(c["y_origin"], y_origin) &&
                  space::jsonValueTo(c["resolution"], resolution) &&
                  space::jsonValueTo(c["occupied_thresh"], occupied_thresh) &&
                  space::jsonValueTo(c["free_thresh"], free_thresh);

        if (ok)
        {
            return true;
        }
        return false;
    }

    static bool __getMapInfo(const std::string &map_path, const std::string &yaml_path, Eigen::MatrixXf &map,
                             float &resolution, float &x_origin, float &y_origin)
    {
        float occupied_thresh, free_thresh;

        if (!__ReadMapInfo(yaml_path, resolution, x_origin, y_origin, occupied_thresh, free_thresh))
        {
            return false;
        }

        cv::Mat image;

        image = cv::imread(map_path, cv::IMREAD_GRAYSCALE);

        unsigned char *p;
        unsigned char pix = 0;

        map.setZero(image.rows, image.cols);

        for (size_t x = 0; x < image.rows; ++x)
        {
            p = image.ptr<uchar>(x);
            for (size_t y = 0; y < image.cols; ++y)
            {
                pix = *(p + y);
                float cm = float(pix) / 255.f;

                if (cm < 1 - occupied_thresh)
                {
                    map(x, y) = 1 - cm;
                }
                else if (cm > 1 - free_thresh)
                {
                    map(x, y) = 0;
                }
                else
                {
                    map(x, y) = 0;
                }
            }
        }
        return true;
    }

    void match()
    {
    }

    static void __shutDown()
    {
        for (ros::Subscriber &sub : subs_)
        {
            sub.shutdown();
        }
        subs_.clear();
        std_msgs::Int32MultiArray msg;

        msg.data.push_back(2);
        msg.data.push_back(0);
        package_init_pub_.publish(msg);
    }

    static void __start()
    {
        Json::Value lasers = __readLaserConfig();

        __shutDown();
        __CacluLaserToBaseLinkTransform();

        map_pose_pub_ = nh_->advertise<geometry_msgs::Pose2D>("map_pose", 1);
        subs_.push_back(nh_->subscribe<nav_msgs::Odometry>("odom", 1, &__odomDataCallback));
        ROS_WARN("laser.size() = %d", lasers.size());
        for (int i = 0; i < lasers.size(); i++)
        {
            std::string topic = "";

            if (space::jsonValueTo(lasers[i]["topic"], topic))
            {
                subs_.push_back(nh_->subscribe<sensor_msgs::LaserScan>(topic, 1, &__laserDataCallback));
                ROS_WARN("Topic: %s", topic.c_str());
            }
        }
        LOG(INFO) << "[LOCATION] start ok! 11111";

        std_msgs::Int32MultiArray msg;

        msg.data.push_back(2); // = 2;
        msg.data.push_back(1);
        package_init_pub_.publish(msg);
    }

}

int main(int argc, char **argv)
{
    static ros::Subscriber sub;
    ros::init(argc, argv, "space_location_node");

    ROS_WARN(" GET IN space_slam ros_node !!");

    // if(space::key::chk()) {
    //     LOG(WARNING) << "推荐使用正版软件";
    //     space::key::rt(86400);
    //     return 0;
    // }

    trans_tfb_ = std::unique_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());
    tf_ = std::unique_ptr<tf::TransformListener>(new tf::TransformListener());
    nh_ = std::unique_ptr<ros::NodeHandle>(new ros::NodeHandle());
    // 用激光数据激活定位包

    map_pub_ = nh_->advertise<nav_msgs::OccupancyGrid>("map", 1, true);
    package_init_pub_ = nh_->advertise<std_msgs::Int32MultiArray>("/package_init", 1, true);
    point_cloud_pub_ = nh_->advertise<sensor_msgs::PointCloud>("/point_cloud", 1);
    show_laser_pub_ = nh_->advertise<sensor_msgs::PointCloud>("/show_laser", 1);
    cost_pub_ = nh_->advertise<std_msgs::Float64>("/location_cost", 1);

    sub = nh_->subscribe<base_msgs::global_position>("location_msgs", 1, &__locationMsgsCallback);

    ros::spin();
    return 1;
}