标签：类型 lod 转换 管理所 比较 源码 管理 distance receive
add_executable(amcl src/amcl_node.cpp)
target_link_libraries(amcl
amcl_sensors amcl_map amcl_pf
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
该项目生成一个amcl节点；以及amcl_sensors amcl_map amcl_pf三个库
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_[3]; double init_cov_[3]; 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 == 1) { // run using ROS input ros::spin(); } else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag")) { amcl_node_ptr->runFromBag(argv[2]); } // Without this, our boost locks are not shut down nicely amcl_node_ptr.reset(); // To quote Morgan, Hooray! return(0); }
0.构造函数AmclNode()
——>参数配置：粒子滤波参数,运动模型参数,观测模型参数等
——>updatePoseFromServer()：从参数服务器中获取初始位姿及初始分布
——>pose和particle息发布:
——>创建服务：
——>订阅话题：
——>一个15秒的定时器：AmclNode::checkLaserReceived，检查 15上一次收到激光雷达数据至今是否超过15秒，如超过则报错。
1.requestmap()
——>requestMap：一直请求服务static_map直到成功
——>handleMapMessage()： 1.将受到的msg转换成标准地图，0->-1(不是障碍);100->+1（障碍）;else->0（不明）
2.提取非障碍部分，列入Vector类型的free_space_indices
3.创建粒子滤波器——>updatePoseFromServer()——>初始化粒子滤波器——>初始化传感器（odom，laser）——>applyInitialPose()
2.laserReceived()
——>获取laser对应于baselink的坐标
——>获取baselink对应于odom的坐标
——>根据里程计的变化值+高斯噪音 更新 pf_t中samples的内里程计值（运动模型）
odom->updateAction()
——>根据当前雷达数据更新各里程计对应的权值weights
laser_[laser_index]->updateSensor（）
——>得到滤波结果后,分别在话题/amcl_pose和/ particlecloud上发布位姿和粒子集
1. eig3.c
实现的是一个3x3对称矩阵的特征值与特征向量的计算,首先用Householder矩阵将矩阵变换为三对角矩阵,然后使用ql分解迭代计算 。
2. pf_kdtree.c定义了一个kdtree以及维护方法来管理所有粒子 ：创建、销毁、清除元素、插入元素、计算概率估计、比较、查找、
typedef struct { // Cell size double size[3]; // 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;
3.pf_pdf.c主要定义了一个从给定pdf中采样粒子的方法
4.pf_vector.c定义了三维列向量和三维矩阵和基本的运算方法：加、减、全局和局部坐标系变换、是否NAN或INF
5.pf.c定义了粒子单元pf_sample_t,粒子集pf_sample_set_t,粒子滤波pf_t的数据类型,还有一个 pf_cluster_t表示粒子集的聚类信息,关键函数主要包含如下三个,分别对应粒子滤波中的运动更新,观测更新,重采样三个过程
1. amcl_sencor.cpp
——>定义了基类，以虚函数InitSensor()、UpdateSensor()、UpdateAction()提供接口
2. amcl_laser.cpp
——>定义了激光数据类型,三种观测更新模型(详细见<<概率机器人>>),具体实现了UpdateSensor,用于计算粒子权值
3. amcl_odom.cpp
——>具体实现了基类定义的UpdateAction函数,用于根据运动更新粒子,定义了两种运动模型,差分和全向
——>map中主要定义了概率栅格地图的数据表示
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;
部分参考：https://blog.csdn.net/qq_27753669/article/details/80011156
标签：类型 lod 转换 管理所 比较 源码 管理 distance receive
原文地址：https://www.cnblogs.com/yrm1160029237/p/10304368.html
