add_executable(amcl  src/amcl_node.cpp)

target_link_libraries(amcl

    amcl_sensors amcl_map amcl_pf

    ${Boost_LIBRARIES}

    ${catkin_LIBRARIES}

)

class amcl_node

{

public:

    amcl_node();

    ~amcl_node();

    void runFromBag(const std::string &in_bag_fn);//根据信息记录包来运行amcl

    int process();

    void savePoseToServer();////把位姿信息保存到参数服务器

private:

    std::shared_ptr<tf2_ros::TransformBroadcaster> tfb_;

    std::shared_ptr<tf2_ros::TransformListener> tfl_;

    std::shared_ptr<tf2_ros::Buffer> tf_;

    bool sent_first_transform_;

    tf2::Transform latest_tf_;

    bool latest_tf_valid_;

    static pf_vector_t uniformPoseGenerator(void* arg);

    static std::vector<std::pair<int, int> > free_space_indices;

    // Callbacks

    bool globalLocalizationCallback(std_srvs::Empty::Request& req,

        std_srvs::Empty::Response& res);

    bool nomotionUpdateCallback(std_srvs::Empty::Request& req,

        std_srvs::Empty::Response& res);

    bool setMapCallback(nav_msgs::SetMap::Request& req,

        nav_msgs::SetMap::Response& res);

    void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan);

    void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);

    void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg);

    void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg);

    void handleMapMessage(const nav_msgs::OccupancyGrid& msg);

    void freeMapDependentMemory();

    map_t* convertMap(const nav_msgs::OccupancyGrid& map_msg);

    void updatePoseFromServer();

    void applyInitialPose();

    //parameter for what odom to use

    std::string odom_frame_id_;

    //paramater to store latest odom pose

    geometry_msgs::PoseStamped latest_odom_pose_;

    //parameter for what base to use

    std::string base_frame_id_;

    std::string global_frame_id_;

    bool use_map_topic_;

    bool first_map_only_;

    ros::Duration gui_publish_period;

    ros::Time save_pose_last_time;

    ros::Duration save_pose_period;

    geometry_msgs::PoseWithCovarianceStamped last_published_pose;

    map_t* map_;

    char* mapdata;

    int sx, sy;

    double resolution;

    message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;

    tf2_ros::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;

    ros::Subscriber initial_pose_sub_;

    std::vector< AMCLLaser* > lasers_;

    std::vector< bool > lasers_update_;

    std::map< std::string, int > frame_to_laser_;

    // Particle filter

    pf_t *pf_;

    double pf_err_, pf_z_;

    bool pf_init_;

    pf_vector_t pf_odom_pose_;

    double d_thresh_, a_thresh_;

    int resample_interval_;

    int resample_count_;

    double laser_min_range_;

    double laser_max_range_;

    //Nomotion update control

    bool m_force_update;  // used to temporarily let amcl update samples even when no motion occurs...

    AMCLOdom* odom_;

    AMCLLaser* laser_;

    ros::Duration cloud_pub_interval;

    ros::Time last_cloud_pub_time;

    // For slowing play-back when reading directly from a bag file

    ros::WallDuration bag_scan_period_;

    void requestMap();//请求服务static_server提供map，然后调用handleMapMessage处理地图信息

    // Helper to get odometric pose from transform system

    bool getOdomPose(geometry_msgs::PoseStamped& pose,

        double& x, double& y, double& yaw,

        const ros::Time& t, const std::string& f);

    //time for tolerance on the published transform,

    //basically defines how long a map->odom transform is good for

    ros::Duration transform_tolerance_;

    ros::NodeHandle nh_;

    ros::NodeHandle private_nh_;

    ros::Publisher pose_pub_;

    ros::Publisher particlecloud_pub_;

    ros::ServiceServer global_loc_srv_;

    ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion

    ros::ServiceServer set_map_srv_;

    ros::Subscriber initial_pose_sub_old_;

    ros::Subscriber map_sub_;

    amcl_hyp_t* initial_pose_hyp_;

    bool first_map_received_;

    bool first_reconfigure_call_;

    boost::recursive_mutex configuration_mutex_;

    dynamic_reconfigure::Server<amcl::AMCLConfig> *dsrv_;

    amcl::AMCLConfig default_config_;

    ros::Timer check_laser_timer_;

    int max_beams_, min_particles_, max_particles_;

    double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;

    double alpha_slow_, alpha_fast_;

    double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;

    //beam skip related params

    bool do_beamskip_;

    double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_;

    double laser_likelihood_max_dist_;

    odom_model_t odom_model_type_;

    double init_pose_[];

    double init_cov_[];

    laser_model_t laser_model_type_;

    bool tf_broadcast_;

    void reconfigureCB(amcl::AMCLConfig &config, uint32_t level);

    ros::Time last_laser_received_ts_;

    ros::Duration laser_check_interval_;

    void checkLaserReceived(const ros::TimerEvent& event);

};

int main(int argc, char** argv)

{

  ros::init(argc, argv, "amcl");

  ros::NodeHandle nh;

  // Override default sigint handler

  signal(SIGINT, sigintHandler);

  // Make our node available to sigintHandler

  amcl_node_ptr.reset(new AmclNode());

  if (argc == )

  {

    // run using ROS input

    ros::spin();

  }

  else if ((argc == ) && (std::string(argv[]) == "--run-from-bag"))

  {

    amcl_node_ptr->runFromBag(argv[]);

  }

  // Without this, our boost locks are not shut down nicely

  amcl_node_ptr.reset();

  // To quote Morgan, Hooray!

  return();

}

typedef struct

{

  // Cell size

  double size[];

  // The root node of the tree

  pf_kdtree_node_t *root;

  // The number of nodes in the tree

  int node_count, node_max_count;

  pf_kdtree_node_t *nodes;

  // The number of leaf nodes in the tree

  int leaf_count;

} pf_kdtree_t;

typedef struct

{

int occ_state;// Occupancy state (-1 = free, 0 = unknown, +1 = occ)

double occ_dist;// Distance to the nearest occupied cell

} map_cell_t;

// Description for a map

typedef struct

{

  // Map origin; the map is a viewport onto a conceptual larger map.

  double origin_x, origin_y;

  // Map scale (m/cell)

  double scale;

  // Map dimensions (number of cells)

  int size_x, size_y;

  // The map data, stored as a grid

  map_cell_t *cells;

  // Max distance at which we care about obstacles, for constructing

  // likelihood field

  double max_occ_dist;

} map_t;