├── src ├── CMakeLists.txt └── mcl_ros │ ├── launch │ ├── map_server.launch │ ├── robot_tf.launch │ └── mcl.launch │ ├── CMakeLists.txt │ ├── src │ └── mcl.cpp │ ├── include │ └── mcl_ros │ │ ├── Particle.h │ │ ├── Pose.h │ │ └── MCL.h │ └── package.xml ├── .catkin_workspace ├── .gitignore ├── README.md ├── rviz └── mcl.rviz └── LICENSE /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /src/mcl_ros/launch/map_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Build space 2 | build 3 | build/* 4 | devel 5 | devel/* 6 | 7 | # Files 8 | src/mcl_ros/src/gl* 9 | src/mcl_ros/src/recorder* 10 | src/mcl_ros/src/scan* 11 | src/mcl_ros/include/mcl_ros/GL* 12 | src/mcl_ros/launch/gl* 13 | src/mcl_ros/*tmp 14 | 15 | # Prerequisites 16 | *.d 17 | 18 | # Compiled Object files 19 | *.slo 20 | *.lo 21 | *.o 22 | *.obj 23 | 24 | # Precompiled Headers 25 | *.gch 26 | *.pch 27 | 28 | # Compiled Dynamic libraries 29 | *.so 30 | *.dylib 31 | *.dll 32 | 33 | # Fortran module files 34 | *.mod 35 | *.smod 36 | 37 | # Compiled Static libraries 38 | *.lai 39 | *.la 40 | *.a 41 | *.lib 42 | 43 | # Executables 44 | *.exe 45 | *.out 46 | *.app 47 | -------------------------------------------------------------------------------- /src/mcl_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mcl_ros) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | cv_bridge 6 | geometry_msgs 7 | nav_msgs 8 | roscpp 9 | rospy 10 | sensor_msgs 11 | std_msgs 12 | tf 13 | ) 14 | 15 | catkin_package( 16 | # INCLUDE_DIRS include 17 | # LIBRARIES mcl_ros 18 | # CATKIN_DEPENDS cv_bridge geometry_msgs nav_msgs roscpp rospy sensor_msgs std_msgs tf 19 | # DEPENDS system_lib 20 | ) 21 | 22 | include_directories( 23 | ${catkin_INCLUDE_DIRS} 24 | ${OpenCV_INCLUDE_DIRS} 25 | "${PROJECT_SOURCE_DIR}/include" 26 | ) 27 | 28 | add_executable(mcl src/mcl.cpp) 29 | target_link_libraries(mcl ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 30 | 31 | -------------------------------------------------------------------------------- /src/mcl_ros/launch/robot_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 15 | 16 | -------------------------------------------------------------------------------- /src/mcl_ros/src/mcl.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl_ros: Monte Carlo localization with ROS 3 | * Copyright (C) 2021-2022 Naoki Akai 4 | * 5 | * This Source Code Form is subject to the terms of the Mozilla Public 6 | * License, v. 2.0. If a copy of the MPL was not distributed with this file, 7 | * You can obtain one at https://mozilla.org/MPL/2.0/. 8 | * 9 | * @author Naoki Akai 10 | ****************************************************************************/ 11 | 12 | #include 13 | #include 14 | 15 | int main(int argc, char **argv) { 16 | ros::init(argc, argv, "mcl_ros/mcl"); 17 | 18 | mcl_ros::MCL mcl; 19 | double localizationHz = mcl.getLocalizationHz(); 20 | ros::Rate loopRate(localizationHz); 21 | 22 | while (ros::ok()) { 23 | ros::spinOnce(); 24 | mcl.updateParticlesByMotionModel(); 25 | mcl.setCanUpdateScan(false); 26 | mcl.calculateLikelihoodsByMeasurementModel(); 27 | mcl.calculateEffectiveSampleSize(); 28 | mcl.estimatePose(); 29 | mcl.resampleParticles(); 30 | mcl.estimateLocalizationCorrectness(); 31 | mcl.publishROSMessages(); 32 | mcl.broadcastTF(); 33 | mcl.setCanUpdateScan(true); 34 | mcl.printResult(); 35 | loopRate.sleep(); 36 | } 37 | 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /src/mcl_ros/include/mcl_ros/Particle.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl_ros: Monte Carlo localization with ROS 3 | * Copyright (C) 2021-2022 Naoki Akai 4 | * 5 | * This Source Code Form is subject to the terms of the Mozilla Public 6 | * License, v. 2.0. If a copy of the MPL was not distributed with this file, 7 | * You can obtain one at https://mozilla.org/MPL/2.0/. 8 | * 9 | * @author Naoki Akai 10 | ****************************************************************************/ 11 | 12 | #ifndef __PARTICLE_H__ 13 | #define __PARTICLE_H__ 14 | 15 | #include 16 | 17 | namespace mcl_ros { 18 | 19 | class Particle { 20 | private: 21 | Pose pose_; 22 | double w_; 23 | 24 | public: 25 | Particle(): pose_(0.0, 0.0, 0.0), w_(0.0) {}; 26 | 27 | Particle(double x, double y, double yaw, double w): pose_(x, y, yaw), w_(w) {}; 28 | 29 | Particle(Pose p, double w): pose_(p), w_(w) {}; 30 | 31 | ~Particle() {}; 32 | 33 | inline double getX(void) { return pose_.getX(); } 34 | inline double getY(void) { return pose_.getY(); } 35 | inline double getYaw(void) { return pose_.getYaw(); } 36 | inline Pose getPose(void) { return pose_; } 37 | inline double getW(void) { return w_; } 38 | 39 | inline void setX(double x) { pose_.setX(x); } 40 | inline void setY(double y) { pose_.setY(y); } 41 | inline void setYaw(double yaw) { pose_.setYaw(yaw); } 42 | inline void setPose(double x, double y, double yaw) { pose_.setPose(x, y, yaw); } 43 | inline void setPose(Pose p) { pose_.setPose(p); } 44 | inline void setW(double w) { w_ = w; } 45 | }; // class Particle 46 | 47 | } // namespace mcl_ros 48 | 49 | #endif // __PARTICLE_H__ 50 | -------------------------------------------------------------------------------- /src/mcl_ros/include/mcl_ros/Pose.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl_ros: Monte Carlo localization with ROS 3 | * Copyright (C) 2021-2022 Naoki Akai 4 | * 5 | * This Source Code Form is subject to the terms of the Mozilla Public 6 | * License, v. 2.0. If a copy of the MPL was not distributed with this file, 7 | * You can obtain one at https://mozilla.org/MPL/2.0/. 8 | * 9 | * @author Naoki Akai 10 | ****************************************************************************/ 11 | 12 | #ifndef __POSE_H__ 13 | #define __POSE_H__ 14 | 15 | #include 16 | 17 | namespace mcl_ros { 18 | 19 | class Pose { 20 | private: 21 | double x_, y_, yaw_; 22 | 23 | void modifyYaw(void) { 24 | while (yaw_ < -M_PI) 25 | yaw_ += 2.0 * M_PI; 26 | while (yaw_ > M_PI) 27 | yaw_ -= 2.0 * M_PI; 28 | } 29 | 30 | public: 31 | Pose(): 32 | x_(0.0), y_(0.0), yaw_(0.0) {}; 33 | 34 | Pose(double x, double y, double yaw): 35 | x_(x), y_(y), yaw_(yaw) { modifyYaw(); }; 36 | 37 | ~Pose() {}; 38 | 39 | inline void setX(double x) { x_ = x; } 40 | inline void setY(double y) { y_ = y; } 41 | inline void setYaw(double yaw) { yaw_ = yaw, modifyYaw(); } 42 | inline void setPose(double x, double y, double yaw) { x_ = x, y_ = y, yaw_ = yaw, modifyYaw(); } 43 | inline void setPose(Pose p) { x_ = p.x_, y_ = p.y_, yaw_ = p.yaw_, modifyYaw(); } 44 | 45 | inline double getX(void) { return x_; } 46 | inline double getY(void) { return y_; } 47 | inline double getYaw(void) { return yaw_; } 48 | inline Pose getPose(void) { return Pose(x_, y_, yaw_); } 49 | 50 | }; // class Pose 51 | 52 | } // namespace mcl_ros 53 | 54 | #endif // __POSE_H__ 55 | -------------------------------------------------------------------------------- /src/mcl_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mcl_ros 4 | 0.0.0 5 | The mcl_ros package 6 | 7 | 8 | 9 | 10 | akai 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 | cv_bridge 53 | geometry_msgs 54 | nav_msgs 55 | roscpp 56 | rospy 57 | sensor_msgs 58 | std_msgs 59 | tf 60 | cv_bridge 61 | geometry_msgs 62 | nav_msgs 63 | roscpp 64 | rospy 65 | sensor_msgs 66 | std_msgs 67 | tf 68 | cv_bridge 69 | geometry_msgs 70 | nav_msgs 71 | roscpp 72 | rospy 73 | sensor_msgs 74 | std_msgs 75 | tf 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Caution 2 | 3 | **[als_ros](https://github.com/NaokiAkai/als_ros) that is upper compatible of mcl_ros is available now.** 4 | 5 | 6 | 7 | # mcl_ros 8 | 9 | ## About mcl_ros 10 | 11 | mcl_ros is a ROS package for mobile robot localization with 2D LiDAR. Monte Carlo localization (MCL) is used as a localization algorithm. 12 | 13 | I confirmed that mcl_ros works on Ubuntu 18.04 and 20.04. 14 | 15 | 16 | 17 | 18 | 19 | ## How to install 20 | 21 | You need to install ROS environment first. Then, install mcl_ros as 22 | 23 | ~~~ 24 | $ git clone https://github.com/NaokiAkai/mcl_ros.git 25 | $ cd mcl_ros 26 | $ catkin_make 27 | ~~~ 28 | 29 | 30 | 31 | If you do not want to make a new workspace for mcl_ros, please copy the mcl_ros package to your workspace. 32 | 33 | 34 | 35 | 36 | 37 | ## How to run 38 | 39 | You need to publish sensor_msgs::LaserScan, nav_msgs::Odometry, and nav_msgs::OccupancyGrid messages. Default topic names are "/scan", "/odom", and "/map". 40 | 41 | In addition, you need to set static transform publisher between the base link to laser sensor frames. Default frame names are "base_link" and "laser". 42 | 43 | Then, run mcl_ros as 44 | 45 | ~~~ 46 | $ cd mcl_ros 47 | $ source devel/setup.bash 48 | $ roslaunch mcl_ros mcl.launch 49 | ~~~ 50 | 51 | 52 | 53 | 54 | 55 | ## Parameter descriptions 56 | 57 | There is a launch file under mcl_ros/src/mcl_ros/launch/ directory, named mcl.launch. The parameter descriptions can be seen in the launch file. 58 | 59 | 60 | 61 | 62 | 63 | ## Characteristics 64 | 65 | ### Robust localization 66 | 67 | mcl_ros contains the localization method presented in [1]. The localization method simultaneously estimates the robot pose and sensor measurement classes. Here, two sensor measurements classes are considered; known and unknown, that is, mapped and unmapped obstacles. Because the method can simultaneously estimate the unknown measurement while localization, the localization robustness to unknown obstacles can be increased. 68 | 69 | To use the localization method, measurement_model_type is needed to be set to 2. 70 | 71 | ~~~ 72 | $ roslaunch mcl_ros mcl.launch measurement_model_type:=2 73 | ~~~ 74 | 75 | 76 | 77 | ### Adding random particles in resampling 78 | 79 | Adding random particles in resampling is a simple method to improve localization robustness. mcl_ros uses this method. 80 | 81 | To use the method, use_augmented_mcl or add_random_particles_in_resampling is needed to be set to true. 82 | 83 | ~~~ 84 | $ roslaunch mcl_ros mcl.launch use_augmented_mcl:=true 85 | ~~~ 86 | 87 | or 88 | 89 | ~~~ 90 | $ roslaunch mcl_ros mcl.launch add_random_particles_in_resampling:=true 91 | ~~~ 92 | 93 | If use_augmented_mcl is true, mcl_ros works as augmented MCL [2]. In my experience, I really recommend to set add_random_particles_in_resampling to true, instead of use_augmented_mcl. 94 | 95 | ### 96 | 97 | ### Rejecting unknown scan 98 | 99 | In [2], a method to reject scans that might measure unknown obstacles is presented. To reject the unknown scan, the beam model [2] is utilized. The unknown scan rejection method is implemented in mcl_ros. 100 | 101 | To use the unknown scan rejection, reject_unknown_scan is needed to be set to true. 102 | 103 | ~~~ 104 | $ roslaunch mcl_ros mcl.launch reject_unknown_scan:=true 105 | ~~~ 106 | 107 | Note that the unknown scans are automatically detected if the method presented in [1] is used since it simultaneously estimates the unknown class measurements. 108 | 109 | 110 | 111 | ## 112 | 113 | ## Reference 114 | 115 | [1] Naoki Akai, Luis Yoichi Morales, and Hiroshi Murase. "Mobile robot localization considering class of sensor observations," In *Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, pp. 3159-3166, 2018. 116 | 117 | [2] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. "Probabilistic robotics," *MIT Press*, 2005. 118 | 119 | 120 | 121 | 122 | 123 | ## License 124 | 125 | Mozilla Public License Version 2.0 126 | -------------------------------------------------------------------------------- /rviz/mcl.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | - /TF1/Frames1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1087 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: Scan 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 0.699999988079071 56 | Class: rviz/Map 57 | Color Scheme: map 58 | Draw Behind: false 59 | Enabled: true 60 | Name: Map 61 | Topic: /map 62 | Unreliable: false 63 | Use Timestamp: false 64 | Value: true 65 | - Class: rviz/TF 66 | Enabled: true 67 | Frame Timeout: 15 68 | Frames: 69 | All Enabled: false 70 | base_link: 71 | Value: true 72 | ground_truth: 73 | Value: true 74 | laser: 75 | Value: false 76 | map: 77 | Value: true 78 | odom: 79 | Value: true 80 | scanned_ground_truth: 81 | Value: true 82 | world: 83 | Value: true 84 | Marker Alpha: 1 85 | Marker Scale: 5 86 | Name: TF 87 | Show Arrows: false 88 | Show Axes: true 89 | Show Names: true 90 | Tree: 91 | world: 92 | map: 93 | ground_truth: 94 | {} 95 | odom: 96 | base_link: 97 | laser: 98 | {} 99 | scanned_ground_truth: 100 | {} 101 | Update Interval: 0 102 | Value: true 103 | - Alpha: 1 104 | Arrow Length: 1 105 | Axes Length: 0.30000001192092896 106 | Axes Radius: 0.009999999776482582 107 | Class: rviz/PoseArray 108 | Color: 0; 0; 255 109 | Enabled: true 110 | Head Length: 0.07000000029802322 111 | Head Radius: 0.029999999329447746 112 | Name: MCL particles 113 | Queue Size: 10 114 | Shaft Length: 0.23000000417232513 115 | Shaft Radius: 0.009999999776482582 116 | Shape: Arrow (Flat) 117 | Topic: /mcl_particles 118 | Unreliable: false 119 | Value: true 120 | - Alpha: 1 121 | Autocompute Intensity Bounds: true 122 | Autocompute Value Bounds: 123 | Max Value: 10 124 | Min Value: -10 125 | Value: true 126 | Axis: Z 127 | Channel Name: intensity 128 | Class: rviz/LaserScan 129 | Color: 239; 41; 41 130 | Color Transformer: FlatColor 131 | Decay Time: 0 132 | Enabled: true 133 | Invert Rainbow: false 134 | Max Color: 255; 255; 255 135 | Min Color: 0; 0; 0 136 | Name: Scan 137 | Position Transformer: XYZ 138 | Queue Size: 10 139 | Selectable: true 140 | Size (Pixels): 5 141 | Size (m): 0.009999999776482582 142 | Style: Points 143 | Topic: /scan 144 | Unreliable: false 145 | Use Fixed Frame: true 146 | Use rainbow: true 147 | Value: true 148 | - Alpha: 1 149 | Autocompute Intensity Bounds: true 150 | Autocompute Value Bounds: 151 | Max Value: 10 152 | Min Value: -10 153 | Value: true 154 | Axis: Z 155 | Channel Name: intensity 156 | Class: rviz/LaserScan 157 | Color: 252; 233; 79 158 | Color Transformer: FlatColor 159 | Decay Time: 0 160 | Enabled: true 161 | Invert Rainbow: false 162 | Max Color: 255; 255; 255 163 | Min Color: 0; 0; 0 164 | Name: Unknown scan 165 | Position Transformer: XYZ 166 | Queue Size: 10 167 | Selectable: true 168 | Size (Pixels): 10 169 | Size (m): 0.009999999776482582 170 | Style: Points 171 | Topic: /unknown_scan 172 | Unreliable: false 173 | Use Fixed Frame: true 174 | Use rainbow: true 175 | Value: true 176 | - Alpha: 1 177 | Autocompute Intensity Bounds: true 178 | Autocompute Value Bounds: 179 | Max Value: 10 180 | Min Value: -10 181 | Value: true 182 | Axis: Z 183 | Channel Name: intensity 184 | Class: rviz/PointCloud 185 | Color: 239; 41; 41 186 | Color Transformer: FlatColor 187 | Decay Time: 0 188 | Enabled: true 189 | Invert Rainbow: false 190 | Max Color: 255; 255; 255 191 | Min Color: 0; 0; 0 192 | Name: Scan points 193 | Position Transformer: XYZ 194 | Queue Size: 10 195 | Selectable: true 196 | Size (Pixels): 8 197 | Size (m): 0.009999999776482582 198 | Style: Points 199 | Topic: /scan_point_cloud 200 | Unreliable: false 201 | Use Fixed Frame: true 202 | Use rainbow: true 203 | Value: true 204 | Enabled: true 205 | Global Options: 206 | Background Color: 48; 48; 48 207 | Default Light: true 208 | Fixed Frame: map 209 | Frame Rate: 30 210 | Name: root 211 | Tools: 212 | - Class: rviz/Interact 213 | Hide Inactive Objects: true 214 | - Class: rviz/MoveCamera 215 | - Class: rviz/Select 216 | - Class: rviz/FocusCamera 217 | - Class: rviz/Measure 218 | - Class: rviz/SetInitialPose 219 | Theta std deviation: 0.2617993950843811 220 | Topic: /initialpose 221 | X std deviation: 0.5 222 | Y std deviation: 0.5 223 | - Class: rviz/SetGoal 224 | Topic: /move_base_simple/goal 225 | - Class: rviz/PublishPoint 226 | Single click: true 227 | Topic: /clicked_point 228 | Value: true 229 | Views: 230 | Current: 231 | Angle: 0 232 | Class: rviz/TopDownOrtho 233 | Enable Stereo Rendering: 234 | Stereo Eye Separation: 0.05999999865889549 235 | Stereo Focal Distance: 1 236 | Swap Stereo Eyes: false 237 | Value: false 238 | Invert Z Axis: false 239 | Name: Current View 240 | Near Clip Distance: 0.009999999776482582 241 | Scale: 59.56999588012695 242 | Target Frame: base_link 243 | X: 0 244 | Y: 0 245 | Saved: ~ 246 | Window Geometry: 247 | Displays: 248 | collapsed: false 249 | Height: 1376 250 | Hide Left Dock: false 251 | Hide Right Dock: false 252 | QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b8000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000747000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 253 | Selection: 254 | collapsed: false 255 | Time: 256 | collapsed: false 257 | Tool Properties: 258 | collapsed: false 259 | Views: 260 | collapsed: false 261 | Width: 2488 262 | X: 2632 263 | Y: 27 264 | -------------------------------------------------------------------------------- /src/mcl_ros/launch/mcl.launch: -------------------------------------------------------------------------------- 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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 43 | 44 | 45 | 47 | 48 | 49 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 69 | 70 | 71 | 76 | 77 | 78 | 79 | 80 | 82 | 83 | 84 | 89 | 90 | 91 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 108 | 109 | 110 | 111 | 112 | 113 | 115 | 116 | 117 | 120 | 121 | 122 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | [0.0, 0.0, 0.0] 140 | 141 | 142 | [0.3, 0.3, 0.02] 143 | 144 | 146 | [0.1, 0.1, 0.02] 147 | 148 | 149 | [1.0, 0.5, 0.5, 1.0] 150 | [1.0, 0.5, 0.5, 0.5, 1.0, 0.5, 0.5, 0.5, 1.0] 151 | 152 | 154 | [0.2, 0.2, 0.2, 0.02, -0.1] 155 | 156 | 157 | 158 | 159 | 160 | 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 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Mozilla Public License Version 2.0 2 | ================================== 3 | 4 | 1. Definitions 5 | -------------- 6 | 7 | 1.1. "Contributor" 8 | means each individual or legal entity that creates, contributes to 9 | the creation of, or owns Covered Software. 10 | 11 | 1.2. "Contributor Version" 12 | means the combination of the Contributions of others (if any) used 13 | by a Contributor and that particular Contributor's Contribution. 14 | 15 | 1.3. "Contribution" 16 | means Covered Software of a particular Contributor. 17 | 18 | 1.4. "Covered Software" 19 | means Source Code Form to which the initial Contributor has attached 20 | the notice in Exhibit A, the Executable Form of such Source Code 21 | Form, and Modifications of such Source Code Form, in each case 22 | including portions thereof. 23 | 24 | 1.5. "Incompatible With Secondary Licenses" 25 | means 26 | 27 | (a) that the initial Contributor has attached the notice described 28 | in Exhibit B to the Covered Software; or 29 | 30 | (b) that the Covered Software was made available under the terms of 31 | version 1.1 or earlier of the License, but not also under the 32 | terms of a Secondary License. 33 | 34 | 1.6. "Executable Form" 35 | means any form of the work other than Source Code Form. 36 | 37 | 1.7. "Larger Work" 38 | means a work that combines Covered Software with other material, in 39 | a separate file or files, that is not Covered Software. 40 | 41 | 1.8. "License" 42 | means this document. 43 | 44 | 1.9. "Licensable" 45 | means having the right to grant, to the maximum extent possible, 46 | whether at the time of the initial grant or subsequently, any and 47 | all of the rights conveyed by this License. 48 | 49 | 1.10. "Modifications" 50 | means any of the following: 51 | 52 | (a) any file in Source Code Form that results from an addition to, 53 | deletion from, or modification of the contents of Covered 54 | Software; or 55 | 56 | (b) any new file in Source Code Form that contains any Covered 57 | Software. 58 | 59 | 1.11. "Patent Claims" of a Contributor 60 | means any patent claim(s), including without limitation, method, 61 | process, and apparatus claims, in any patent Licensable by such 62 | Contributor that would be infringed, but for the grant of the 63 | License, by the making, using, selling, offering for sale, having 64 | made, import, or transfer of either its Contributions or its 65 | Contributor Version. 66 | 67 | 1.12. "Secondary License" 68 | means either the GNU General Public License, Version 2.0, the GNU 69 | Lesser General Public License, Version 2.1, the GNU Affero General 70 | Public License, Version 3.0, or any later versions of those 71 | licenses. 72 | 73 | 1.13. "Source Code Form" 74 | means the form of the work preferred for making modifications. 75 | 76 | 1.14. "You" (or "Your") 77 | means an individual or a legal entity exercising rights under this 78 | License. For legal entities, "You" includes any entity that 79 | controls, is controlled by, or is under common control with You. For 80 | purposes of this definition, "control" means (a) the power, direct 81 | or indirect, to cause the direction or management of such entity, 82 | whether by contract or otherwise, or (b) ownership of more than 83 | fifty percent (50%) of the outstanding shares or beneficial 84 | ownership of such entity. 85 | 86 | 2. License Grants and Conditions 87 | -------------------------------- 88 | 89 | 2.1. Grants 90 | 91 | Each Contributor hereby grants You a world-wide, royalty-free, 92 | non-exclusive license: 93 | 94 | (a) under intellectual property rights (other than patent or trademark) 95 | Licensable by such Contributor to use, reproduce, make available, 96 | modify, display, perform, distribute, and otherwise exploit its 97 | Contributions, either on an unmodified basis, with Modifications, or 98 | as part of a Larger Work; and 99 | 100 | (b) under Patent Claims of such Contributor to make, use, sell, offer 101 | for sale, have made, import, and otherwise transfer either its 102 | Contributions or its Contributor Version. 103 | 104 | 2.2. Effective Date 105 | 106 | The licenses granted in Section 2.1 with respect to any Contribution 107 | become effective for each Contribution on the date the Contributor first 108 | distributes such Contribution. 109 | 110 | 2.3. Limitations on Grant Scope 111 | 112 | The licenses granted in this Section 2 are the only rights granted under 113 | this License. No additional rights or licenses will be implied from the 114 | distribution or licensing of Covered Software under this License. 115 | Notwithstanding Section 2.1(b) above, no patent license is granted by a 116 | Contributor: 117 | 118 | (a) for any code that a Contributor has removed from Covered Software; 119 | or 120 | 121 | (b) for infringements caused by: (i) Your and any other third party's 122 | modifications of Covered Software, or (ii) the combination of its 123 | Contributions with other software (except as part of its Contributor 124 | Version); or 125 | 126 | (c) under Patent Claims infringed by Covered Software in the absence of 127 | its Contributions. 128 | 129 | This License does not grant any rights in the trademarks, service marks, 130 | or logos of any Contributor (except as may be necessary to comply with 131 | the notice requirements in Section 3.4). 132 | 133 | 2.4. Subsequent Licenses 134 | 135 | No Contributor makes additional grants as a result of Your choice to 136 | distribute the Covered Software under a subsequent version of this 137 | License (see Section 10.2) or under the terms of a Secondary License (if 138 | permitted under the terms of Section 3.3). 139 | 140 | 2.5. Representation 141 | 142 | Each Contributor represents that the Contributor believes its 143 | Contributions are its original creation(s) or it has sufficient rights 144 | to grant the rights to its Contributions conveyed by this License. 145 | 146 | 2.6. Fair Use 147 | 148 | This License is not intended to limit any rights You have under 149 | applicable copyright doctrines of fair use, fair dealing, or other 150 | equivalents. 151 | 152 | 2.7. Conditions 153 | 154 | Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted 155 | in Section 2.1. 156 | 157 | 3. Responsibilities 158 | ------------------- 159 | 160 | 3.1. Distribution of Source Form 161 | 162 | All distribution of Covered Software in Source Code Form, including any 163 | Modifications that You create or to which You contribute, must be under 164 | the terms of this License. You must inform recipients that the Source 165 | Code Form of the Covered Software is governed by the terms of this 166 | License, and how they can obtain a copy of this License. You may not 167 | attempt to alter or restrict the recipients' rights in the Source Code 168 | Form. 169 | 170 | 3.2. Distribution of Executable Form 171 | 172 | If You distribute Covered Software in Executable Form then: 173 | 174 | (a) such Covered Software must also be made available in Source Code 175 | Form, as described in Section 3.1, and You must inform recipients of 176 | the Executable Form how they can obtain a copy of such Source Code 177 | Form by reasonable means in a timely manner, at a charge no more 178 | than the cost of distribution to the recipient; and 179 | 180 | (b) You may distribute such Executable Form under the terms of this 181 | License, or sublicense it under different terms, provided that the 182 | license for the Executable Form does not attempt to limit or alter 183 | the recipients' rights in the Source Code Form under this License. 184 | 185 | 3.3. Distribution of a Larger Work 186 | 187 | You may create and distribute a Larger Work under terms of Your choice, 188 | provided that You also comply with the requirements of this License for 189 | the Covered Software. If the Larger Work is a combination of Covered 190 | Software with a work governed by one or more Secondary Licenses, and the 191 | Covered Software is not Incompatible With Secondary Licenses, this 192 | License permits You to additionally distribute such Covered Software 193 | under the terms of such Secondary License(s), so that the recipient of 194 | the Larger Work may, at their option, further distribute the Covered 195 | Software under the terms of either this License or such Secondary 196 | License(s). 197 | 198 | 3.4. Notices 199 | 200 | You may not remove or alter the substance of any license notices 201 | (including copyright notices, patent notices, disclaimers of warranty, 202 | or limitations of liability) contained within the Source Code Form of 203 | the Covered Software, except that You may alter any license notices to 204 | the extent required to remedy known factual inaccuracies. 205 | 206 | 3.5. Application of Additional Terms 207 | 208 | You may choose to offer, and to charge a fee for, warranty, support, 209 | indemnity or liability obligations to one or more recipients of Covered 210 | Software. However, You may do so only on Your own behalf, and not on 211 | behalf of any Contributor. You must make it absolutely clear that any 212 | such warranty, support, indemnity, or liability obligation is offered by 213 | You alone, and You hereby agree to indemnify every Contributor for any 214 | liability incurred by such Contributor as a result of warranty, support, 215 | indemnity or liability terms You offer. You may include additional 216 | disclaimers of warranty and limitations of liability specific to any 217 | jurisdiction. 218 | 219 | 4. Inability to Comply Due to Statute or Regulation 220 | --------------------------------------------------- 221 | 222 | If it is impossible for You to comply with any of the terms of this 223 | License with respect to some or all of the Covered Software due to 224 | statute, judicial order, or regulation then You must: (a) comply with 225 | the terms of this License to the maximum extent possible; and (b) 226 | describe the limitations and the code they affect. Such description must 227 | be placed in a text file included with all distributions of the Covered 228 | Software under this License. Except to the extent prohibited by statute 229 | or regulation, such description must be sufficiently detailed for a 230 | recipient of ordinary skill to be able to understand it. 231 | 232 | 5. Termination 233 | -------------- 234 | 235 | 5.1. The rights granted under this License will terminate automatically 236 | if You fail to comply with any of its terms. However, if You become 237 | compliant, then the rights granted under this License from a particular 238 | Contributor are reinstated (a) provisionally, unless and until such 239 | Contributor explicitly and finally terminates Your grants, and (b) on an 240 | ongoing basis, if such Contributor fails to notify You of the 241 | non-compliance by some reasonable means prior to 60 days after You have 242 | come back into compliance. Moreover, Your grants from a particular 243 | Contributor are reinstated on an ongoing basis if such Contributor 244 | notifies You of the non-compliance by some reasonable means, this is the 245 | first time You have received notice of non-compliance with this License 246 | from such Contributor, and You become compliant prior to 30 days after 247 | Your receipt of the notice. 248 | 249 | 5.2. If You initiate litigation against any entity by asserting a patent 250 | infringement claim (excluding declaratory judgment actions, 251 | counter-claims, and cross-claims) alleging that a Contributor Version 252 | directly or indirectly infringes any patent, then the rights granted to 253 | You by any and all Contributors for the Covered Software under Section 254 | 2.1 of this License shall terminate. 255 | 256 | 5.3. In the event of termination under Sections 5.1 or 5.2 above, all 257 | end user license agreements (excluding distributors and resellers) which 258 | have been validly granted by You or Your distributors under this License 259 | prior to termination shall survive termination. 260 | 261 | ************************************************************************ 262 | * * 263 | * 6. Disclaimer of Warranty * 264 | * ------------------------- * 265 | * * 266 | * Covered Software is provided under this License on an "as is" * 267 | * basis, without warranty of any kind, either expressed, implied, or * 268 | * statutory, including, without limitation, warranties that the * 269 | * Covered Software is free of defects, merchantable, fit for a * 270 | * particular purpose or non-infringing. The entire risk as to the * 271 | * quality and performance of the Covered Software is with You. * 272 | * Should any Covered Software prove defective in any respect, You * 273 | * (not any Contributor) assume the cost of any necessary servicing, * 274 | * repair, or correction. This disclaimer of warranty constitutes an * 275 | * essential part of this License. No use of any Covered Software is * 276 | * authorized under this License except under this disclaimer. * 277 | * * 278 | ************************************************************************ 279 | 280 | ************************************************************************ 281 | * * 282 | * 7. Limitation of Liability * 283 | * -------------------------- * 284 | * * 285 | * Under no circumstances and under no legal theory, whether tort * 286 | * (including negligence), contract, or otherwise, shall any * 287 | * Contributor, or anyone who distributes Covered Software as * 288 | * permitted above, be liable to You for any direct, indirect, * 289 | * special, incidental, or consequential damages of any character * 290 | * including, without limitation, damages for lost profits, loss of * 291 | * goodwill, work stoppage, computer failure or malfunction, or any * 292 | * and all other commercial damages or losses, even if such party * 293 | * shall have been informed of the possibility of such damages. This * 294 | * limitation of liability shall not apply to liability for death or * 295 | * personal injury resulting from such party's negligence to the * 296 | * extent applicable law prohibits such limitation. Some * 297 | * jurisdictions do not allow the exclusion or limitation of * 298 | * incidental or consequential damages, so this exclusion and * 299 | * limitation may not apply to You. * 300 | * * 301 | ************************************************************************ 302 | 303 | 8. Litigation 304 | ------------- 305 | 306 | Any litigation relating to this License may be brought only in the 307 | courts of a jurisdiction where the defendant maintains its principal 308 | place of business and such litigation shall be governed by laws of that 309 | jurisdiction, without reference to its conflict-of-law provisions. 310 | Nothing in this Section shall prevent a party's ability to bring 311 | cross-claims or counter-claims. 312 | 313 | 9. Miscellaneous 314 | ---------------- 315 | 316 | This License represents the complete agreement concerning the subject 317 | matter hereof. If any provision of this License is held to be 318 | unenforceable, such provision shall be reformed only to the extent 319 | necessary to make it enforceable. Any law or regulation which provides 320 | that the language of a contract shall be construed against the drafter 321 | shall not be used to construe this License against a Contributor. 322 | 323 | 10. Versions of the License 324 | --------------------------- 325 | 326 | 10.1. New Versions 327 | 328 | Mozilla Foundation is the license steward. Except as provided in Section 329 | 10.3, no one other than the license steward has the right to modify or 330 | publish new versions of this License. Each version will be given a 331 | distinguishing version number. 332 | 333 | 10.2. Effect of New Versions 334 | 335 | You may distribute the Covered Software under the terms of the version 336 | of the License under which You originally received the Covered Software, 337 | or under the terms of any subsequent version published by the license 338 | steward. 339 | 340 | 10.3. Modified Versions 341 | 342 | If you create software not governed by this License, and you want to 343 | create a new license for such software, you may create and use a 344 | modified version of this License if you rename the license and remove 345 | any references to the name of the license steward (except to note that 346 | such modified license differs from this License). 347 | 348 | 10.4. Distributing Source Code Form that is Incompatible With Secondary 349 | Licenses 350 | 351 | If You choose to distribute Source Code Form that is Incompatible With 352 | Secondary Licenses under the terms of this version of the License, the 353 | notice described in Exhibit B of this License must be attached. 354 | 355 | Exhibit A - Source Code Form License Notice 356 | ------------------------------------------- 357 | 358 | This Source Code Form is subject to the terms of the Mozilla Public 359 | License, v. 2.0. If a copy of the MPL was not distributed with this 360 | file, You can obtain one at http://mozilla.org/MPL/2.0/. 361 | 362 | If it is not possible or desirable to put the notice in a particular 363 | file, then You may include the notice in a location (such as a LICENSE 364 | file in a relevant directory) where a recipient would be likely to look 365 | for such a notice. 366 | 367 | You may add additional accurate notices of copyright ownership. 368 | 369 | Exhibit B - "Incompatible With Secondary Licenses" Notice 370 | --------------------------------------------------------- 371 | 372 | This Source Code Form is "Incompatible With Secondary Licenses", as 373 | defined by the Mozilla Public License, v. 2.0. 374 | 375 | -------------------------------------------------------------------------------- /src/mcl_ros/include/mcl_ros/MCL.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl_ros: Monte Carlo localization with ROS 3 | * Copyright (C) 2021-2022 Naoki Akai 4 | * 5 | * This Source Code Form is subject to the terms of the Mozilla Public 6 | * License, v. 2.0. If a copy of the MPL was not distributed with this file, 7 | * You can obtain one at https://mozilla.org/MPL/2.0/. 8 | * 9 | * @author Naoki Akai 10 | ****************************************************************************/ 11 | 12 | #ifndef __MCL_H__ 13 | #define __MCL_H__ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace mcl_ros { 32 | 33 | class MCL { 34 | private: 35 | ros::NodeHandle nh_; 36 | 37 | // subscribers 38 | std::string scanName_, odomName_, mapName_; 39 | ros::Subscriber scanSub_, odomSub_, mapSub_, initialPoseSub_; 40 | 41 | // publishers 42 | std::string poseName_, particlesName_, unknownScanName_, residualErrorsName_; 43 | ros::Publisher posePub_, particlesPub_, unknownScanPub_, residualErrorsPub_; 44 | 45 | // frames 46 | std::string laserFrame_, baseLinkFrame_, mapFrame_, odomFrame_; 47 | bool useOdomFrame_; 48 | 49 | std::vector initialPose_; 50 | Pose mclPose_, prevMCLPose_, baseLink2Laser_, odomPose_; 51 | ros::Time mclPoseStamp_, odomPoseStamp_; 52 | // prevMCLPose_ is used to predict the particles' pose by linear interpolation. 53 | // this prediction will be performed if useOdomMsg_ is false. 54 | 55 | // particles 56 | int particlesNum_; 57 | std::vector particles_; 58 | std::vector resampleIndices_; 59 | std::vector initialNoise_; 60 | bool useAugmentedMCL_, addRandomParticlesInResampling_; 61 | double randomParticlesRate_; 62 | std::vector randomParticlesNoise_; 63 | 64 | // map 65 | nav_msgs::OccupancyGrid ogm_; 66 | cv::Mat distMap_; 67 | double mapResolution_; 68 | Pose mapOrigin_; 69 | int mapWidth_, mapHeight_; 70 | double freeSpaceMinX_, freeSpaceMaxX_, freeSpaceMinY_, freeSpaceMaxY_; 71 | bool gotMap_; 72 | 73 | // motion 74 | double deltaX_, deltaY_, deltaDist_, deltaYaw_; 75 | double deltaXSum_, deltaYSum_, deltaDistSum_, deltaYawSum_, deltaTimeSum_; 76 | std::vector resampleThresholds_; 77 | std::vector odomNoiseDDM_, odomNoiseODM_; 78 | bool useOmniDirectionalModel_; 79 | bool useOdomMsg_; 80 | 81 | // measurements 82 | sensor_msgs::LaserScan scan_, unknownScan_; 83 | bool canUpdateScan_; 84 | 85 | // measurement model 86 | // 0: likelihood field model, 1: beam model, 2: class conditional measurement model 87 | int measurementModelType_; 88 | double zHit_, zShort_, zMax_, zRand_; 89 | double varHit_, lambdaShort_, lambdaUnknown_; 90 | double normConstHit_, denomHit_, pRand_; 91 | double measurementModelRandom_, measurementModelInvalidScan_; 92 | double pKnownPrior_, pUnknownPrior_, unknownScanProbThreshold_; 93 | double alphaSlow_, alphaFast_, omegaSlow_, omegaFast_; 94 | int scanStep_; 95 | bool rejectUnknownScan_, publishUnknownScan_, publishResidualErrors_; 96 | bool gotScan_; 97 | double resampleThresholdESS_; 98 | 99 | // localization type 100 | bool performGlobalLocalization_; 101 | 102 | // broadcast tf 103 | bool broadcastTF_; 104 | 105 | // localization result 106 | double totalLikelihood_, averageLikelihood_, maxLikelihood_; 107 | double amclRandomParticlesRate_, effectiveSampleSize_; 108 | int maxLikelihoodParticleIdx_; 109 | 110 | // localization correctness estimation 111 | float maxResidualError_; 112 | int minValidResidualErrorNum_; 113 | float meanAbsoluteErrorThreshold_; 114 | int failureCntThreshold_; 115 | bool useEstimationResetting_; 116 | double meanAbsoluteError_; 117 | int failureCnt_; 118 | 119 | // other parameters 120 | tf::TransformBroadcaster tfBroadcaster_; 121 | tf::TransformListener tfListener_; 122 | bool isInitialized_; 123 | double localizationHz_; 124 | 125 | // constant parameters 126 | const double rad2deg_; 127 | 128 | public: 129 | // inline setting functions 130 | inline void setCanUpdateScan(bool canUpdateScan) { canUpdateScan_ = canUpdateScan; } 131 | inline void setDeltaX(double deltaX) { deltaX_ = deltaX; } 132 | inline void setDeltaY(double deltaY) { deltaY_ = deltaY; } 133 | inline void setDeltaDist(double deltaDist) { deltaDist_ = deltaDist; } 134 | inline void setDeltaYaw(double deltaYaw) { deltaYaw_ = deltaYaw; } 135 | inline void setDeltaXSum(double deltaXSum) { deltaXSum_ = deltaXSum; } 136 | inline void setDeltaYSum(double deltaYSum) { deltaYSum_ = deltaYSum; } 137 | inline void setDeltaDistSum(double deltaDistSum) { deltaDistSum_ = deltaDistSum; } 138 | inline void setDeltaYawSum(double deltaYawSum) { deltaYawSum_ = deltaYawSum; } 139 | inline void setParticlePose(int i, double x, double y, double yaw) { particles_[i].setPose(x, y, yaw); } 140 | inline void setParticleW(int i, double w) { particles_[i].setW(w); } 141 | inline void setUseOdomMsg(bool useOdomMsg) { useOdomMsg_ = useOdomMsg; } 142 | inline void setTotalLikelihood(double totalLikelihood) { totalLikelihood_ = totalLikelihood; } 143 | inline void setAverageLikelihood(double averageLikelihood) { averageLikelihood_ = averageLikelihood; } 144 | inline void setMaxLikelihood(double maxLikelihood) { maxLikelihood_ = maxLikelihood; } 145 | inline void setMaxLikelihoodParticleIdx(int maxLikelihoodParticleIdx) { maxLikelihoodParticleIdx_ = maxLikelihoodParticleIdx; } 146 | 147 | // inline getting functions 148 | inline double getLocalizationHz(void) { return localizationHz_; } 149 | inline double getDeltaX(void) { return deltaX_; } 150 | inline double getDeltaY(void) { return deltaY_; } 151 | inline double getDeltaDist(void) { return deltaDist_; } 152 | inline double getDeltaYaw(void) { return deltaYaw_; } 153 | inline double getDeltaXSum(void) { return deltaXSum_; } 154 | inline double getDeltaYSum(void) { return deltaYSum_; } 155 | inline double getDeltaDistSum(void) { return deltaDistSum_; } 156 | inline double getDeltaYawSum(void) { return deltaYawSum_; } 157 | inline bool getUseOmniDirectionalModel(void) { return useOmniDirectionalModel_; } 158 | inline std::vector getOdomNoiseDDM(void) { return odomNoiseDDM_; } 159 | inline std::vector getOdomNoiseODM(void) { return odomNoiseODM_; } 160 | inline Pose getMCLPose(void) { return mclPose_; } 161 | inline Pose getBaseLink2Laser(void) { return baseLink2Laser_; } 162 | inline int getParticlesNum(void) { return particlesNum_; } 163 | inline Pose getParticlePose(int i) { return particles_[i].getPose(); } 164 | inline double getParticleW(int i) { return particles_[i].getW(); } 165 | inline std::vector getResampleIndices(void) { return resampleIndices_; } 166 | inline double getResampleThresholdESS(void) { return resampleThresholdESS_; } 167 | inline double getEffectiveSampleSize(void) { return effectiveSampleSize_; } 168 | 169 | MCL(void): 170 | nh_("~"), 171 | scanName_("/scan"), 172 | odomName_("/odom"), 173 | mapName_("/map"), 174 | poseName_("/mcl_pose"), 175 | particlesName_("/mcl_particles"), 176 | unknownScanName_("/unknown_scan"), 177 | residualErrorsName_("/residual_errors"), 178 | laserFrame_("laser"), 179 | baseLinkFrame_("base_link"), 180 | mapFrame_("map"), 181 | odomFrame_("odom"), 182 | useOdomFrame_(true), 183 | initialPose_({0.0, 0.0, 0.0}), 184 | particlesNum_(100), 185 | initialNoise_({0.2, 0.2, 0.02}), 186 | useAugmentedMCL_(false), 187 | addRandomParticlesInResampling_(true), 188 | randomParticlesRate_(0.1), 189 | randomParticlesNoise_({0.1, 0.1, 0.01}), 190 | odomNoiseDDM_({1.0, 0.5, 0.5, 1.0}), 191 | odomNoiseODM_({1.0, 0.5, 0.5, 0.5, 1.0, 0.5, 0.5, 0.5, 1.0}), 192 | deltaXSum_(0.0), 193 | deltaYSum_(0.0), 194 | deltaDistSum_(0.0), 195 | deltaYawSum_(0.0), 196 | deltaTimeSum_(0.0), 197 | resampleThresholds_({-1.0, -1.0, -1.0, -1.0, -1.0}), 198 | useOmniDirectionalModel_(false), 199 | useOdomMsg_(true), 200 | measurementModelType_(0), 201 | pKnownPrior_(0.5), 202 | pUnknownPrior_(0.5), 203 | scanStep_(10), 204 | zHit_(0.9), 205 | zShort_(0.2), 206 | zMax_(0.05), 207 | zRand_(0.05), 208 | varHit_(0.1), 209 | lambdaShort_(3.0), 210 | lambdaUnknown_(1.0), 211 | alphaSlow_(0.001), 212 | alphaFast_(0.9), 213 | omegaSlow_(0.0), 214 | omegaFast_(0.0), 215 | rejectUnknownScan_(false), 216 | publishUnknownScan_(false), 217 | publishResidualErrors_(false), 218 | resampleThresholdESS_(0.5), 219 | maxResidualError_(1.0f), 220 | minValidResidualErrorNum_(10), 221 | meanAbsoluteErrorThreshold_(0.5f), 222 | failureCntThreshold_(10), 223 | useEstimationResetting_(false), 224 | localizationHz_(10.0), 225 | performGlobalLocalization_(false), 226 | broadcastTF_(true), 227 | gotMap_(false), 228 | gotScan_(false), 229 | isInitialized_(true), 230 | canUpdateScan_(true), 231 | tfListener_(), 232 | rad2deg_(180.0 / M_PI) 233 | { 234 | // set seed for the random values based on the current time 235 | srand((unsigned int)time(NULL)); 236 | 237 | // topic and frame names 238 | nh_.param("scan_name", scanName_, scanName_); 239 | nh_.param("odom_name", odomName_, odomName_); 240 | nh_.param("map_name", mapName_, mapName_); 241 | nh_.param("pose_name", poseName_, poseName_); 242 | nh_.param("particles_name", particlesName_, particlesName_); 243 | nh_.param("unknown_scan_name", unknownScanName_, unknownScanName_); 244 | nh_.param("residual_errors_name", residualErrorsName_, residualErrorsName_); 245 | nh_.param("laser_frame", laserFrame_, laserFrame_); 246 | nh_.param("base_link_frame", baseLinkFrame_, baseLinkFrame_); 247 | nh_.param("map_frame", mapFrame_, mapFrame_); 248 | nh_.param("odom_frame", odomFrame_, odomFrame_); 249 | nh_.param("use_odom_frame", useOdomFrame_, useOdomFrame_); 250 | 251 | // particle filter parameters 252 | nh_.param("initial_pose", initialPose_, initialPose_); 253 | nh_.param("particle_num", particlesNum_, particlesNum_); 254 | nh_.param("initial_noise", initialNoise_, initialNoise_); 255 | nh_.param("use_augmented_mcl", useAugmentedMCL_, useAugmentedMCL_); 256 | nh_.param("add_random_particles_in_resampling", addRandomParticlesInResampling_, addRandomParticlesInResampling_); 257 | nh_.param("random_particles_rate", randomParticlesRate_, randomParticlesRate_); 258 | nh_.param("random_particles_noise", randomParticlesNoise_, randomParticlesNoise_); 259 | 260 | // motion 261 | nh_.param("odom_noise_ddm", odomNoiseDDM_, odomNoiseDDM_); 262 | nh_.param("odom_noise_odm", odomNoiseODM_, odomNoiseODM_); 263 | nh_.param("use_omni_directional_model", useOmniDirectionalModel_, useOmniDirectionalModel_); 264 | nh_.param("use_odom_msg", useOdomMsg_, useOdomMsg_); 265 | 266 | // measurement model 267 | nh_.param("measurement_model_type", measurementModelType_, measurementModelType_); 268 | nh_.param("scan_step", scanStep_, scanStep_); 269 | nh_.param("z_hit", zHit_, zHit_); 270 | nh_.param("z_short", zShort_, zShort_); 271 | nh_.param("z_max", zMax_, zMax_); 272 | nh_.param("z_rand", zRand_, zRand_); 273 | nh_.param("var_hit", varHit_, varHit_); 274 | nh_.param("lambda_short", lambdaShort_, lambdaShort_); 275 | nh_.param("lambda_unknown", lambdaUnknown_, lambdaUnknown_); 276 | nh_.param("known_class_prior", pKnownPrior_, pKnownPrior_); 277 | nh_.param("unknown_scan_prob_threshold", unknownScanProbThreshold_, unknownScanProbThreshold_); 278 | nh_.param("alpha_slow", alphaSlow_, alphaSlow_); 279 | nh_.param("alpha_fast", alphaFast_, alphaFast_); 280 | nh_.param("reject_unknown_scan", rejectUnknownScan_, rejectUnknownScan_); 281 | nh_.param("publish_unknown_scan", publishUnknownScan_, publishUnknownScan_); 282 | nh_.param("publish_residual_errors", publishResidualErrors_, publishResidualErrors_); 283 | nh_.param("resample_threshold_ess", resampleThresholdESS_, resampleThresholdESS_); 284 | nh_.param("resample_thresholds", resampleThresholds_, resampleThresholds_); 285 | pUnknownPrior_ = 1.0 - pKnownPrior_; 286 | 287 | // localization correctness estimation 288 | nh_.param("use_estimation_resetting", useEstimationResetting_, useEstimationResetting_); 289 | nh_.param("max_residual_error", maxResidualError_, maxResidualError_); 290 | nh_.param("min_valid_residual_error_num", minValidResidualErrorNum_, minValidResidualErrorNum_); 291 | nh_.param("mean_absolute_error_threshold", meanAbsoluteErrorThreshold_, meanAbsoluteErrorThreshold_); 292 | nh_.param("failure_cnt_threshold", failureCntThreshold_, failureCntThreshold_); 293 | 294 | // other parameters 295 | nh_.param("localization_hz", localizationHz_, localizationHz_); 296 | nh_.param("broadcast_tf", broadcastTF_, broadcastTF_); 297 | 298 | // localization type 299 | nh_.param("perform_global_localization", performGlobalLocalization_, performGlobalLocalization_); 300 | 301 | // set subscribers 302 | scanSub_ = nh_.subscribe(scanName_, 10, &MCL::scanCB, this); 303 | odomSub_ = nh_.subscribe(odomName_, 100, &MCL::odomCB, this); 304 | mapSub_ = nh_.subscribe(mapName_, 1, &MCL::mapCB, this); 305 | initialPoseSub_ = nh_.subscribe("/initialpose", 1, &MCL::initialPoseCB, this); 306 | 307 | // set publishers 308 | posePub_ = nh_.advertise(poseName_, 1); 309 | particlesPub_ = nh_.advertise(particlesName_, 1); 310 | unknownScanPub_ = nh_.advertise(unknownScanName_, 1); 311 | residualErrorsPub_ = nh_.advertise(residualErrorsName_, 1); 312 | 313 | // set initial pose 314 | mclPose_.setPose(initialPose_[0], initialPose_[1], initialPose_[2]); 315 | prevMCLPose_ = mclPose_; 316 | odomPose_.setPose(0.0, 0.0, 0.0); 317 | deltaX_ = deltaY_ = deltaDist_ = deltaYaw_ = 0.0; 318 | 319 | // get the relative pose from the base link to the laser from the tf tree 320 | ros::Rate loopRate(10); 321 | tf::StampedTransform tfBaseLink2Laser; 322 | int tfFailedCnt = 0; 323 | while (ros::ok()) { 324 | ros::spinOnce(); 325 | try { 326 | ros::Time now = ros::Time::now(); 327 | tfListener_.waitForTransform(baseLinkFrame_, laserFrame_, now, ros::Duration(5.0)); 328 | tfListener_.lookupTransform(baseLinkFrame_, laserFrame_, now, tfBaseLink2Laser); 329 | break; 330 | } catch (tf::TransformException ex) { 331 | tfFailedCnt++; 332 | if (tfFailedCnt >= 10) { 333 | ROS_ERROR("Cannot get the relative pose from the base link to the laser from the tf tree." 334 | " Did you set the static transform publisher between %s to %s?", 335 | baseLinkFrame_.c_str(), laserFrame_.c_str()); 336 | exit(1); 337 | } 338 | loopRate.sleep(); 339 | } 340 | } 341 | tf::Quaternion quatBaseLink2Laser(tfBaseLink2Laser.getRotation().x(), 342 | tfBaseLink2Laser.getRotation().y(), 343 | tfBaseLink2Laser.getRotation().z(), 344 | tfBaseLink2Laser.getRotation().w()); 345 | double baseLink2LaserRoll, baseLink2LaserPitch, baseLink2LaserYaw; 346 | tf::Matrix3x3 rotMatBaseLink2Laser(quatBaseLink2Laser); 347 | rotMatBaseLink2Laser.getRPY(baseLink2LaserRoll, baseLink2LaserPitch, baseLink2LaserYaw); 348 | baseLink2Laser_.setX(tfBaseLink2Laser.getOrigin().x()); 349 | baseLink2Laser_.setY(tfBaseLink2Laser.getOrigin().y()); 350 | baseLink2Laser_.setYaw(baseLink2LaserYaw); 351 | 352 | int mapFailedCnt = 0; 353 | while (ros::ok()) { 354 | ros::spinOnce(); 355 | if (gotMap_) 356 | break; 357 | mapFailedCnt++; 358 | if (mapFailedCnt >= 100) { 359 | ROS_ERROR("Cannot get a map message." 360 | " Did you pulish the map?" 361 | " Expected map topic name is %s\n", mapName_.c_str()); 362 | exit(1); 363 | } 364 | loopRate.sleep(); 365 | } 366 | 367 | int scanFailedCnt = 0; 368 | while (ros::ok()) { 369 | ros::spinOnce(); 370 | if (gotScan_) 371 | break; 372 | scanFailedCnt++; 373 | if (scanFailedCnt >= 100) { 374 | ROS_ERROR("Cannot get a scan message." 375 | " Did you pulish the scan?" 376 | " Expected scan topic name is %s\n", scanName_.c_str()); 377 | exit(1); 378 | } 379 | loopRate.sleep(); 380 | } 381 | 382 | // reset particles distribution 383 | if (performGlobalLocalization_) 384 | resetParticlesDistributionGlobally(); 385 | else 386 | resetParticlesDistribution(); 387 | resampleIndices_.resize(particlesNum_); 388 | 389 | // measurement model 390 | normConstHit_ = 1.0 / sqrt(2.0 * varHit_ * M_PI); 391 | denomHit_ = 1.0 / (2.0 * varHit_); 392 | pRand_ = 1.0 / (scan_.range_max / mapResolution_); 393 | measurementModelRandom_ = zRand_ * pRand_; 394 | measurementModelInvalidScan_ = zMax_ + zRand_ * pRand_; 395 | 396 | isInitialized_ = true; 397 | if (!useOdomMsg_) 398 | isInitialized_ = false; 399 | 400 | ROS_INFO("MCL ready to localize\n"); 401 | } 402 | 403 | void updateParticlesByMotionModel(void) { 404 | double deltaX, deltaY, deltaDist, deltaYaw; 405 | if (useOdomMsg_) { 406 | deltaX = deltaX_; 407 | deltaY = deltaY_; 408 | deltaDist = deltaDist_; 409 | deltaYaw = deltaYaw_; 410 | deltaX_ = deltaY_ = deltaDist_ = deltaYaw_ = 0.0; 411 | } else { 412 | // estimate robot's moving using the linear interpolation of the estimated pose 413 | double dx = mclPose_.getX() - prevMCLPose_.getX(); 414 | double dy = mclPose_.getY() - prevMCLPose_.getY(); 415 | double dyaw = mclPose_.getYaw() - prevMCLPose_.getYaw(); 416 | while (dyaw < -M_PI) 417 | dyaw += 2.0 * M_PI; 418 | while (dyaw > M_PI) 419 | dyaw -= 2.0 * M_PI; 420 | double t = -(atan2(dy, dx) - prevMCLPose_.getYaw()); 421 | deltaX = dx * cos(t) - dy * sin(t); 422 | deltaY = dx * sin(t) + dy * cos(t); 423 | deltaYaw = dyaw; 424 | deltaDist = sqrt(dx * dx + dy * dy); 425 | if (dx < 0.0) 426 | deltaDist *= -1.0; 427 | prevMCLPose_ = mclPose_; 428 | 429 | // calculate odometry 430 | if (!useOmniDirectionalModel_) { 431 | double t = odomPose_.getYaw() + deltaYaw / 2.0; 432 | double x = odomPose_.getX() + deltaDist * cos(t); 433 | double y = odomPose_.getY() + deltaDist * sin(t); 434 | double yaw = odomPose_.getYaw() + deltaYaw; 435 | odomPose_.setPose(x, y, yaw); 436 | } else { 437 | double t = odomPose_.getYaw() + deltaYaw / 2.0; 438 | double x = odomPose_.getX() + deltaX * cos(t) + deltaY * cos(t + M_PI / 2.0f); 439 | double y = odomPose_.getY() + deltaX * sin(t) + deltaY * sin(t + M_PI / 2.0f);; 440 | double yaw = odomPose_.getYaw() + deltaYaw; 441 | odomPose_.setPose(x, y, yaw); 442 | } 443 | } 444 | deltaXSum_ += fabs(deltaX); 445 | deltaYSum_ += fabs(deltaY); 446 | deltaDistSum_ += fabs(deltaDist); 447 | deltaYawSum_ += fabs(deltaYaw); 448 | 449 | if (!useOmniDirectionalModel_) { 450 | // differential drive model 451 | double dist2 = deltaDist * deltaDist; 452 | double yaw2 = deltaYaw * deltaYaw; 453 | double distRandVal = dist2 * odomNoiseDDM_[0] + yaw2 * odomNoiseDDM_[1]; 454 | double yawRandVal = dist2 * odomNoiseDDM_[2] + yaw2 * odomNoiseDDM_[3]; 455 | for (int i = 0; i < particlesNum_; ++i) { 456 | double ddist = deltaDist + nrand(distRandVal); 457 | double dyaw = deltaYaw + nrand(yawRandVal); 458 | double yaw = particles_[i].getYaw(); 459 | double t = yaw + dyaw / 2.0; 460 | double x = particles_[i].getX() + ddist * cos(t); 461 | double y = particles_[i].getY() + ddist * sin(t); 462 | yaw += dyaw; 463 | particles_[i].setPose(x, y, yaw); 464 | } 465 | } else { 466 | // omni directional model 467 | double x2 = deltaX * deltaX; 468 | double y2 = deltaY * deltaY; 469 | double yaw2 = deltaYaw * deltaYaw; 470 | double xRandVal = x2 * odomNoiseODM_[0] + y2 * odomNoiseODM_[1] + yaw2 * odomNoiseODM_[2]; 471 | double yRandVal = x2 * odomNoiseODM_[3] + y2 * odomNoiseODM_[4] + yaw2 * odomNoiseODM_[5]; 472 | double yawRandVal = x2 * odomNoiseODM_[6] + y2 * odomNoiseODM_[7] + yaw2 * odomNoiseODM_[8]; 473 | for (int i = 0; i < particlesNum_; ++i) { 474 | double dx = deltaX + nrand(xRandVal); 475 | double dy = deltaY + nrand(yRandVal); 476 | double dyaw = deltaYaw + nrand(yawRandVal); 477 | double yaw = particles_[i].getYaw(); 478 | double t = yaw + dyaw / 2.0; 479 | double x = particles_[i].getX() + dx * cos(t) + dy * cos(t + M_PI / 2.0f); 480 | double y = particles_[i].getY() + dx * sin(t) + dy * sin(t + M_PI / 2.0f);; 481 | yaw += dyaw; 482 | particles_[i].setPose(x, y, yaw); 483 | } 484 | } 485 | } 486 | 487 | void calculateLikelihoodsByMeasurementModel(void) { 488 | if (rejectUnknownScan_ && (measurementModelType_ == 0 || measurementModelType_ == 1)) 489 | rejectUnknownScan(); 490 | 491 | mclPoseStamp_ = scan_.header.stamp; 492 | double xo = baseLink2Laser_.getX(); 493 | double yo = baseLink2Laser_.getY(); 494 | double yawo = baseLink2Laser_.getYaw(); 495 | std::vector sensorPoses(particlesNum_); 496 | for (int i = 0; i < particlesNum_; ++i) { 497 | double yaw = particles_[i].getYaw(); 498 | double sensorX = xo * cos(yaw) - yo * sin(yaw) + particles_[i].getX(); 499 | double sensorY = xo * sin(yaw) + yo * cos(yaw) + particles_[i].getY(); 500 | double sensorYaw = yawo + yaw; 501 | Pose sensorPose(sensorX, sensorY, sensorYaw); 502 | sensorPoses[i] = sensorPose; 503 | particles_[i].setW(0.0); 504 | } 505 | 506 | for (int i = 0; i < (int)scan_.ranges.size(); i += scanStep_) { 507 | double range = scan_.ranges[i]; 508 | double rangeAngle = (double)i * scan_.angle_increment + scan_.angle_min; 509 | double max; 510 | for (int j = 0; j < particlesNum_; ++j) { 511 | double p; 512 | if (measurementModelType_ == 0) 513 | p = calculateLikelihoodFieldModel(sensorPoses[j], range, rangeAngle); 514 | else if (measurementModelType_ == 1) 515 | p = calculateBeamModel(sensorPoses[j], range, rangeAngle); 516 | else 517 | p = calculateClassConditionalMeasurementModel(sensorPoses[j], range, rangeAngle); 518 | double w = particles_[j].getW(); 519 | w += log(p); 520 | particles_[j].setW(w); 521 | if (j == 0) { 522 | max = w; 523 | } else { 524 | if (max < w) 525 | max = w; 526 | } 527 | } 528 | 529 | // Too small values cannot be calculated. 530 | // The log sum values are shifted if the maximum value is less than threshold. 531 | if (max < -300.0) { 532 | for (int j = 0; j < particlesNum_; ++j) { 533 | double w = particles_[j].getW() + 300.0; 534 | particles_[j].setW(w); 535 | } 536 | } 537 | } 538 | 539 | double sum = 0.0; 540 | double max; 541 | int maxIdx; 542 | for (int i = 0; i < particlesNum_; ++i) { 543 | // The log sum is converted to the probability. 544 | double w = exp(particles_[i].getW()); 545 | particles_[i].setW(w); 546 | sum += w; 547 | if (i == 0) { 548 | max = w; 549 | maxIdx = i; 550 | } else if (max < w) { 551 | max = w; 552 | maxIdx = i; 553 | } 554 | } 555 | totalLikelihood_ = sum; 556 | averageLikelihood_ = sum / (double)particlesNum_; 557 | maxLikelihood_ = max; 558 | maxLikelihoodParticleIdx_ = maxIdx; 559 | 560 | // augmented MCL 561 | // std::cout << "totalLikelihood_ = " << totalLikelihood_ << std::endl; 562 | // std::cout << "maxLikelihood_ = " << maxLikelihood_ << std::endl; 563 | // std::cout << "averageLikelihood_ = " << averageLikelihood_ << std::endl; 564 | omegaSlow_ += alphaSlow_ * (averageLikelihood_ - omegaSlow_); 565 | omegaFast_ += alphaFast_ * (averageLikelihood_ - omegaFast_); 566 | amclRandomParticlesRate_ = 1.0 - omegaFast_ / omegaSlow_; 567 | if (amclRandomParticlesRate_ < 0.0 || std::isnan(amclRandomParticlesRate_)) 568 | amclRandomParticlesRate_ = 0.0; 569 | 570 | // If publishUnknownScan_ is true and the class conditional measurement model is used, 571 | // unknown scan measurements are estimated based on the maximum likelihood particle. 572 | if (publishUnknownScan_ && measurementModelType_ == 2) { 573 | Pose mlPose = particles_[maxLikelihoodParticleIdx_].getPose(); 574 | estimateUnknownScanWithClassConditionalMeasurementModel(mlPose); 575 | } 576 | } 577 | 578 | void calculateEffectiveSampleSize(void) { 579 | // calculate the effective sample size 580 | double sum = 0.0; 581 | double wo = 1.0 / (double)particlesNum_; 582 | for (int i = 0; i < particlesNum_; ++i) { 583 | double w = particles_[i].getW() / totalLikelihood_; 584 | if (std::isnan(w)) 585 | w = wo; 586 | particles_[i].setW(w); 587 | sum += w * w; 588 | } 589 | effectiveSampleSize_ = 1.0 / sum; 590 | } 591 | 592 | void estimatePose(void) { 593 | double tmpYaw = mclPose_.getYaw(); 594 | double x = 0.0, y = 0.0, yaw = 0.0; 595 | for (size_t i = 0; i < particlesNum_; ++i) { 596 | double w = particles_[i].getW(); 597 | x += particles_[i].getX() * w; 598 | y += particles_[i].getY() * w; 599 | double dyaw = tmpYaw - particles_[i].getYaw(); 600 | while (dyaw < -M_PI) 601 | dyaw += 2.0 * M_PI; 602 | while (dyaw > M_PI) 603 | dyaw -= 2.0 * M_PI; 604 | yaw += dyaw * w; 605 | } 606 | yaw = tmpYaw - yaw; 607 | mclPose_.setPose(x, y, yaw); 608 | } 609 | 610 | void resampleParticles(void) { 611 | double threshold = (double)particlesNum_ * resampleThresholdESS_; 612 | if (effectiveSampleSize_ > threshold) 613 | return; 614 | 615 | if (deltaXSum_ < resampleThresholds_[0] && deltaYSum_ < resampleThresholds_[1] && 616 | deltaDistSum_ < resampleThresholds_[2] && deltaYawSum_ < resampleThresholds_[3] && 617 | deltaTimeSum_ < resampleThresholds_[4]) 618 | return; 619 | 620 | deltaXSum_ = deltaYSum_ = deltaDistSum_ = deltaYSum_ = deltaTimeSum_ = 0.0; 621 | std::vector wBuffer(particlesNum_); 622 | wBuffer[0] = particles_[0].getW(); 623 | for (int i = 1; i < particlesNum_; ++i) 624 | wBuffer[i] = particles_[i].getW() + wBuffer[i - 1]; 625 | 626 | std::vector tmpParticles = particles_; 627 | double wo = 1.0 / (double)particlesNum_; 628 | 629 | if (!addRandomParticlesInResampling_ && !useAugmentedMCL_) { 630 | // normal resampling 631 | for (int i = 0; i < particlesNum_; ++i) { 632 | double darts = (double)rand() / ((double)RAND_MAX + 1.0); 633 | for (int j = 0; j < particlesNum_; ++j) { 634 | if (darts < wBuffer[j]) { 635 | particles_[i].setPose(tmpParticles[j].getPose()); 636 | particles_[i].setW(wo); 637 | resampleIndices_[i] = j; 638 | break; 639 | } 640 | } 641 | } 642 | } else { 643 | // resampling and add random particles 644 | double randomParticlesRate = randomParticlesRate_; 645 | if (useAugmentedMCL_ && amclRandomParticlesRate_ > 0.0) { 646 | omegaSlow_ = omegaFast_ = 0.0; 647 | randomParticlesRate = amclRandomParticlesRate_; 648 | } else if (!addRandomParticlesInResampling_) { 649 | randomParticlesRate = 0.0; 650 | } 651 | int resampledParticlesNum = (int)((1.0 - randomParticlesRate) * (double)particlesNum_); 652 | int randomParticlesNum = particlesNum_ - resampledParticlesNum; 653 | for (int i = 0; i < resampledParticlesNum; ++i) { 654 | double darts = (double)rand() / ((double)RAND_MAX + 1.0); 655 | for (int j = 0; j < particlesNum_; ++j) { 656 | if (darts < wBuffer[j]) { 657 | particles_[i].setPose(tmpParticles[j].getPose()); 658 | particles_[i].setW(wo); 659 | resampleIndices_[i] = j; 660 | break; 661 | } 662 | } 663 | } 664 | double xo = mclPose_.getX(); 665 | double yo = mclPose_.getY(); 666 | double yawo = mclPose_.getYaw(); 667 | for (int i = resampledParticlesNum; i < resampledParticlesNum + randomParticlesNum; ++i) { 668 | double x = xo + nrand(randomParticlesNoise_[0]); 669 | double y = yo + nrand(randomParticlesNoise_[1]); 670 | double yaw = yawo + nrand(randomParticlesNoise_[2]); 671 | particles_[i].setPose(x, y, yaw); 672 | particles_[i].setW(wo); 673 | resampleIndices_[i] = -1; 674 | } 675 | } 676 | } 677 | 678 | std::vector getResidualErrors(void) { 679 | double yaw = mclPose_.getYaw(); 680 | double sensorX = baseLink2Laser_.getX() * cos(yaw) - baseLink2Laser_.getY() * sin(yaw) + mclPose_.getX(); 681 | double sensorY = baseLink2Laser_.getX() * sin(yaw) + baseLink2Laser_.getY() * cos(yaw) + mclPose_.getY(); 682 | double sensorYaw = baseLink2Laser_.getYaw() + yaw; 683 | std::vector residualErrors((int)scan_.ranges.size()); 684 | for (int i = 0; i < (int)scan_.ranges.size(); ++i) { 685 | double r = scan_.ranges[i]; 686 | if (r <= scan_.range_min || scan_.range_max <= r) { 687 | residualErrors[i] = -1.0; 688 | continue; 689 | } 690 | double t = (double)i * scan_.angle_increment + scan_.angle_min + sensorYaw; 691 | double x = r * cos(t) + sensorX; 692 | double y = r * sin(t) + sensorY; 693 | int u, v; 694 | xy2uv(x, y, &u, &v); 695 | if (onMap(u, v)) { 696 | float dist = distMap_.at(v, u); 697 | residualErrors[i] = dist; 698 | } else { 699 | residualErrors[i] = -1.0; 700 | } 701 | } 702 | return residualErrors; 703 | } 704 | 705 | float getMeanAbsoluteError(std::vector residualErrors) { 706 | float sum = 0.0f; 707 | int validRENum = 0; 708 | for (int i = 0; i < (int)residualErrors.size(); ++i) { 709 | float e = residualErrors[i]; 710 | if (0.0 <= e && e < maxResidualError_) { 711 | sum += e; 712 | validRENum++; 713 | } 714 | } 715 | if (validRENum < minValidResidualErrorNum_) 716 | return -1.0f; 717 | else 718 | return (sum / (float)validRENum); 719 | } 720 | 721 | void estimateLocalizationCorrectness(void) { 722 | if (!useEstimationResetting_) 723 | return; 724 | 725 | static int failureCnt = 0; 726 | std::vector residualErrors = getResidualErrors(); 727 | float mae = getMeanAbsoluteError(residualErrors); 728 | if (mae < 0.0f || meanAbsoluteErrorThreshold_ < mae) 729 | failureCnt++; 730 | else 731 | failureCnt = 0; 732 | meanAbsoluteError_ = mae; 733 | failureCnt_ = failureCnt; 734 | if (failureCnt >= failureCntThreshold_) { 735 | if (performGlobalLocalization_) 736 | resetParticlesDistributionGlobally(); 737 | else 738 | resetParticlesDistribution(); 739 | failureCnt = 0; 740 | } 741 | } 742 | 743 | void printResult(void) { 744 | std::cout << "MCL: x = " << mclPose_.getX() << " [m], y = " << mclPose_.getY() << " [m], yaw = " << mclPose_.getYaw() * rad2deg_ << " [deg]" << std::endl; 745 | std::cout << "Odom: x = " << odomPose_.getX() << " [m], y = " << odomPose_.getY() << " [m], yaw = " << odomPose_.getYaw() * rad2deg_ << " [deg]" << std::endl; 746 | std::cout << "total likelihood = " << totalLikelihood_ << std::endl; 747 | std::cout << "average likelihood = " << averageLikelihood_ << std::endl; 748 | std::cout << "max likelihood = " << maxLikelihood_ << std::endl; 749 | std::cout << "effective sample size = " << effectiveSampleSize_ << std::endl; 750 | if (useAugmentedMCL_) 751 | std::cout << "amcl random particles rate = " << amclRandomParticlesRate_ << std::endl; 752 | if (useEstimationResetting_) 753 | std::cout << "mean absolute error = " << meanAbsoluteError_ << " [m] (failureCnt = " << failureCnt_ << ")" << std::endl; 754 | std::cout << std::endl; 755 | } 756 | 757 | void publishROSMessages(void) { 758 | // pose 759 | geometry_msgs::PoseStamped pose; 760 | pose.header.frame_id = mapFrame_; 761 | pose.header.stamp = mclPoseStamp_; 762 | pose.pose.position.x = mclPose_.getX(); 763 | pose.pose.position.y = mclPose_.getY(); 764 | pose.pose.orientation = tf::createQuaternionMsgFromYaw(mclPose_.getYaw()); 765 | posePub_.publish(pose); 766 | 767 | // particles 768 | geometry_msgs::PoseArray particlesPoses; 769 | particlesPoses.header.frame_id = mapFrame_; 770 | particlesPoses.header.stamp = mclPoseStamp_; 771 | particlesPoses.poses.resize(particlesNum_); 772 | for (int i = 0; i < particlesNum_; ++i) { 773 | geometry_msgs::Pose pose; 774 | pose.position.x = particles_[i].getX(); 775 | pose.position.y = particles_[i].getY(); 776 | pose.orientation = tf::createQuaternionMsgFromYaw(particles_[i].getYaw()); 777 | particlesPoses.poses[i] = pose; 778 | } 779 | particlesPub_.publish(particlesPoses); 780 | 781 | // unknown scan 782 | if (publishUnknownScan_ && (rejectUnknownScan_ || measurementModelType_ == 2)) 783 | unknownScanPub_.publish(unknownScan_); 784 | 785 | // residual errors 786 | if (publishResidualErrors_) { 787 | sensor_msgs::LaserScan residualErrors = scan_; 788 | residualErrors.ranges = getResidualErrors(); 789 | residualErrorsPub_.publish(residualErrors); 790 | } 791 | } 792 | 793 | void broadcastTF(void) { 794 | if (!broadcastTF_) 795 | return; 796 | 797 | if (useOdomFrame_) { 798 | if (!useOdomMsg_) { 799 | geometry_msgs::TransformStamped odom2baseLinkTrans; 800 | odom2baseLinkTrans.header.stamp = mclPoseStamp_; 801 | odom2baseLinkTrans.header.frame_id = odomFrame_; 802 | odom2baseLinkTrans.child_frame_id = baseLinkFrame_; 803 | odom2baseLinkTrans.transform.translation.x = odomPose_.getX(); 804 | odom2baseLinkTrans.transform.translation.y = odomPose_.getY(); 805 | odom2baseLinkTrans.transform.translation.z = 0.0; 806 | odom2baseLinkTrans.transform.rotation = tf::createQuaternionMsgFromYaw(odomPose_.getYaw()); 807 | tfBroadcaster_.sendTransform(odom2baseLinkTrans); 808 | } 809 | 810 | geometry_msgs::Pose poseOnMap; 811 | poseOnMap.position.x = mclPose_.getX(); 812 | poseOnMap.position.y = mclPose_.getY(); 813 | poseOnMap.position.z = 0.0; 814 | poseOnMap.orientation = tf::createQuaternionMsgFromYaw(mclPose_.getYaw()); 815 | tf2::Transform map2baseTrans; 816 | tf2::convert(poseOnMap, map2baseTrans); 817 | 818 | geometry_msgs::Pose poseOnOdom; 819 | poseOnOdom.position.x = odomPose_.getX(); 820 | poseOnOdom.position.y = odomPose_.getY(); 821 | poseOnOdom.position.z = 0.0; 822 | poseOnOdom.orientation = tf::createQuaternionMsgFromYaw(odomPose_.getYaw()); 823 | tf2::Transform odom2baseTrans; 824 | tf2::convert(poseOnOdom, odom2baseTrans); 825 | 826 | tf2::Transform map2odomTrans = map2baseTrans * odom2baseTrans.inverse(); 827 | geometry_msgs::TransformStamped map2odomStampedTrans; 828 | map2odomStampedTrans.header.stamp = mclPoseStamp_; 829 | map2odomStampedTrans.header.frame_id = mapFrame_; 830 | map2odomStampedTrans.child_frame_id = odomFrame_; 831 | tf2::convert(map2odomTrans, map2odomStampedTrans.transform); 832 | tfBroadcaster_.sendTransform(map2odomStampedTrans); 833 | } else { 834 | tf::Transform tf; 835 | tf::Quaternion q; 836 | tf.setOrigin(tf::Vector3(mclPose_.getX(), mclPose_.getY(), 0.0)); 837 | q.setRPY(0.0, 0.0, mclPose_.getYaw()); 838 | tf.setRotation(q); 839 | tfBroadcaster_.sendTransform(tf::StampedTransform(tf, mclPoseStamp_, mapFrame_, baseLinkFrame_)); 840 | } 841 | } 842 | 843 | private: 844 | inline double nrand(double n) { return (n * sqrt(-2.0 * log((double)rand() / RAND_MAX)) * cos(2.0 * M_PI * rand() / RAND_MAX)); } 845 | inline double urand(double min, double max) { return ((max - min) * (double)rand() / RAND_MAX + min); } 846 | 847 | inline bool onMap(int u, int v) { 848 | if (0 <= u && u < mapWidth_ && 0 <= v && v < mapHeight_) 849 | return true; 850 | else 851 | return false; 852 | } 853 | 854 | inline bool onFreeSpace(int u, int v) { 855 | if (!onMap(u, v)) 856 | return false; 857 | int node = v * mapWidth_ + u; 858 | if (ogm_.data[node] == 0) 859 | return true; 860 | else 861 | return false; 862 | } 863 | 864 | inline void xy2uv(double x, double y, int *u, int *v) { 865 | double dx = x - mapOrigin_.getX(); 866 | double dy = y - mapOrigin_.getY(); 867 | double yaw = -mapOrigin_.getYaw(); 868 | double xx = dx * cos(yaw) - dy * sin(yaw); 869 | double yy = dx * sin(yaw) + dy * cos(yaw); 870 | *u = (int)(xx / mapResolution_); 871 | *v = (int)(yy / mapResolution_); 872 | } 873 | 874 | inline void uv2xy(int u, int v, double *x, double *y) { 875 | double xx = (double)u * mapResolution_; 876 | double yy = (double)v * mapResolution_; 877 | double yaw = mapOrigin_.getYaw(); 878 | double dx = xx * cos(yaw) - yy * sin(yaw); 879 | double dy = xx * sin(yaw) + yy * cos(yaw); 880 | *x = dx + mapOrigin_.getX(); 881 | *y = dy + mapOrigin_.getY(); 882 | } 883 | 884 | void scanCB(const sensor_msgs::LaserScan::ConstPtr &msg) { 885 | if (canUpdateScan_) 886 | scan_ = *msg; 887 | if (!gotScan_) 888 | gotScan_ = true; 889 | } 890 | 891 | void odomCB(const nav_msgs::Odometry::ConstPtr &msg) { 892 | if (!useOdomMsg_) 893 | return; 894 | 895 | static double prevTime; 896 | double currTime = msg->header.stamp.toSec(); 897 | if (isInitialized_) { 898 | prevTime = currTime; 899 | isInitialized_ = false; 900 | return; 901 | } 902 | odomPoseStamp_ = msg->header.stamp; 903 | double deltaTime = currTime - prevTime; 904 | deltaX_ += msg->twist.twist.linear.x * deltaTime; 905 | deltaY_ += msg->twist.twist.linear.y * deltaTime; 906 | deltaDist_ += msg->twist.twist.linear.x * deltaTime; 907 | deltaYaw_ += msg->twist.twist.angular.z * deltaTime; 908 | while (deltaYaw_ < -M_PI) 909 | deltaYaw_ += 2.0 * M_PI; 910 | while (deltaYaw_ > M_PI) 911 | deltaYaw_ -= 2.0 * M_PI; 912 | deltaTimeSum_ += deltaTime; 913 | 914 | tf::Quaternion q(msg->pose.pose.orientation.x, 915 | msg->pose.pose.orientation.y, 916 | msg->pose.pose.orientation.z, 917 | msg->pose.pose.orientation.w); 918 | double roll, pitch, yaw; 919 | tf::Matrix3x3 m(q); 920 | m.getRPY(roll, pitch, yaw); 921 | odomPose_.setPose(msg->pose.pose.position.x, msg->pose.pose.position.y, yaw); 922 | 923 | prevTime = currTime; 924 | } 925 | 926 | void mapCB(const nav_msgs::OccupancyGrid::ConstPtr &msg) { 927 | // store the map information 928 | ogm_ = *msg; 929 | mapWidth_ = msg->info.width; 930 | mapHeight_ = msg->info.height; 931 | mapResolution_ = msg->info.resolution; 932 | tf::Quaternion q(msg->info.origin.orientation.x, 933 | msg->info.origin.orientation.y, 934 | msg->info.origin.orientation.z, 935 | msg->info.origin.orientation.w); 936 | double roll, pitch, yaw; 937 | tf::Matrix3x3 m(q); 938 | m.getRPY(roll, pitch, yaw); 939 | mapOrigin_.setX(msg->info.origin.position.x); 940 | mapOrigin_.setY(msg->info.origin.position.y); 941 | mapOrigin_.setYaw(yaw); 942 | 943 | // perform distance transform to build the distance field 944 | // the min and max points of the free space are also obtained 945 | cv::Mat binMap(mapHeight_, mapWidth_, CV_8UC1); 946 | double minX, maxX, minY, maxY; 947 | bool isFirst = true; 948 | for (int v = 0; v < mapHeight_; v++) { 949 | for (int u = 0; u < mapWidth_; u++) { 950 | int node = v * mapWidth_ + u; 951 | int val = msg->data[node]; 952 | if (val == 100) { 953 | // occupied grid 954 | binMap.at(v, u) = 0; 955 | } else { 956 | binMap.at(v, u) = 1; 957 | if (val == 0) { 958 | double x, y; 959 | uv2xy(u, v, &x, &y); 960 | if (isFirst) { 961 | minX = maxX = x; 962 | minY = maxY = y; 963 | isFirst = false; 964 | } else { 965 | if (x < minX) 966 | minX = x; 967 | if (x > maxX) 968 | maxX = x; 969 | if (y < minY) 970 | minY = y; 971 | if (y > maxY) 972 | maxY = y; 973 | } 974 | } 975 | } 976 | } 977 | } 978 | freeSpaceMinX_ = minX; 979 | freeSpaceMaxX_ = maxX; 980 | freeSpaceMinY_ = minY; 981 | freeSpaceMaxY_ = maxY; 982 | cv::Mat distMap(mapHeight_, mapWidth_, CV_32FC1); 983 | cv::distanceTransform(binMap, distMap, cv::DIST_L2, 5); 984 | for (int v = 0; v < mapHeight_; v++) { 985 | for (int u = 0; u < mapWidth_; u++) { 986 | float d = distMap.at(v, u) * (float)mapResolution_; 987 | distMap.at(v, u) = d; 988 | } 989 | } 990 | distMap_ = distMap; 991 | gotMap_ = true; 992 | } 993 | 994 | void initialPoseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) { 995 | tf::Quaternion q(msg->pose.pose.orientation.x, 996 | msg->pose.pose.orientation.y, 997 | msg->pose.pose.orientation.z, 998 | msg->pose.pose.orientation.w); 999 | double roll, pitch, yaw; 1000 | tf::Matrix3x3 m(q); 1001 | m.getRPY(roll, pitch, yaw); 1002 | mclPose_.setPose(msg->pose.pose.position.x, msg->pose.pose.position.y, yaw); 1003 | resetParticlesDistribution(); 1004 | omegaSlow_ = omegaFast_ = 0.0; 1005 | isInitialized_ = true; 1006 | } 1007 | 1008 | void resetParticlesDistribution(void) { 1009 | particles_.resize(particlesNum_); 1010 | double xo = mclPose_.getX(); 1011 | double yo = mclPose_.getY(); 1012 | double yawo = mclPose_.getYaw(); 1013 | double wo = 1.0 / (double)particlesNum_; 1014 | for (int i = 0; i < particlesNum_; ++i) { 1015 | double x = xo + nrand(initialNoise_[0]); 1016 | double y = yo + nrand(initialNoise_[1]); 1017 | double yaw = yawo + nrand(initialNoise_[2]); 1018 | particles_[i].setPose(x, y, yaw); 1019 | particles_[i].setW(wo); 1020 | } 1021 | } 1022 | 1023 | void resetParticlesDistributionGlobally(void) { 1024 | particles_.resize(particlesNum_); 1025 | double wo = 1.0 / (double)particlesNum_; 1026 | for (int i = 0; i < particlesNum_; ++i) { 1027 | for (;;) { 1028 | double x = urand(freeSpaceMinX_, freeSpaceMaxX_); 1029 | double y = urand(freeSpaceMinY_, freeSpaceMaxY_); 1030 | int u, v; 1031 | xy2uv(x, y, &u, &v); 1032 | if (onFreeSpace(u, v)) { 1033 | double yaw = urand(-M_PI, M_PI); 1034 | particles_[i].setPose(x, y, yaw); 1035 | particles_[i].setW(wo); 1036 | break; 1037 | } 1038 | } 1039 | } 1040 | } 1041 | 1042 | void rejectUnknownScan(void) { 1043 | unknownScan_ = scan_; 1044 | double xo = baseLink2Laser_.getX(); 1045 | double yo = baseLink2Laser_.getY(); 1046 | double yawo = baseLink2Laser_.getYaw(); 1047 | double hitThreshold = 0.5 * mapResolution_; 1048 | for (int i = 0; i < (int)unknownScan_.ranges.size(); ++i) { 1049 | if (i % scanStep_ != 0) { 1050 | unknownScan_.ranges[i] = 0.0; 1051 | continue; 1052 | } 1053 | double r = unknownScan_.ranges[i]; 1054 | if (r <= unknownScan_.range_min || unknownScan_.range_max <= r) { 1055 | unknownScan_.ranges[i] = 0.0; 1056 | continue; 1057 | } 1058 | double laserYaw = (double)i * unknownScan_.angle_increment + unknownScan_.angle_min; 1059 | double pShortSum = 0.0, pBeamSum = 0.0; 1060 | for (int j = 0; j < particlesNum_; ++j) { 1061 | double yaw = particles_[j].getYaw(); 1062 | double x = xo * cos(yaw) - yo * sin(yaw) + particles_[j].getX(); 1063 | double y = xo * sin(yaw) + yo * cos(yaw) + particles_[j].getY(); 1064 | double t = yawo + yaw + laserYaw; 1065 | double dx = mapResolution_ * cos(t); 1066 | double dy = mapResolution_ * sin(t); 1067 | int u, v; 1068 | double expectedRange = -1.0; 1069 | for (double range = 0.0; range <= unknownScan_.range_max; range += mapResolution_) { 1070 | xy2uv(x, y, &u, &v); 1071 | if (onMap(u, v)) { 1072 | double dist = (double)distMap_.at(v, u); 1073 | if (dist < hitThreshold) { 1074 | expectedRange = range; 1075 | break; 1076 | } 1077 | } else { 1078 | break; 1079 | } 1080 | x += dx; 1081 | y += dy; 1082 | } 1083 | if (r <= expectedRange) { 1084 | double error = expectedRange - r; 1085 | double pHit = normConstHit_ * exp(-(error * error) * denomHit_) * mapResolution_; 1086 | double pShort = lambdaShort_ * exp(-lambdaShort_ * r) / (1.0 - exp(-lambdaShort_ * unknownScan_.range_max)) * mapResolution_; 1087 | pShortSum += pShort; 1088 | pBeamSum += zHit_ * pHit + zShort_ * pShort + measurementModelRandom_; 1089 | } else { 1090 | pBeamSum += measurementModelRandom_; 1091 | } 1092 | } 1093 | double pShort = pShortSum; 1094 | double pBeam = pBeamSum; 1095 | double pUnknown = pShortSum / pBeamSum; 1096 | if (pUnknown < unknownScanProbThreshold_) { 1097 | unknownScan_.ranges[i] = 0.0; 1098 | } else { 1099 | // unknown scan is rejected from the scan message used for localization 1100 | scan_.ranges[i] = 0.0; 1101 | } 1102 | } 1103 | } 1104 | 1105 | double calculateLikelihoodFieldModel(Pose pose, double range, double rangeAngle) { 1106 | if (range <= scan_.range_min || scan_.range_max <= range) 1107 | return measurementModelInvalidScan_; 1108 | 1109 | double t = pose.getYaw() + rangeAngle; 1110 | double x = range * cos(t) + pose.getX(); 1111 | double y = range * sin(t) + pose.getY(); 1112 | int u, v; 1113 | xy2uv(x, y, &u, &v); 1114 | double p; 1115 | if (onMap(u, v)) { 1116 | double dist = (double)distMap_.at(v, u); 1117 | double pHit = normConstHit_ * exp(-(dist * dist) * denomHit_) * mapResolution_; 1118 | p = zHit_ * pHit + measurementModelRandom_; 1119 | } else { 1120 | p = measurementModelRandom_; 1121 | } 1122 | if (p > 1.0) 1123 | p = 1.0; 1124 | return p; 1125 | } 1126 | 1127 | double calculateBeamModel(Pose pose, double range, double rangeAngle) { 1128 | if (range <= scan_.range_min || scan_.range_max <= range) 1129 | return measurementModelInvalidScan_; 1130 | 1131 | double t = pose.getYaw() + rangeAngle; 1132 | double x = pose.getX(); 1133 | double y = pose.getY(); 1134 | double dx = mapResolution_ * cos(t); 1135 | double dy = mapResolution_ * sin(t); 1136 | int u, v; 1137 | double expectedRange = -1.0; 1138 | double hitThreshold = 0.5 * mapResolution_; 1139 | for (double r = 0.0; r < scan_.range_max; r += mapResolution_) { 1140 | xy2uv(x, y, &u, &v); 1141 | if (onMap(u, v)) { 1142 | double dist = (double)distMap_.at(v, u); 1143 | if (dist < hitThreshold) { 1144 | expectedRange = r; 1145 | break; 1146 | } 1147 | } else { 1148 | break; 1149 | } 1150 | x += dx; 1151 | y += dy; 1152 | } 1153 | 1154 | double p; 1155 | if (range <= expectedRange) { 1156 | double error = expectedRange - range; 1157 | double pHit = normConstHit_ * exp(-(error * error) * denomHit_) * mapResolution_; 1158 | double pShort = lambdaShort_ * exp(-lambdaShort_ * range) / (1.0 - exp(-lambdaShort_ * scan_.range_max)) * mapResolution_; 1159 | p = zHit_ * pHit + zShort_ * pShort + measurementModelRandom_; 1160 | } else { 1161 | p = measurementModelRandom_; 1162 | } 1163 | if (p > 1.0) 1164 | p = 1.0; 1165 | return p; 1166 | } 1167 | 1168 | double calculateClassConditionalMeasurementModel(Pose pose, double range, double rangeAngle) { 1169 | if (range <= scan_.range_min || scan_.range_max <= range) 1170 | return measurementModelInvalidScan_; 1171 | 1172 | double t = pose.getYaw() + rangeAngle; 1173 | double x = range * cos(t) + pose.getX(); 1174 | double y = range * sin(t) + pose.getY(); 1175 | double pUnknown = lambdaUnknown_ * exp(-lambdaUnknown_ * range) / (1.0 - exp(-lambdaUnknown_ * scan_.range_max)) * mapResolution_ * pUnknownPrior_; 1176 | int u, v; 1177 | xy2uv(x, y, &u, &v); 1178 | double p = pUnknown; 1179 | if (onMap(u, v)) { 1180 | double dist = (double)distMap_.at(v, u); 1181 | double pHit = normConstHit_ * exp(-(dist * dist) * denomHit_) * mapResolution_; 1182 | p += (zHit_ * pHit + measurementModelRandom_) * pKnownPrior_; 1183 | } else { 1184 | p += measurementModelRandom_ * pKnownPrior_; 1185 | } 1186 | if (p > 1.0) 1187 | p = 1.0; 1188 | return p; 1189 | } 1190 | 1191 | void estimateUnknownScanWithClassConditionalMeasurementModel(Pose pose) { 1192 | unknownScan_ = scan_; 1193 | double yaw = pose.getYaw(); 1194 | double sensorX = baseLink2Laser_.getX() * cos(yaw) - baseLink2Laser_.getY() * sin(yaw) + pose.getX(); 1195 | double sensorY = baseLink2Laser_.getX() * sin(yaw) + baseLink2Laser_.getY() * cos(yaw) + pose.getY(); 1196 | double sensorYaw = baseLink2Laser_.getYaw() + yaw; 1197 | for (int i = 0; i < (int)unknownScan_.ranges.size(); ++i) { 1198 | double r = unknownScan_.ranges[i]; 1199 | if (r <= unknownScan_.range_min || unknownScan_.range_max <= r) { 1200 | unknownScan_.ranges[i] = 0.0; 1201 | continue; 1202 | } 1203 | double t = sensorYaw + (double)i * unknownScan_.angle_increment + unknownScan_.angle_min; 1204 | double x = r * cos(t) + sensorX; 1205 | double y = r * sin(t) + sensorY; 1206 | int u, v; 1207 | xy2uv(x, y, &u, &v); 1208 | double pKnown; 1209 | double pUnknown = lambdaUnknown_ * exp(-lambdaUnknown_ * r) / (1.0 - exp(-lambdaUnknown_ * unknownScan_.range_max)) * mapResolution_ * pUnknownPrior_; 1210 | if (onMap(u, v)) { 1211 | double dist = (double)distMap_.at(v, u); 1212 | double pHit = normConstHit_ * exp(-(dist * dist) * denomHit_) * mapResolution_; 1213 | pKnown = (zHit_ * pHit + measurementModelRandom_) * pKnownPrior_; 1214 | } else { 1215 | pKnown = measurementModelRandom_ * pKnownPrior_; 1216 | } 1217 | double sum = pKnown + pUnknown; 1218 | pUnknown /= sum; 1219 | if (pUnknown < unknownScanProbThreshold_) 1220 | unknownScan_.ranges[i] = 0.0; 1221 | } 1222 | } 1223 | 1224 | }; // class MCL 1225 | 1226 | } // namespace mcl_ros 1227 | 1228 | #endif // __MCL_H__ 1229 | --------------------------------------------------------------------------------