├── .gitattributes ├── CMakeLists.txt ├── README.md ├── data ├── log │ ├── ascii-robotdata2.log │ ├── ascii-robotdata3.log │ ├── ascii-robotdata4.log │ ├── robotdata1.log │ ├── robotdata1.log.gz │ ├── robotdata2.log.gz │ ├── robotdata3.log.gz │ ├── robotdata4.log.gz │ ├── robotdata5.log │ └── robotdata5.log.gz └── map │ ├── instruct.txt │ ├── robotmovie1.gif │ ├── wean.dat │ ├── wean.dat.gz │ └── wean.gif ├── img └── MCL.gif ├── include ├── MCL_localization.h ├── load_log.h ├── load_map.h └── localization_node.h ├── launch └── mcl_localization.launch ├── package.xml ├── rviz └── localization.rviz ├── src ├── MCL_localization.cpp ├── load_log.cpp ├── load_map.cpp └── localization_node.cpp └── urdf └── three_wheels_robot.urdf /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pf_localization) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | tf 15 | sensor_msgs 16 | nav_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # std_msgs # Or other packages containing msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES pf_localization 111 | # CATKIN_DEPENDS ros_cpp ros_py std_msg 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | include_directories( 122 | include/ 123 | # ${BULLET_INCLUDE_DIRS} 124 | # ${catkin_INCLUDE_DIRS} 125 | 126 | include ${catkin_INCLUDE_DIRS} 127 | # ./include/pf_localization 128 | # ${catkin_INCLUDE_DIRS} 129 | ) 130 | 131 | ## Declare a C++ library 132 | # add_library(${PROJECT_NAME} 133 | # src/${PROJECT_NAME}/pf_localization.cpp 134 | # ) 135 | 136 | ## Add cmake target dependencies of the library 137 | ## as an example, code may need to be generated before libraries 138 | ## either from message generation or dynamic reconfigure 139 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | ## Declare a C++ executable 142 | ## With catkin_make all packages are built within a single CMake context 143 | ## The recommended prefix ensures that target names across packages don't collide 144 | # add_executable(${PROJECT_NAME}_node src/pf_localization_node.cpp) 145 | 146 | ## Rename C++ executable without prefix 147 | ## The above recommended prefix causes long target names, the following renames the 148 | ## target back to the shorter version for ease of user use 149 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 150 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 151 | 152 | ## Add cmake target dependencies of the executable 153 | ## same as for the library above 154 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 155 | 156 | ## Specify libraries to link a library or executable target against 157 | # target_link_libraries(${PROJECT_NAME}_node 158 | # ${catkin_LIBRARIES} 159 | # ) 160 | 161 | ############# 162 | ## Install ## 163 | ############# 164 | 165 | # all install targets should use catkin DESTINATION variables 166 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 167 | 168 | ## Mark executable scripts (Python etc.) for installation 169 | ## in contrast to setup.py, you can choose the destination 170 | # install(PROGRAMS 171 | # scripts/my_python_script 172 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark executables for installation 176 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 177 | # install(TARGETS ${PROJECT_NAME}_node 178 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark libraries for installation 182 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 183 | # install(TARGETS ${PROJECT_NAME} 184 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 185 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 186 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 187 | # ) 188 | 189 | ## Mark cpp header files for installation 190 | # install(DIRECTORY include/${PROJECT_NAME}/ 191 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 192 | # FILES_MATCHING PATTERN "*.h" 193 | # PATTERN ".svn" EXCLUDE 194 | # ) 195 | 196 | ## Mark other files for installation (e.g. launch and bag files, etc.) 197 | # install(FILES 198 | # # myfile1 199 | # # myfile2 200 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 201 | # ) 202 | 203 | ############# 204 | ## Testing ## 205 | ############# 206 | 207 | ## Add gtest based cpp test target and link libraries 208 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pf_localization.cpp) 209 | # if(TARGET ${PROJECT_NAME}-test) 210 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 211 | # endif() 212 | 213 | ## Add folders to be run by python nosetests 214 | # catkin_add_nosetests(test) 215 | 216 | 217 | MESSAGE(STATUS "This is BINARY dir " ${pf_localization_BINARY_DIR}) 218 | MESSAGE(STATUS "This is SOURCE dir " ${pf_localization_SOURCE_DIR}) 219 | 220 | # SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/../src/pf_localization/bin) 221 | 222 | add_executable(localization_node src/localization_node.cpp 223 | src/MCL_localization.cpp 224 | src/load_map.cpp 225 | src/load_log.cpp) 226 | 227 | target_link_libraries(localization_node ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # The Homework of Probabilistic Robotics 2 | 3 | ## Task Description 4 | 　　The goal of this homework is to become familiar with robot localization and particle filtering.You will be implementing a global localization for a lost robot (global meaning that you do not know the initial pose of the robot). You may implement this using any programming language (there is no real-time-ness requirement).Feel free to utilize any techniques that we have discussed in class,as well as extension discussed in Probabilistic Robotics or elsewhere.In addition to the readings for lecture, Chapters 5 and 6 of Probabilistic Robotics may be helpful for this assignment. 5 | 6 | 　　Your lost robot is operating in a building with nothing but odometry and a laser range finder.Fortunately, you have a map of and a deep understanding of particle filtering to help it localize.The data directory that you received with this handout has the following files: 7 | 8 | + instruct.txt –Format description for the map and the data logs. 9 | + robotdataN.log.gz –Five data logs (odometry and laser data). 10 | + wean.dat.gz –Map for localization. 11 | + wean.gif –Image of map (just for your info). 12 | + robotmovie1.gif –Animation of data log 1 (just for your info). 13 | 14 | 15 | ## 1.Prerequisites 16 | + Ubuntu 64-bit 16.04. [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu). 17 | 18 | ## 2.Build on ROS 19 | + Clone this repository to your catkin workspace and catkin_make. 20 | ``` 21 | cd ${YOUR_WORKSPACE_PATH}/src 22 | git clone https://github.com/Mesywang/Particle-Filter-Localization.git 23 | ``` 24 | + Change the name of the folder that you just cloned to "pf_localization" , which matches the name of ROS package. 25 | ``` 26 | cd ../ 27 | catkin_make 28 | ``` 29 | + Modify the **value** of the parameter **package_path_param** in line 16 of launch/mcl_localization.launch to the path where the pf_localization package is located. E.g : **value="/home/wsy/catkin_ws/src/"** 30 | 31 | + Run the Simulation : roslaunch pf_localization mcl_localization.launch 32 | 33 | + The result 34 | 35 | 　　As shown in the following GIF, the pink arrows represent the set of particles, and the motion of black robot model represents the pose of the actual robot estimated by the MCL algorithm in the map. 36 |
Monte Carlo Localization
37 |
38 | 39 |
40 | -------------------------------------------------------------------------------- /data/log/robotdata1.log.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mesywang/Particle-Filter-Localization/5d05ce9462d71628dcd127a051b088e147030d04/data/log/robotdata1.log.gz -------------------------------------------------------------------------------- /data/log/robotdata2.log.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mesywang/Particle-Filter-Localization/5d05ce9462d71628dcd127a051b088e147030d04/data/log/robotdata2.log.gz -------------------------------------------------------------------------------- /data/log/robotdata3.log.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mesywang/Particle-Filter-Localization/5d05ce9462d71628dcd127a051b088e147030d04/data/log/robotdata3.log.gz -------------------------------------------------------------------------------- /data/log/robotdata4.log.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mesywang/Particle-Filter-Localization/5d05ce9462d71628dcd127a051b088e147030d04/data/log/robotdata4.log.gz -------------------------------------------------------------------------------- /data/log/robotdata5.log.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mesywang/Particle-Filter-Localization/5d05ce9462d71628dcd127a051b088e147030d04/data/log/robotdata5.log.gz -------------------------------------------------------------------------------- /data/map/instruct.txt: -------------------------------------------------------------------------------- 1 | 2 | MAP FILE FORMAT: 3 | 4 | It is pretty self-explanatory: 5 | -1 = don't know 6 | any value in [0;1] is a probability for occupancy: 7 | 1 = occupied with probability 1 8 | 0 = unoccupied with probability 1 9 | 0.5 = occupied with probability 0.5 10 | The function in bee-map.c should make reading the map pretty easy. 11 | 12 | 13 | LOG DATA FILE FORMAT: 14 | 15 | In general, x and y coordinates are in centimeters, thetas are in 16 | radians, range values are in centimeters 17 | 18 | The laser on the robot is approximately 25 cm offset forward from the 19 | true center of the robot. 20 | 21 | Entry Type #1 (odometry): 22 | 23 | O x y theta ts 24 | 25 | x y theta - coordinates of the robot in standard odometry frame 26 | ts - timestamp of odometry reading (0 at start of run) 27 | 28 | Entry Type #2 (laser) 29 | 30 | L x y theta xl yl thetal r1 ... r180 ts 31 | 32 | x y theta - coodinates of the robot in standard odometry frame when 33 | laser reading was taken (interpolated) 34 | xl yl thetal - coordinates of the *laser* in standard odometry frame 35 | when the laser reading was taken (interpolated) 36 | r1 .. r180 - 180 range readings of laser in cm. The 180 readings span 37 | 180 degrees *STARTING FROM THE RIGHT AND GOING LEFT* Just like angles, 38 | the laser readings are in counterclockwise order. 39 | ts - timestamp of laser reading 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /data/map/robotmovie1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mesywang/Particle-Filter-Localization/5d05ce9462d71628dcd127a051b088e147030d04/data/map/robotmovie1.gif -------------------------------------------------------------------------------- /data/map/wean.dat.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mesywang/Particle-Filter-Localization/5d05ce9462d71628dcd127a051b088e147030d04/data/map/wean.dat.gz -------------------------------------------------------------------------------- /data/map/wean.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mesywang/Particle-Filter-Localization/5d05ce9462d71628dcd127a051b088e147030d04/data/map/wean.gif -------------------------------------------------------------------------------- /img/MCL.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mesywang/Particle-Filter-Localization/5d05ce9462d71628dcd127a051b088e147030d04/img/MCL.gif -------------------------------------------------------------------------------- /include/MCL_localization.h: -------------------------------------------------------------------------------- 1 | #ifndef MCL_LOCALIZATION_H 2 | #define MCL_LOCALIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "load_map.h" 8 | #include "load_log.h" 9 | #include "geometry_msgs/PoseArray.h" 10 | #include "geometry_msgs/PoseStamped.h" 11 | #include "tf/tf.h" 12 | #include 13 | 14 | 15 | #define PARTICLES_NUM 3000 16 | #define pi 3.1415926536 17 | #define ODOM_DATA 0 18 | #define LIDAR_DATA 1 19 | #define LASER_BEAM_NUM 180 20 | 21 | 22 | typedef struct 23 | { 24 | float x; 25 | float y; 26 | float theta; 27 | }robot_state; 28 | 29 | 30 | typedef struct 31 | { 32 | float x; 33 | float y; 34 | float theta; 35 | float weight; 36 | }particle_state; 37 | 38 | 39 | typedef struct 40 | { 41 | float x_front; 42 | float y_front; 43 | float theta_front; 44 | float x_rear; 45 | float y_rear; 46 | float theta_rear; 47 | }motion_model; 48 | 49 | 50 | typedef struct 51 | { 52 | float* readings; 53 | }lidar_measure; 54 | 55 | 56 | class PFLocalization 57 | { 58 | public: 59 | 60 | PFLocalization(map_type* mapdata,vector& logdata,ros::NodeHandle node); 61 | ~PFLocalization(); 62 | void InitParticles(); 63 | void MCLAlgorithm(); 64 | void CalRobotPose(); 65 | void Visualize(); 66 | 67 | 68 | private: 69 | particle_state SampleMotionModelOdometry(particle_state particle); 70 | float SampleStandardNormalDistribution(float var); 71 | float MeasurementBeamModel(particle_state particle); 72 | float MeasurementScoreModel(particle_state particle); 73 | float ProbMeasurementHit(float zkt_star, float zkt); 74 | float ProbMeasurementShort(float zkt_star, float zkt); 75 | float ProbMeasurementMaxVal(float zkt); 76 | float ProbMeasurementRandVal(float zkt); 77 | void LowVarianceSampler(); 78 | 79 | 80 | ros::Publisher publish_particlecloud_; 81 | ros::Publisher publish_pose_; 82 | ros::NodeHandle node_; 83 | map_type* map_; 84 | vector log_data_; 85 | vector particles_; 86 | geometry_msgs::PoseArray particles_ros_; 87 | geometry_msgs::PoseStamped robot_ros_; 88 | robot_state robot_pose_; 89 | motion_model motion_; 90 | lidar_measure measurement_; 91 | 92 | float alpha1_,alpha2_,alpha3_,alpha4_; 93 | float sigmahit_, lambdashort_; 94 | float z_hit_,z_short_,z_rand_,z_max_; 95 | float threshold_, map_threshold_, obstacle_threshold_, lidar_offset_; 96 | int numParticles_, ray_tracing_step_, resolution_, lidar_range_max_; 97 | 98 | }; 99 | 100 | 101 | #endif 102 | 103 | 104 | -------------------------------------------------------------------------------- /include/load_log.h: -------------------------------------------------------------------------------- 1 | #ifndef LOAD_LOG_H 2 | #define LOAD_LOG_H 3 | 4 | 5 | #include "load_map.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #define total_readings 180 13 | 14 | using namespace std; 15 | 16 | 17 | typedef struct 18 | { 19 | int data_type; // 0-里程计数据,1-激光雷达数据 20 | float x_robot,y_robot,theta_robot; // 机器人位姿 21 | float x_lidar,y_lidar,theta_lidar; // 激光雷达位姿 22 | float* readings; // 激光雷达读数 23 | float time_stamp; // 时间戳 24 | }log_data; 25 | 26 | 27 | class LoadLog 28 | { 29 | public: 30 | LoadLog(const char* log_str); 31 | ~LoadLog(); 32 | int ReadFromData(const char* logfile, vector& logfile_data); 33 | vector GetLog(); 34 | void ShowLogData(); 35 | 36 | private: 37 | vector log; 38 | }; 39 | 40 | 41 | #endif -------------------------------------------------------------------------------- /include/load_map.h: -------------------------------------------------------------------------------- 1 | #ifndef LOAD_MAP_H 2 | #define LOAD_MAP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "nav_msgs/MapMetaData.h" 9 | #include "nav_msgs/OccupancyGrid.h" 10 | 11 | 12 | 13 | typedef struct 14 | { 15 | int resolution; //地图分辨率 16 | int size_x,size_y; //地图尺寸 17 | int min_x,min_y,max_x,max_y; //地图有效区域 18 | float offset_x, offset_y; //地图偏移量 19 | float** prob; //地图概率值 20 | } map_type; 21 | 22 | 23 | class LoadMap 24 | { 25 | public: 26 | LoadMap(const char* map_str); //LoadMap类构造函数 27 | ~LoadMap(); //LoadMap类析构函数 28 | int ReadFromData(const char* map_name); //从wean.data中读取数据 29 | void PublishMap(ros::NodeHandle node_); //将地图转换格式,并发布topic在rviz中显示 30 | map_type* GetMap(); //将地图传递到类外 31 | 32 | private: 33 | map_type* map; 34 | ros::Publisher map_pub; 35 | ros::Publisher map_meta_pub; 36 | }; 37 | 38 | #endif 39 | 40 | 41 | -------------------------------------------------------------------------------- /include/localization_node.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCALIZATION_NODE_H 2 | #define LOCALIZATION_NODE_H 3 | 4 | #include 5 | #include 6 | #include "MCL_localization.h" 7 | #include "load_map.h" 8 | #include "load_log.h" 9 | 10 | 11 | 12 | 13 | #endif -------------------------------------------------------------------------------- /launch/mcl_localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pf_localization 4 | 0.0.0 5 | The pf_localization package 6 | 7 | 8 | 9 | 10 | wsy 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | tf 56 | sensor_msgs 57 | nav_msgs 58 | 59 | roscpp 60 | rospy 61 | std_msgs 62 | 63 | roscpp 64 | rospy 65 | std_msgs 66 | tf 67 | sensor_msgs 68 | nav_msgs 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /rviz/localization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Map1 10 | - /PoseArray1 11 | - /Pose1 12 | Splitter Ratio: 0.5 13 | Tree Height: 775 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679016 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.0299999993 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 0.699999988 57 | Class: rviz/Map 58 | Color Scheme: raw 59 | Draw Behind: false 60 | Enabled: true 61 | Name: Map 62 | Topic: map 63 | Unreliable: false 64 | Use Timestamp: false 65 | Value: true 66 | - Alpha: 1 67 | Arrow Length: 0.300000012 68 | Axes Length: 0.300000012 69 | Axes Radius: 0.00999999978 70 | Class: rviz/PoseArray 71 | Color: 255; 0; 255 72 | Enabled: true 73 | Head Length: 0.0700000003 74 | Head Radius: 0.0299999993 75 | Name: PoseArray 76 | Shaft Length: 0.230000004 77 | Shaft Radius: 0.00999999978 78 | Shape: Arrow (Flat) 79 | Topic: /particle_cloud 80 | Unreliable: false 81 | Value: true 82 | - Alpha: 1 83 | Axes Length: 1 84 | Axes Radius: 0.100000001 85 | Class: rviz/Pose 86 | Color: 0; 255; 127 87 | Enabled: true 88 | Head Length: 0.300000012 89 | Head Radius: 0.100000001 90 | Name: Pose 91 | Shaft Length: 1 92 | Shaft Radius: 0.0500000007 93 | Shape: Arrow 94 | Topic: /robot_pose 95 | Unreliable: false 96 | Value: true 97 | - Class: rviz/Axes 98 | Enabled: true 99 | Length: 1 100 | Name: Axes 101 | Radius: 0.100000001 102 | Reference Frame: 103 | Value: true 104 | - Alpha: 1 105 | Class: rviz/RobotModel 106 | Collision Enabled: false 107 | Enabled: true 108 | Links: 109 | A_infrared_sensor: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | Value: true 114 | A_motor: 115 | Alpha: 1 116 | Show Axes: false 117 | Show Trail: false 118 | Value: true 119 | A_pillar: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | Value: true 124 | A_wheel: 125 | Alpha: 1 126 | Show Axes: false 127 | Show Trail: false 128 | Value: true 129 | All Links Enabled: true 130 | B_infrared_sensor: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | Value: true 135 | B_motor: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | Value: true 140 | B_pannel: 141 | Alpha: 1 142 | Show Axes: false 143 | Show Trail: false 144 | Value: true 145 | B_pillar: 146 | Alpha: 1 147 | Show Axes: false 148 | Show Trail: false 149 | Value: true 150 | B_wheel: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | Value: true 155 | C_infrared_sensor: 156 | Alpha: 1 157 | Show Axes: false 158 | Show Trail: false 159 | Value: true 160 | C_motor: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | Value: true 165 | C_pannel: 166 | Alpha: 1 167 | Show Axes: false 168 | Show Trail: false 169 | Value: true 170 | C_pillar: 171 | Alpha: 1 172 | Show Axes: false 173 | Show Trail: false 174 | Value: true 175 | C_wheel: 176 | Alpha: 1 177 | Show Axes: false 178 | Show Trail: false 179 | Value: true 180 | Expand Joint Details: false 181 | Expand Link Details: false 182 | Expand Tree: false 183 | Lidar_pannel: 184 | Alpha: 1 185 | Show Axes: false 186 | Show Trail: false 187 | Value: true 188 | Link Tree Style: Links in Alphabetic Order 189 | Pi_pannel: 190 | Alpha: 1 191 | Show Axes: false 192 | Show Trail: false 193 | Value: true 194 | alarm_base: 195 | Alpha: 1 196 | Show Axes: false 197 | Show Trail: false 198 | Value: true 199 | alarm_light: 200 | Alpha: 1 201 | Show Axes: false 202 | Show Trail: false 203 | Value: true 204 | arduino_board: 205 | Alpha: 1 206 | Show Axes: false 207 | Show Trail: false 208 | Value: true 209 | base_imu_link: 210 | Alpha: 1 211 | Show Axes: false 212 | Show Trail: false 213 | Value: true 214 | base_link: 215 | Alpha: 1 216 | Show Axes: false 217 | Show Trail: false 218 | Value: true 219 | battery: 220 | Alpha: 1 221 | Show Axes: false 222 | Show Trail: false 223 | Value: true 224 | blue_A_pillar: 225 | Alpha: 1 226 | Show Axes: false 227 | Show Trail: false 228 | Value: true 229 | blue_B_pillar: 230 | Alpha: 1 231 | Show Axes: false 232 | Show Trail: false 233 | Value: true 234 | blue_C_pillar: 235 | Alpha: 1 236 | Show Axes: false 237 | Show Trail: false 238 | Value: true 239 | drop_sensor: 240 | Alpha: 1 241 | Show Axes: false 242 | Show Trail: false 243 | Value: true 244 | laser_frame: 245 | Alpha: 1 246 | Show Axes: false 247 | Show Trail: false 248 | Value: true 249 | left_blue_A_pillar: 250 | Alpha: 1 251 | Show Axes: false 252 | Show Trail: false 253 | Value: true 254 | left_blue_B_pillar: 255 | Alpha: 1 256 | Show Axes: false 257 | Show Trail: false 258 | Value: true 259 | left_blue_pillar: 260 | Alpha: 1 261 | Show Axes: false 262 | Show Trail: false 263 | Value: true 264 | pi_board: 265 | Alpha: 1 266 | Show Axes: false 267 | Show Trail: false 268 | Value: true 269 | right_blue_A_pillar: 270 | Alpha: 1 271 | Show Axes: false 272 | Show Trail: false 273 | Value: true 274 | right_blue_B_pillar: 275 | Alpha: 1 276 | Show Axes: false 277 | Show Trail: false 278 | Value: true 279 | right_blue_pillar: 280 | Alpha: 1 281 | Show Axes: false 282 | Show Trail: false 283 | Value: true 284 | white_A_pillar: 285 | Alpha: 1 286 | Show Axes: false 287 | Show Trail: false 288 | Value: true 289 | white_B_pillar: 290 | Alpha: 1 291 | Show Axes: false 292 | Show Trail: false 293 | Value: true 294 | white_C_pillar: 295 | Alpha: 1 296 | Show Axes: false 297 | Show Trail: false 298 | Value: true 299 | white_D_pillar: 300 | Alpha: 1 301 | Show Axes: false 302 | Show Trail: false 303 | Value: true 304 | white_E_pillar: 305 | Alpha: 1 306 | Show Axes: false 307 | Show Trail: false 308 | Value: true 309 | white_F_pillar: 310 | Alpha: 1 311 | Show Axes: false 312 | Show Trail: false 313 | Value: true 314 | white_G_pillar: 315 | Alpha: 1 316 | Show Axes: false 317 | Show Trail: false 318 | Value: true 319 | Name: RobotModel 320 | Robot Description: robot_description 321 | TF Prefix: "" 322 | Update Interval: 0 323 | Value: true 324 | Visual Enabled: true 325 | Enabled: true 326 | Global Options: 327 | Background Color: 48; 48; 48 328 | Default Light: true 329 | Fixed Frame: map 330 | Frame Rate: 30 331 | Name: root 332 | Tools: 333 | - Class: rviz/Interact 334 | Hide Inactive Objects: true 335 | - Class: rviz/MoveCamera 336 | - Class: rviz/Select 337 | - Class: rviz/FocusCamera 338 | - Class: rviz/Measure 339 | - Class: rviz/SetInitialPose 340 | Topic: /initialpose 341 | - Class: rviz/SetGoal 342 | Topic: /move_base_simple/goal 343 | - Class: rviz/PublishPoint 344 | Single click: true 345 | Topic: /clicked_point 346 | Value: true 347 | Views: 348 | Current: 349 | Class: rviz/ThirdPersonFollower 350 | Distance: 115.186485 351 | Enable Stereo Rendering: 352 | Stereo Eye Separation: 0.0599999987 353 | Stereo Focal Distance: 1 354 | Swap Stereo Eyes: false 355 | Value: false 356 | Focal Point: 357 | X: 48.4389687 358 | Y: 15.5593414 359 | Z: 4.05411301e-05 360 | Focal Shape Fixed Size: true 361 | Focal Shape Size: 0.0500000007 362 | Invert Z Axis: false 363 | Name: Current View 364 | Near Clip Distance: 0.00999999978 365 | Pitch: 1.54479635 366 | Target Frame: 367 | Value: ThirdPersonFollower (rviz) 368 | Yaw: 4.71675205 369 | Saved: ~ 370 | Window Geometry: 371 | Displays: 372 | collapsed: false 373 | Height: 1056 374 | Hide Left Dock: false 375 | Hide Right Dock: true 376 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000393fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000393000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cd0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 377 | Selection: 378 | collapsed: false 379 | Time: 380 | collapsed: false 381 | Tool Properties: 382 | collapsed: false 383 | Views: 384 | collapsed: true 385 | Width: 1853 386 | X: 1987 387 | Y: 24 388 | -------------------------------------------------------------------------------- /src/MCL_localization.cpp: -------------------------------------------------------------------------------- 1 | #include "MCL_localization.h" 2 | 3 | 4 | using namespace std; 5 | 6 | //PFLocalization类的构造函数 7 | PFLocalization::PFLocalization(map_type* mapdata,vector& logdata,ros::NodeHandle node) 8 | { 9 | 10 | this->map_ = mapdata; //地图 11 | this->log_data_ = logdata; //里程计和雷达数据 12 | this->numParticles_ = PARTICLES_NUM; //粒子数 13 | this->map_threshold_ = 0.95; //初始撒随机粒子时的地图阈值，保证粒子都在地图有效区域内 14 | this->node_ = node; //ROS句柄 15 | this->resolution_ = 10; //地图分辨率10cm 16 | this->lidar_offset_ = 25; //激光雷达安装偏移量25cm 17 | this->lidar_range_max_ = 1000; //雷达最大有效数据1000cm 18 | this->ray_tracing_step_ = 1; //ray tracing步长 19 | this->obstacle_threshold_ = 0.2; //地图概率小于此值则认为是障碍物 20 | 21 | 22 | this->alpha1_ = 0.025; //里程计运动模型参数--旋转 23 | this->alpha2_ = 0.025; //里程计运动模型参数--旋转 24 | this->alpha3_ = 0.4; //里程计运动模型参数--平移 25 | this->alpha4_ = 0.4; //里程计运动模型参数--平移 26 | 27 | 28 | this->sigmahit_ = 2; //BeamModel中高斯分布的方差 29 | this->lambdashort_ = 1.5; //BeamModel中指数分布的参数 30 | 31 | this->z_hit_ = 0.8; //BeamModel中高斯分布Phit的权重 32 | this->z_short_ = 0.2; //BeamModel中指数分布Pshort的权重 33 | this->z_rand_ = 0.0; //BeamModel中均匀分布Prand的权重 34 | this->z_max_ = 0.0; //BeamModel中均匀分布Pmax的权重 35 | 36 | //发布粒子集位姿 37 | publish_particlecloud_ = node_.advertise("particle_cloud", 1, true); 38 | this->particles_ros_.header.stamp = ros::Time::now(); 39 | this->particles_ros_.header.frame_id = "map"; 40 | this->particles_ros_.poses.resize(numParticles_); 41 | 42 | //发布机器人位姿 43 | publish_pose_ = node_.advertise("robot_pose", 1, true); 44 | this->robot_ros_.header.stamp = ros::Time::now(); 45 | this->robot_ros_.header.frame_id = "map"; 46 | } 47 | 48 | //PFLocalization类的析构函数 49 | PFLocalization::~PFLocalization() 50 | { 51 | 52 | } 53 | 54 | 55 | //粒子集初始化函数 56 | void PFLocalization::InitParticles() 57 | { 58 | int count = 1; 59 | while (count <= numParticles_) 60 | { 61 | particle_state particle_temp; 62 | 63 | particle_temp.x = rand() / (float)RAND_MAX * (map_->max_x - map_->min_x) + map_->min_x; //初始化粒子X坐标 64 | particle_temp.y = rand() / (float)RAND_MAX * (map_->max_y - map_->min_y) + map_->min_y; //初始化粒子Y坐标 65 | 66 | // particle_temp.x = rand() / (float)RAND_MAX * (500 - 340) + 340; //初始化粒子X坐标 67 | // particle_temp.y = rand() / (float)RAND_MAX * (430 - map_->min_y) + map_->min_y; //初始化粒子Y坐标 68 | 69 | if (map_->prob[(int) particle_temp.x][(int) particle_temp.y] <= map_threshold_) //若随机出的粒子不在地图有效范围内,则重新生成粒子 70 | continue; 71 | 72 | count++; //当有效粒子数 = 粒子总数时，跳出循环 73 | 74 | particle_temp.theta = rand() / (float)RAND_MAX * 2 * pi; //初始化粒子角度theta 75 | // particle_temp.theta = rand() / (float)RAND_MAX * (1/4 * pi - (-5/4 * pi)) - 5/4 * pi ; //初始化粒子角度theta 76 | 77 | //将粒子角度转换到 -pi ～ pi 之间 78 | if(particle_temp.theta > pi) 79 | particle_temp.theta -= 2 * pi; 80 | if(particle_temp.theta < -pi) 81 | particle_temp.theta += 2 * pi; 82 | 83 | particle_temp.weight = 1.0/numParticles_; // 初始化粒子权重为1/NUM 84 | 85 | particles_.push_back(particle_temp); //存入粒子集 86 | } 87 | 88 | geometry_msgs::Pose pose_ros; 89 | geometry_msgs::Quaternion q; 90 | for(int i = 0; i < numParticles_; i++) 91 | { 92 | q = tf::createQuaternionMsgFromRollPitchYaw(0,0,particles_[i].theta); //单位：rad 93 | pose_ros.position.x = 0.1 * particles_[i].x; //单位：dm -> m 94 | pose_ros.position.y = 0.1 * particles_[i].y; 95 | pose_ros.position.z = 0.0; 96 | pose_ros.orientation = q; 97 | 98 | particles_ros_.poses[i] = pose_ros; 99 | } 100 | publish_particlecloud_.publish(particles_ros_); //发布粒子集状态 101 | } 102 | 103 | 104 | //蒙特卡洛定位算法函数 105 | void PFLocalization::MCLAlgorithm() 106 | { 107 | particle_state particle_state_temp; 108 | vector particles_temp; 109 | particles_temp.resize(numParticles_); 110 | 111 | for (int i = 1;i < log_data_.size(); i++) //此层循环遍历数据集 112 | { 113 | //取数据集中相邻两帧数据，作为里程计运动模型的相邻两次状态 114 | motion_.x_front = log_data_[i-1].x_robot; 115 | motion_.y_front = log_data_[i-1].y_robot; 116 | motion_.theta_front = log_data_[i-1].theta_robot; 117 | motion_.x_rear = log_data_[i].x_robot; 118 | motion_.y_rear =log_data_[i].y_robot; 119 | motion_.theta_rear = log_data_[i].theta_robot; 120 | 121 | for(int j = 0; j < numParticles_; j++) 122 | { 123 | particle_state_temp = SampleMotionModelOdometry(particles_[j]); //从里程计运动模型中采样 124 | // particles_temp.push_back(particle_state_temp); 125 | particles_temp[j] = particle_state_temp; 126 | } 127 | 128 | // cout << "Reading (" << i << "/" << log_data_.size() << ") log data! "; 129 | // cout << "Robot Position (X,Y,theta): (" << log_data_[i].x_robot << "," << log_data_[i].y_robot << "," << log_data_[i].theta_robot << ")" << std::endl; 130 | 131 | particles_ = particles_temp; 132 | 133 | if(log_data_[i].data_type == ODOM_DATA) //此帧数据只包括odometry 134 | { 135 | Visualize(); //显示实时更新的粒子集状态 136 | 137 | continue; 138 | } 139 | else if(log_data_[i].data_type == LIDAR_DATA) //此帧数据包括lidar 140 | { 141 | measurement_.readings = log_data_[i].readings; //当前帧雷达数据首地址 142 | 143 | double total_weight = 0; 144 | for(int j = 0; j < numParticles_; j++) 145 | { 146 | // float weight = MeasurementBeamModel(particles_[j]); //根据光束模型计算每个粒子的权重，运行速度太慢且定位效果不好 147 | float weight = MeasurementScoreModel(particles_[j]); //根据似然法计算每个粒子的权重 148 | // cout << " each weight is : " << weight << endl; 149 | particles_[j].weight = weight; 150 | total_weight += particles_[j].weight; //粒子权重总和 151 | } 152 | 153 | for(int j = 0; j < numParticles_; j++) 154 | { 155 | particles_[j].weight /= total_weight; //所有粒子权重归一化 156 | // avg_weight += particles_[j].weight/numParticles_; 157 | // cout << " Normalized weight is : " << particles_[j].weight << endl; 158 | } 159 | 160 | float avg_weight = total_weight / numParticles_; 161 | // cout << " The average weight is : " << avg_weight << endl; 162 | 163 | LowVarianceSampler(); //低方差重采样算法 164 | 165 | CalRobotPose(); //计算机器人位姿，并发布topic 166 | } 167 | else 168 | { 169 | cout << " ERROR：The Log Data Is Error!!!" << endl; 170 | } 171 | 172 | Visualize(); //显示实时更新的粒子集状态 173 | 174 | // sleep(1); //1s 175 | 176 | if(i <= 20) 177 | usleep(300000); //前20帧数据延迟大一些，更清晰的观察粒子的重采样情况 178 | else 179 | usleep(40000); 180 | } 181 | 182 | } 183 | 184 | 185 | //概率机器人P103---基于里程计运动模型中采样算法 186 | particle_state PFLocalization::SampleMotionModelOdometry(particle_state particle) 187 | { 188 | float deltarot1 = atan2(motion_.y_rear - motion_.y_front,motion_.x_rear - motion_.x_front) - motion_.theta_rear; 189 | float deltatrans1 = sqrt(pow((motion_.x_rear - motion_.x_front),2) + pow((motion_.y_rear - motion_.y_front),2)); 190 | float deltarot2 = motion_.theta_rear - motion_.theta_front - deltarot1; 191 | 192 | float deltarot1_hat = deltarot1 - SampleStandardNormalDistribution(alpha1_*deltarot1 + alpha2_*deltatrans1); 193 | float deltatrans1_hat = deltatrans1 - SampleStandardNormalDistribution(alpha3_*deltatrans1 + alpha4_*(deltarot1 + deltarot2)); 194 | float deltarot2_hat = deltarot2 - SampleStandardNormalDistribution(alpha1_*deltarot2 + alpha2_*deltatrans1); 195 | 196 | particle_state particle_temp; 197 | //地图是以dm为单位,初始化的粒子位置是基于地图生成的,所以也是dm单位,而里程计数据单位是cm,需在这里进行单位转换 198 | particle_temp.x = particle.x + (deltatrans1_hat * cos(particle.theta + deltarot1_hat)) / resolution_; 199 | particle_temp.y = particle.y + (deltatrans1_hat * sin(particle.theta + deltarot1_hat)) / resolution_; 200 | particle_temp.theta = particle.theta + deltarot1_hat + deltarot2_hat; 201 | particle_temp.weight = particle.weight; 202 | 203 | return particle_temp; 204 | } 205 | 206 | 207 | //从标准正态分布中采样 208 | float PFLocalization::SampleStandardNormalDistribution(float var) 209 | { 210 | float sum = 0; 211 | for (int i = 0;i < 12; i++) 212 | //LO + static_cast (rand()) /( static_cast (RAND_MAX/(HI-LO))) 213 | sum += (rand() - RAND_MAX / 2) / (float)RAND_MAX * 2; 214 | return (var / 6.0) * sum; 215 | } 216 | 217 | 218 | 219 | //根据光束模型计算每个粒子的权重 220 | float PFLocalization::MeasurementBeamModel(particle_state particle) 221 | { 222 | double q = 1,p = 1; 223 | robot_state lidar_pose; 224 | 225 | float step_x,step_y,zkt_star = 0, zkt = 0; 226 | 227 | //计算激光雷达在地图坐标系下的位姿 228 | lidar_pose.x = particle.x + (lidar_offset_ * cos(particle.theta)) / resolution_; //(单位：dm) 229 | lidar_pose.y = particle.y + (lidar_offset_ * sin(particle.theta)) / resolution_; //(单位：dm) 230 | lidar_pose.theta = particle.theta; 231 | 232 | //若雷达位姿在地图有效区域外,则终止此次计算,该粒子权重为0 233 | if(map_->prob[(int)lidar_pose.x][(int)lidar_pose.y] <= map_threshold_) 234 | return 0.0; 235 | 236 | for (int i = 0; i < LASER_BEAM_NUM; i++) 237 | { 238 | zkt = measurement_.readings[i]; //单位：dm,从log文件读取时就已经转换成dm了 239 | 240 | //此光束无效 241 | if (zkt > (lidar_range_max_ / resolution_)) 242 | continue; 243 | 244 | //计算第i个激光束在世界坐标系下的角度 245 | float step_theta = ((double)i / 180.0) * pi + lidar_pose.theta; 246 | 247 | /******************************* Ray Tracing **********************************/ 248 | int step = 1; 249 | int invaild_flag = 0; 250 | while(1) 251 | { 252 | //减90度是因为激光雷达第一个激光点角度与机器人坐标系相差90度 253 | step_x = lidar_pose.x + step * cos( (step_theta - pi/2.0)); 254 | step_y = lidar_pose.y + step * sin( (step_theta - pi/2.0)); 255 | 256 | //若激光束打到地图以外区域,则此激光束权值为0 257 | if(step_x >= map_->max_x || step_y >= map_->max_y || step_x < map_->min_x || step_y < map_->min_y || map_->prob[(int)step_x][(int)step_y] < 0) 258 | { 259 | invaild_flag = 1; 260 | break; 261 | } 262 | else if(map_->prob[(int)step_x][(int)step_y] <= obstacle_threshold_) //遇到障碍 263 | { 264 | zkt_star = step; //Ray Tracing的结果,单位：dm 265 | break; 266 | } 267 | 268 | step += ray_tracing_step_; 269 | } 270 | if(invaild_flag == 1) 271 | continue; 272 | 273 | p = z_hit_ * ProbMeasurementHit(zkt_star,zkt) + z_short_ * ProbMeasurementShort(zkt_star,zkt) 274 | + z_max_ * ProbMeasurementMaxVal(zkt) + z_rand_ * ProbMeasurementRandVal(zkt); 275 | 276 | // q *= p; //累积乘法会导致q趋于无穷小 277 | q += p; 278 | 279 | /******************************************************************************/ 280 | } 281 | return q; 282 | } 283 | 284 | 285 | //计算高斯分布函数的概率 286 | float PFLocalization::ProbMeasurementHit(float zkt_star, float zkt) 287 | { 288 | if (zkt < 0 || zkt > (lidar_range_max_ / resolution_)) 289 | return 0; 290 | else 291 | { 292 | float q; 293 | q = (1.0 / sqrt(2*pi*sigmahit_*sigmahit_)) * exp((-1/2*((zkt - zkt_star)*(zkt - zkt_star)))/(sigmahit_*sigmahit_)); 294 | return q; 295 | } 296 | } 297 | 298 | 299 | //计算指数分布函数的概率 300 | float PFLocalization::ProbMeasurementShort(float zkt_star, float zkt) 301 | { 302 | if(zkt < 0 || zkt < zkt_star) 303 | return 0; 304 | else 305 | { 306 | float q,eeta; 307 | eeta = 1 / (1 - exp(-1.0 * lambdashort_ * zkt_star)); 308 | q = eeta * lambdashort_ * exp(-1.0 * lambdashort_ * zkt); 309 | return q; 310 | } 311 | } 312 | 313 | 314 | //计算均匀分布函数的概率 315 | float PFLocalization::ProbMeasurementRandVal(float zkt) 316 | { 317 | if(zkt < 0 || zkt >= (lidar_range_max_ / resolution_)) 318 | return 0; 319 | else 320 | return 1.0 / (lidar_range_max_ / resolution_); 321 | } 322 | 323 | 324 | //计算是否为测量最大值的概率 325 | float PFLocalization::ProbMeasurementMaxVal(float zkt) 326 | { 327 | if(zkt == (lidar_range_max_ / resolution_)) 328 | return 1; 329 | else 330 | return 0; 331 | } 332 | 333 | 334 | //根据得分模型计算每个粒子的权重 335 | float PFLocalization::MeasurementScoreModel(particle_state particle) 336 | { 337 | robot_state lidar_pose; 338 | float laser_end_x,laser_end_y,score = 0, zkt = 0; 339 | 340 | //计算激光雷达在map坐标系下的位姿 341 | lidar_pose.x = particle.x + (lidar_offset_ * cos(particle.theta)) / resolution_; //(单位：dm) 342 | lidar_pose.y = particle.y + (lidar_offset_ * sin(particle.theta)) / resolution_; //(单位：dm) 343 | lidar_pose.theta = particle.theta; 344 | 345 | //若雷达位姿在地图有效区域外,则终止此次计算,该粒子权重为0 346 | if(map_->prob[(int)lidar_pose.x][(int)lidar_pose.y] <= map_threshold_) 347 | return 0.0; 348 | 349 | for (int i = 0; i < LASER_BEAM_NUM; i++) 350 | { 351 | zkt = measurement_.readings[i]; //第i个激光束的测距，单位：dm,从log文件读取时就已经转换成dm了 352 | 353 | //若超出设置的lidar最大有效值，则此光束无效 354 | if (zkt > (lidar_range_max_ / resolution_)) 355 | continue; 356 | 357 | //计算第i个激光束在世界坐标系下的角度 358 | float step_theta = ((double)i / 180.0) * pi + lidar_pose.theta - pi/2.0; 359 | 360 | laser_end_x = lidar_pose.x + zkt * cos(step_theta); //计算此激光束末端在map坐标系下的X坐标 361 | laser_end_y = lidar_pose.y + zkt * sin(step_theta); //计算此激光束末端在map坐标系下的Y坐标 362 | 363 | //若激光束末端在地图未知区域或无效区域，则跳过此次得分计算 364 | if(laser_end_x >= map_->max_x || laser_end_y >= map_->max_y || laser_end_x < map_->min_x || laser_end_y < map_->min_y 365 | || map_->prob[(int)laser_end_x][(int)laser_end_y] < 0) 366 | continue; 367 | 368 | // if(map_->prob[(int)laser_end_x][(int)laser_end_y] >= 0 && map_->prob[(int)laser_end_x][(int)laser_end_y] < 0.15) 369 | // score++; 370 | 371 | score += map_->prob[(int)laser_end_x][(int)laser_end_y] < 0.15 ? 1 : 0; //累加，计算此帧lidar数据的得分 372 | } 373 | 374 | return score; //返回当前帧lidar数据的得分，用此来表示粒子权重 375 | } 376 | 377 | 378 | 379 | //概率机器人P78---低方差重采样算法 380 | void PFLocalization::LowVarianceSampler() 381 | { 382 | vector particles_temp = particles_; 383 | float r = (rand() / (float)RAND_MAX) * (1.0 / (float)numParticles_); //初始化一个0~1之间的随机数 384 | float c = particles_[0].weight; 385 | int i = 0; 386 | 387 | for (int m = 0;m < numParticles_; m++) 388 | { 389 | float u = r + (float) m/ numParticles_; //移动随机数 390 | while (u > c && i < numParticles_ - 1) 391 | { 392 | i++; //移动到下一个粒子 393 | c += particles_temp[i].weight; 394 | } 395 | particles_[m] = particles_temp[i]; //复制被选择的粒子 396 | particles_[m].weight = 1.0 / numParticles_; //重采样后粒子权重重置 397 | // cout << " each weight is : " << particles_[m].weight << endl; 398 | } 399 | } 400 | 401 | 402 | //计算机器人位姿，并发布状态 403 | void PFLocalization::CalRobotPose() 404 | { 405 | float total_x = 0.0; 406 | float total_y = 0.0; 407 | float total_theta = 0.0; 408 | 409 | for(int i = 0; i < numParticles_; i++) 410 | { 411 | total_x += particles_[i].x; 412 | total_y += particles_[i].y; 413 | total_theta += particles_[i].theta; 414 | } 415 | robot_pose_.x = total_x / numParticles_; 416 | robot_pose_.y = total_y / numParticles_; 417 | robot_pose_.theta = total_theta / numParticles_; 418 | 419 | cout << "Robot pose: X: " << robot_pose_.x << ", Y: " << robot_pose_.y << ", Theta: " << robot_pose_.theta << endl; 420 | 421 | //显示机器人位姿 422 | geometry_msgs::Pose pose_ros; 423 | geometry_msgs::Quaternion q; 424 | q = tf::createQuaternionMsgFromRollPitchYaw(0,0,robot_pose_.theta); 425 | pose_ros.position.x = 0.1 * robot_pose_.x; //单位：dm -> m 426 | pose_ros.position.y = 0.1 * robot_pose_.y; 427 | pose_ros.position.z = 0.0; 428 | pose_ros.orientation = q; 429 | robot_ros_.pose = pose_ros; 430 | 431 | publish_pose_.publish(robot_ros_); //发布机器人状态 432 | 433 | 434 | static tf::TransformBroadcaster br; 435 | tf::Transform transform; 436 | transform.setOrigin( tf::Vector3(0.1 * robot_pose_.x, 0.1 * robot_pose_.y, 0.0) ); 437 | tf::Quaternion tf_q; 438 | tf_q.setRPY(0, 0, robot_pose_.theta); 439 | transform.setRotation(tf_q); 440 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "base_link")); //发布base_link和map的tf关系 441 | 442 | } 443 | 444 | 445 | //显示实时更新的粒子集状态 446 | void PFLocalization::Visualize() 447 | { 448 | geometry_msgs::Pose pose_ros; 449 | geometry_msgs::Quaternion q; 450 | for(int i = 0; i < numParticles_; i++) 451 | { 452 | q = tf::createQuaternionMsgFromRollPitchYaw(0,0,particles_[i].theta); 453 | pose_ros.position.x = 0.1 * particles_[i].x; //单位：dm -> m 454 | pose_ros.position.y = 0.1 * particles_[i].y; 455 | pose_ros.position.z = 0.0; 456 | pose_ros.orientation = q; 457 | 458 | particles_ros_.poses[i] = pose_ros; 459 | } 460 | publish_particlecloud_.publish(particles_ros_); //发布粒子集状态 461 | } -------------------------------------------------------------------------------- /src/load_log.cpp: -------------------------------------------------------------------------------- 1 | #include "load_log.h" 2 | 3 | 4 | 5 | LoadLog::LoadLog(const char* log_str) 6 | { 7 | ReadFromData(log_str,this->log); 8 | } 9 | 10 | LoadLog::~LoadLog() 11 | { 12 | 13 | } 14 | 15 | int LoadLog::ReadFromData(const char* logfile, vector& logfile_data) 16 | { 17 | ifstream file (logfile); 18 | string logline; 19 | log_data logdata_indv; 20 | 21 | if(file.is_open()) 22 | { 23 | while(getline(file,logline)) 24 | { 25 | istringstream iss(logline); 26 | char c; 27 | iss >> c; 28 | int j = 0; 29 | switch(c) 30 | { 31 | case 'L': 32 | logdata_indv.data_type = 1; //此行数据包括odom和lidar 33 | iss >> logdata_indv.x_robot >> logdata_indv.y_robot >> logdata_indv.theta_robot; //读取机器人位姿,单位：cm;cm;rad 34 | iss >> logdata_indv.x_lidar >> logdata_indv.y_lidar >> logdata_indv.theta_lidar; //读取激光雷达位姿,单位：cm;cm;rad 35 | logdata_indv.readings = (float*) malloc(sizeof(float) * total_readings); 36 | while (j < total_readings) 37 | { 38 | iss >> logdata_indv.readings[j]; 39 | logdata_indv.readings[j] /= 10.0; //lidar读数换算成dm,因为地图的分辨率是1dm,统一单位方便后面计算 40 | j++; 41 | // cout << logdata_indv.readings[j] << endl; 42 | } 43 | iss >> logdata_indv.time_stamp; //读取时间戳 44 | break; 45 | case 'O': //此行数据只包括odom 46 | logdata_indv.data_type = 0; 47 | iss >> logdata_indv.x_robot >> logdata_indv.y_robot >> logdata_indv.theta_robot; 48 | iss >> logdata_indv.time_stamp; 49 | break; 50 | default: 51 | break; 52 | } 53 | logfile_data.push_back(logdata_indv); //存入vector中 54 | } 55 | } 56 | else 57 | { 58 | fprintf(stderr, "ERROR: Could not open file %s\n", logfile); 59 | return -1; 60 | } 61 | file.close(); 62 | } 63 | 64 | 65 | vector LoadLog::GetLog() 66 | { 67 | return this->log; 68 | } 69 | 70 | 71 | void LoadLog::ShowLogData() 72 | { 73 | log_data indv; 74 | for (int i = 0;i < this->log.size();i++) 75 | { 76 | indv = this->log[i]; 77 | cout << indv.data_type << endl; 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /src/load_map.cpp: -------------------------------------------------------------------------------- 1 | #include "load_map.h" 2 | 3 | 4 | 5 | LoadMap::LoadMap(const char* map_str) 6 | { 7 | this->map = (map_type*) malloc(sizeof(map_type)); //为地图存储分配内存 8 | ReadFromData(map_str); 9 | } 10 | 11 | LoadMap::~LoadMap() 12 | { 13 | 14 | } 15 | 16 | int LoadMap::ReadFromData(const char* map_name) 17 | { 18 | int x, y, count; 19 | float temp; 20 | char line[256]; 21 | FILE *fp; 22 | 23 | if((fp = fopen(map_name, "rt")) == NULL) //打开文件 24 | { 25 | fprintf(stderr, "ERROR: Could not open file %s\n", map_name); 26 | return -1; 27 | } 28 | fprintf(stderr, "# Reading map: %s\n", map_name); 29 | 30 | //fgets从指定的流 stream 读取一行存入line[],当读取255个字符时,或读取到换行符时,或到达文件末尾时,它会停止 31 | while((fgets(line, 256, fp) != NULL) 32 | && (strncmp("global_map[0]", line , 13) != 0)) 33 | { 34 | if(strncmp(line, "robot_specifications->resolution", 32) == 0) 35 | //从字符串line[]中读取%d格式输入,并存入map->resolution中 36 | if(sscanf(&line[32], "%d", &(map->resolution)) != 0) 37 | printf("# Map resolution: %d cm\n", map->resolution); 38 | if(strncmp(line, "robot_specifications->autoshifted_x", 35) == 0) 39 | if(sscanf(&line[35], "%g", &(map->offset_x)) != 0) 40 | { 41 | map->offset_x = map->offset_x; 42 | printf("# Map offsetX: %g cm\n", map->offset_x); 43 | } 44 | if(strncmp(line, "robot_specifications->autoshifted_y", 35) == 0) 45 | { 46 | if (sscanf(&line[35], "%g", &(map->offset_y)) != 0) 47 | { 48 | map->offset_y = map->offset_y; 49 | printf("# Map offsetY: %g cm\n", map->offset_y); 50 | } 51 | } 52 | } 53 | 54 | if(sscanf(line,"global_map[0]: %d %d", &map->size_y, &map->size_x) != 2) //如果成功则返回成功匹配和赋值的个数 55 | { 56 | fprintf(stderr, "ERROR: corrupted file %s\n", map_name); 57 | fclose(fp); 58 | return -1; 59 | } 60 | printf("# Map size: %d %d\n", map->size_x, map->size_y); 61 | 62 | 63 | map->prob = (float **)calloc(map->size_x, sizeof(float *)); //为二维数组分配内存，calloc同时将数组初始化为0 64 | for(int i = 0; i < map->size_x; i++) 65 | { 66 | map->prob[i] = (float *)calloc(map->size_y, sizeof(float)); 67 | } 68 | 69 | map->min_x = map->size_x; 70 | map->max_x = 0; 71 | map->min_y = map->size_y; 72 | map->max_y = 0; 73 | count = 0; 74 | for(x = 0; x < map->size_x; x++) 75 | { 76 | for(y = 0; y < map->size_y; y++, count++) 77 | { 78 | if(count % 10000 == 0) 79 | fprintf(stderr, "\r# Reading ... (%.2f%%)", count / (float)(map->size_x * map->size_y) * 100); 80 | fscanf(fp,"%e", &temp); //每次读取地图一个像素点的概率值 81 | 82 | if(temp < 0.0) 83 | map->prob[x][y] = -1; //地图状态未知 84 | else 85 | { 86 | if(x < map->min_x) //min~max指的是地图有效区域,默认min,max赋的都是最大值,其值在这里进行更新 87 | map->min_x = x; 88 | else if(x > map->max_x) 89 | map->max_x = x; 90 | if(y < map->min_y) 91 | map->min_y = y; 92 | else if(y > map->max_y) 93 | map->max_y = y; 94 | map->prob[x][y] = temp; //地图概率赋值 95 | } 96 | } 97 | } 98 | //显示地图加载进度 99 | fprintf(stderr, "\r# Reading ... (%.2f%%)\n\n",count / (float)(map->size_x * map->size_y) * 100); 100 | fclose(fp); 101 | return 0; 102 | } 103 | 104 | 105 | void LoadMap::PublishMap(ros::NodeHandle node_) 106 | { 107 | nav_msgs::OccupancyGrid ros_map; 108 | ros_map.header.stamp = ros::Time::now(); 109 | ros_map.header.frame_id = "map"; 110 | ros_map.info.map_load_time = ros::Time::now(); 111 | ros_map.info.resolution = this->map->resolution * 0.01; //rviz中单位为m，1Ocm -> 0.1m 112 | ros_map.info.width = this->map->size_x; 113 | ros_map.info.height = this->map->size_y; 114 | ros_map.info.origin.position.x = 0.0; 115 | ros_map.info.origin.position.y = 0.0; 116 | ros_map.info.origin.position.z = 0.0; 117 | ros_map.info.origin.orientation.x = 0.0; 118 | ros_map.info.origin.orientation.y = 0.0; 119 | ros_map.info.origin.orientation.z = 0.0; 120 | ros_map.info.origin.orientation.w = 1.0; 121 | 122 | ros_map.data.resize(ros_map.info.width * ros_map.info.height); 123 | 124 | for(int x=0; x < ros_map.info.width; x++) 125 | { 126 | for(int y=0; y < ros_map.info.height; y++) 127 | { 128 | // ros_map.data[x*ros_map.info.width + y] = (unsigned int)(map->prob[y][x] * 255); 129 | ros_map.data[y*ros_map.info.height + x] = (uint8_t)(this->map->prob[x][y] * 255); 130 | } 131 | } 132 | 133 | map_pub = node_.advertise("map", 1 ,true); 134 | map_meta_pub = node_.advertise("map_metadata", 1 ,true); 135 | 136 | map_meta_pub.publish(ros_map.info); 137 | map_pub.publish(ros_map); 138 | } 139 | 140 | 141 | map_type* LoadMap::GetMap() 142 | { 143 | return this->map; 144 | } 145 | 146 | 147 | -------------------------------------------------------------------------------- /src/localization_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "localization_node.h" 3 | #include "nav_msgs/MapMetaData.h" 4 | #include "nav_msgs/OccupancyGrid.h" 5 | #include "geometry_msgs/PoseArray.h" 6 | #include "tf/tf.h" 7 | #include "stdlib.h" 8 | 9 | using namespace std; 10 | 11 | 12 | int main(int argc, char** argv) 13 | { 14 | 15 | ros::init(argc, argv,"localization_node"); //初始化节点 16 | // ros::NodeHandle n; 17 | ros::NodeHandle n; 18 | 19 | string package_path; 20 | string map_local_path = "pf_localization/data/map/wean.dat"; 21 | string log_local_path = "pf_localization/data/log/robotdata1.log"; 22 | 23 | n.getParam("/pf_localization/package_path_param",package_path); 24 | string complete_map_path = package_path + map_local_path; 25 | string complete_log_path = package_path + log_local_path; 26 | const char* complete_map_path_c = complete_map_path.c_str(); 27 | const char* complete_log_path_c = complete_log_path.c_str(); 28 | 29 | LoadMap load_map(complete_map_path_c); //加载map 30 | LoadLog load_log(complete_log_path_c); //加载odometry和lidar数据 31 | 32 | // LoadMap load_map("/home/wsy/catkin_ws/src/pf_localization/data/map/wean.dat"); //加载map 33 | // LoadLog load_log("/home/wsy/catkin_ws/src/pf_localization/data/log/robotdata1.log"); //加载odometry和lidar数据 34 | 35 | map_type* map = load_map.GetMap(); //获取地图 36 | vector logfile_data = load_log.GetLog(); //获取数据集 37 | 38 | load_map.PublishMap(n); //发布map，rviz订阅此topic 39 | 40 | PFLocalization pf_localization(map, logfile_data, n); 41 | pf_localization.InitParticles(); //粒子集初始化 42 | sleep(5); //粒子初始化后延时5秒后开始跑数据集 43 | pf_localization.MCLAlgorithm(); //运行蒙特卡洛定位算法 44 | 45 | return 0; 46 | } 47 | 48 | -------------------------------------------------------------------------------- /urdf/three_wheels_robot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 570 | 571 | 572 | 573 | 574 | 575 | 576 | 577 | 578 | 579 | 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 589 | 590 | 591 | 592 | 593 | 594 | 595 | 596 | 597 | 598 | 599 | 600 | 601 | 602 | 603 | 604 | 605 | 606 | 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | 621 | 622 | 623 | 624 | 625 | 626 | 627 | 628 | --------------------------------------------------------------------------------