├── test ├── demo.bash ├── test.launch.xml ├── test.bash └── test_gui.bash ├── .gitignore ├── .github └── workflows │ ├── rerun.yml │ └── test.yml ├── config ├── emcl2.param.yaml └── emcl2_quick_start.param.yaml ├── include └── emcl2 │ ├── Scan.h │ ├── OdomModel.h │ ├── Pose.h │ ├── LikelihoodFieldMap.h │ ├── ExpResetMcl2.h │ ├── Particle.h │ ├── Mcl.h │ └── emcl2_node.h ├── src ├── OdomModel.cpp ├── Scan.cpp ├── Pose.cpp ├── LikelihoodFieldMap.cpp ├── ExpResetMcl2.cpp ├── Particle.cpp ├── Mcl.cpp └── emcl2_node.cpp ├── .clang-format ├── CMakeLists.txt ├── package.xml ├── launch └── emcl2.launch.py ├── COPYING └── README.md /test/demo.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ros2 pkg list | grep -q turtlebot3_gazebo || { echo install turtlebot3_gazebo ; exit 1; } 4 | ros2 pkg list | grep -q nav2_bringup || { echo install nav2_bringup ; exit 1; } 5 | 6 | TURTLEBOT3_MODEL=burger ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py & 7 | sleep 3 8 | ros2 launch nav2_bringup rviz_launch.py & 9 | sleep 1 10 | ros2 launch nav2_bringup navigation_launch.py use_sim_time:=true & 11 | sleep 5 12 | ros2 launch emcl2 emcl2.launch.py params_file:=$(ros2 pkg prefix --share emcl2)/config/emcl2_quick_start.param.yaml map:=$(ros2 pkg prefix --share nav2_bringup)/maps/turtlebot3_world.yaml use_sim_time:=true 13 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Vscode 51 | .vscode 52 | 53 | # Catkin custom files 54 | CATKIN_IGNORE 55 | -------------------------------------------------------------------------------- /.github/workflows/rerun.yml: -------------------------------------------------------------------------------- 1 | name: rerun-test 2 | 3 | on: 4 | workflow_dispatch: 5 | 6 | jobs: 7 | retry: 8 | runs-on: ubuntu-22.04 9 | steps: 10 | - uses: actions/checkout@v2 11 | - name: Check and rerun or cancel workflow 12 | env: 13 | GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} 14 | run: | 15 | STATUS=$(gh run list -w test.yml | awk 'NR==4 {print $1}') 16 | RUN_ID=$(gh run list -w test.yml | grep -oE '[0-9]{10}' | head -n 1) 17 | 18 | if [ "$STATUS" == "completed" ]; then 19 | echo "Job is completed, rerunning..." 20 | gh run rerun $RUN_ID 21 | else 22 | echo "Job is not completed, canceling and rerunning..." 23 | gh run cancel $RUN_ID && gh run rerun $RUN_ID 24 | fi 25 | -------------------------------------------------------------------------------- /config/emcl2.param.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | odom_freq: 20 4 | transform_tolerance: 0.2 5 | global_frame_id: "map" 6 | odom_frame_id: "odom" 7 | base_frame_id: "base_link" 8 | footprint_frame_id: "base_footprint" 9 | laser_min_range: 0.0 10 | laser_max_range: 100000000.0 11 | scan_increment: 1 12 | initial_pose_x: 0.0 13 | initial_pose_y: 0.0 14 | initial_pose_a: 0.0 15 | num_particles: 500 16 | alpha_threshold: 0.5 17 | expansion_radius_position: 0.1 18 | expansion_radius_orientation: 0.2 19 | extraction_rate: 0.1 20 | range_threshold: 0.3 21 | sensor_reset: false 22 | odom_fw_dev_per_fw: 0.19 23 | odom_fw_dev_per_rot: 0.0001 24 | odom_rot_dev_per_fw: 0.13 25 | odom_rot_dev_per_rot: 0.2 26 | laser_likelihood_max_dist: 0.2 27 | -------------------------------------------------------------------------------- /config/emcl2_quick_start.param.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | odom_freq: 20 4 | transform_tolerance: 0.2 5 | global_frame_id: "map" 6 | odom_frame_id: "odom" 7 | base_frame_id: "base_link" 8 | footprint_frame_id: "base_footprint" 9 | laser_min_range: 0.0 10 | laser_max_range: 100000000.0 11 | scan_increment: 1 12 | initial_pose_x: -2.0 13 | initial_pose_y: -0.5 14 | initial_pose_a: 0.0 15 | num_particles: 500 16 | alpha_threshold: 0.5 17 | expansion_radius_position: 0.1 18 | expansion_radius_orientation: 0.2 19 | extraction_rate: 0.1 20 | range_threshold: 0.3 21 | sensor_reset: false 22 | odom_fw_dev_per_fw: 0.19 23 | odom_fw_dev_per_rot: 0.0001 24 | odom_rot_dev_per_fw: 0.13 25 | odom_rot_dev_per_rot: 0.2 26 | laser_likelihood_max_dist: 0.1 27 | -------------------------------------------------------------------------------- /test/test.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /include/emcl2/Scan.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #ifndef EMCL2__SCAN_H_ 5 | #define EMCL2__SCAN_H_ 6 | 7 | #include 8 | #include 9 | 10 | namespace emcl2 11 | { 12 | 13 | class Scan 14 | { 15 | public: 16 | int seq_; 17 | int scan_increment_; 18 | double angle_max_; 19 | double angle_min_; 20 | double angle_increment_; 21 | double range_max_; 22 | double range_min_; 23 | 24 | double lidar_pose_x_; 25 | double lidar_pose_y_; 26 | double lidar_pose_yaw_; 27 | 28 | std::vector ranges_; 29 | std::vector directions_16bit_; 30 | 31 | Scan & operator=(const Scan & s); 32 | int countValidBeams(double * rate = NULL); 33 | bool valid(double range); 34 | }; 35 | 36 | } // namespace emcl2 37 | 38 | #endif // EMCL2__SCAN_H_ 39 | -------------------------------------------------------------------------------- /include/emcl2/OdomModel.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #ifndef EMCL2__ODOMMODEL_H_ 5 | #define EMCL2__ODOMMODEL_H_ 6 | 7 | #include 8 | 9 | namespace emcl2 10 | { 11 | class OdomModel 12 | { 13 | public: 14 | OdomModel(double ff, double fr, double rf, double rr); 15 | void setDev(double length, double angle); 16 | double drawFwNoise(void); 17 | double drawRotNoise(void); 18 | 19 | private: 20 | double fw_dev_; 21 | double rot_dev_; 22 | 23 | double fw_var_per_fw_; 24 | double fw_var_per_rot_; 25 | double rot_var_per_fw_; 26 | double rot_var_per_rot_; 27 | 28 | std::random_device seed_gen_; 29 | std::default_random_engine engine_{seed_gen_()}; 30 | 31 | std::normal_distribution<> std_norm_dist_; 32 | }; 33 | 34 | } // namespace emcl2 35 | 36 | #endif // EMCL2__ODOMMODEL_H_ 37 | -------------------------------------------------------------------------------- /include/emcl2/Pose.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #ifndef EMCL2__POSE_H_ 5 | #define EMCL2__POSE_H_ 6 | 7 | #include 8 | #include 9 | 10 | namespace emcl2 11 | { 12 | 13 | class Pose 14 | { 15 | public: 16 | Pose() {} 17 | Pose(double x, double y, double t); 18 | Pose(const Pose & other); 19 | 20 | void set(double x, double y, double t); 21 | void set(const Pose & p); 22 | std::string to_s(void); 23 | 24 | void normalizeAngle(void); 25 | void move( 26 | double length, double direction, double rotation, double fw_noise, double rot_noise); 27 | 28 | Pose operator-(const Pose & p) const; 29 | Pose operator=(const Pose & p); 30 | 31 | bool nearlyZero(void); 32 | 33 | double x_, y_, t_; 34 | 35 | uint16_t get16bitRepresentation(void); 36 | static uint16_t get16bitRepresentation(double); 37 | }; 38 | 39 | } // namespace emcl2 40 | 41 | #endif // EMCL2__POSE_H_ 42 | -------------------------------------------------------------------------------- /src/OdomModel.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #include "emcl2/OdomModel.h" 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace emcl2 12 | { 13 | OdomModel::OdomModel(double ff, double fr, double rf, double rr) 14 | : fw_dev_(0.0), rot_dev_(0.0), engine_(seed_gen_()), std_norm_dist_(0.0, 1.0) 15 | { 16 | fw_var_per_fw_ = ff * ff; 17 | fw_var_per_rot_ = fr * fr; 18 | rot_var_per_fw_ = rf * rf; 19 | rot_var_per_rot_ = rr * rr; 20 | } 21 | 22 | void OdomModel::setDev(double length, double angle) 23 | { 24 | fw_dev_ = sqrt(fabs(length) * fw_var_per_fw_ + fabs(angle) * fw_var_per_rot_); 25 | rot_dev_ = sqrt(fabs(length) * rot_var_per_fw_ + fabs(angle) * rot_var_per_rot_); 26 | } 27 | 28 | double OdomModel::drawFwNoise(void) { return std_norm_dist_(engine_) * fw_dev_; } 29 | 30 | double OdomModel::drawRotNoise(void) { return std_norm_dist_(engine_) * rot_dev_; } 31 | 32 | } // namespace emcl2 33 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Google 3 | 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: AlwaysBreak 6 | BraceWrapping: 7 | AfterClass: true 8 | AfterFunction: true 9 | AfterNamespace: true 10 | AfterStruct: true 11 | BreakBeforeBraces: Custom 12 | ColumnLimit: 100 13 | ConstructorInitializerIndentWidth: 0 14 | ContinuationIndentWidth: 2 15 | DerivePointerAlignment: false 16 | UseTab: Always 17 | IndentWidth: 8 18 | TabWidth: 8 19 | PointerAlignment: Middle 20 | ReflowComments: false 21 | 22 | IncludeCategories: 23 | - Regex: <[a-z_]*> 24 | Priority: 6 25 | CaseSensitive: true 26 | - Regex: <.*\.h> 27 | Priority: 5 28 | CaseSensitive: true 29 | - Regex: boost/.* 30 | Priority: 4 31 | CaseSensitive: true 32 | - Regex: .*_msgs/.* 33 | Priority: 3 34 | CaseSensitive: true 35 | - Regex: .*_srvs/.* 36 | Priority: 3 37 | CaseSensitive: true 38 | - Regex: <.*> 39 | Priority: 2 40 | CaseSensitive: true 41 | - Regex: '".*"' 42 | Priority: 1 43 | CaseSensitive: true -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(emcl2) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-O3 -Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake_auto REQUIRED) 13 | 14 | ament_auto_find_build_dependencies() 15 | 16 | ament_auto_add_executable(emcl2_node 17 | src/Mcl.cpp 18 | src/ExpResetMcl2.cpp 19 | src/Particle.cpp 20 | src/OdomModel.cpp 21 | src/Pose.cpp 22 | src/Scan.cpp 23 | src/LikelihoodFieldMap.cpp 24 | src/emcl2_node.cpp 25 | ) 26 | 27 | if(BUILD_TESTING) 28 | find_package(ament_lint_auto REQUIRED) 29 | set(ament_cmake_copyright_FOUND TRUE) 30 | set(ament_cmake_cpplint_FOUND TRUE) 31 | ament_lint_auto_find_test_dependencies() 32 | endif() 33 | 34 | install(PROGRAMS 35 | test/test.bash 36 | test/test_gui.bash 37 | DESTINATION lib/${PROJECT_NAME} 38 | ) 39 | 40 | ament_auto_package( 41 | INSTALL_TO_SHARE 42 | config 43 | launch 44 | test 45 | ) 46 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | emcl2 5 | 0.0.0 6 | The emcl2 package 7 | 8 | Ryuichi Ueda 9 | LGPL 10 | 11 | https://github.com/ryuichiueda/emcl2/ 12 | 13 | ament_cmake_auto 14 | 15 | rclcpp 16 | geometry_msgs 17 | nav_msgs 18 | sensor_msgs 19 | ros2launch 20 | tf2 21 | tf2_ros 22 | tf2_msgs 23 | tf2_geometry_msgs 24 | std_srvs 25 | 26 | ament_lint_auto 27 | ament_lint_common 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /include/emcl2/LikelihoodFieldMap.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #ifndef EMCL2__LIKELIHOODFIELDMAP_H_ 5 | #define EMCL2__LIKELIHOODFIELDMAP_H_ 6 | 7 | #include "emcl2/Pose.h" 8 | #include "emcl2/Scan.h" 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace emcl2 16 | { 17 | class LikelihoodFieldMap 18 | { 19 | public: 20 | LikelihoodFieldMap(const nav_msgs::msg::OccupancyGrid & map, double likelihood_range); 21 | ~LikelihoodFieldMap(); 22 | 23 | void setLikelihood(int x, int y, double range); 24 | uint8_t likelihood(double x, double y); 25 | 26 | std::vector likelihoods_; 27 | int width_; 28 | int height_; 29 | 30 | double resolution_; 31 | double origin_x_; 32 | double origin_y_; 33 | 34 | void drawFreePoses(int num, std::vector & result); 35 | 36 | private: 37 | std::vector> free_cells_; 38 | 39 | void normalize(void); 40 | }; 41 | 42 | } // namespace emcl2 43 | 44 | #endif // EMCL2__LIKELIHOODFIELDMAP_H_ 45 | -------------------------------------------------------------------------------- /src/Scan.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #include "emcl2/Scan.h" 5 | 6 | #include 7 | 8 | namespace emcl2 9 | { 10 | Scan & Scan::operator=(const Scan & s) 11 | { 12 | if (this == &s) { 13 | return *this; 14 | } 15 | 16 | seq_ = s.seq_; 17 | scan_increment_ = s.scan_increment_; 18 | angle_max_ = s.angle_max_; 19 | angle_min_ = s.angle_min_; 20 | angle_increment_ = s.angle_increment_; 21 | range_max_ = s.range_max_; 22 | range_min_ = s.range_min_; 23 | 24 | // It's not thread safe. 25 | ranges_.clear(); 26 | copy(s.ranges_.begin(), s.ranges_.end(), back_inserter(ranges_)); 27 | 28 | return *this; 29 | } 30 | 31 | int Scan::countValidBeams(double * rate) 32 | { 33 | int ans = 0; 34 | for (size_t i = 0; i < ranges_.size(); i += scan_increment_) { 35 | if (valid(ranges_[i])) { 36 | ans++; 37 | } 38 | } 39 | 40 | if (rate != NULL) { 41 | *rate = static_cast(ans) / ranges_.size() * scan_increment_; 42 | } 43 | 44 | return ans; 45 | } 46 | 47 | bool Scan::valid(double range) 48 | { 49 | if (std::isnan(range) || std::isinf(range)) { 50 | return false; 51 | } 52 | 53 | return range_min_ <= range && range <= range_max_; 54 | } 55 | 56 | } // namespace emcl2 57 | -------------------------------------------------------------------------------- /include/emcl2/ExpResetMcl2.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #ifndef EMCL2__EXPRESETMCL2_H_ 5 | #define EMCL2__EXPRESETMCL2_H_ 6 | 7 | #include "emcl2/Mcl.h" 8 | 9 | #include 10 | 11 | namespace emcl2 12 | { 13 | class ExpResetMcl2 : public Mcl 14 | { 15 | public: 16 | ExpResetMcl2( 17 | const Pose & p, int num, const Scan & scan, const std::shared_ptr & odom_model, 18 | const std::shared_ptr & map, double alpha_th, 19 | double expansion_radius_position, double expansion_radius_orientation, 20 | double extraction_rate, double successive_penetration_threshold, bool sensor_reset); 21 | ~ExpResetMcl2(); 22 | 23 | void sensorUpdate(double lidar_x, double lidar_y, double lidar_t, bool inv); 24 | 25 | private: 26 | double alpha_threshold_; 27 | double expansion_radius_position_; 28 | double expansion_radius_orientation_; 29 | 30 | double extraction_rate_; 31 | double range_threshold_; 32 | bool sensor_reset_; 33 | 34 | void expansionReset(void); 35 | 36 | // bool Particle::isPenetrating( 37 | double nonPenetrationRate(int skip, LikelihoodFieldMap * map, Scan & scan); 38 | }; 39 | 40 | } // namespace emcl2 41 | 42 | #endif // EMCL2__EXPRESETMCL2_H_ 43 | -------------------------------------------------------------------------------- /include/emcl2/Particle.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #ifndef EMCL2__PARTICLE_H_ 5 | #define EMCL2__PARTICLE_H_ 6 | 7 | #include "emcl2/LikelihoodFieldMap.h" 8 | #include "emcl2/Pose.h" 9 | 10 | namespace emcl2 11 | { 12 | class Particle 13 | { 14 | public: 15 | Particle(double x, double y, double t, double w); 16 | Particle(const Particle & other) = default; 17 | 18 | Particle & operator=(const Particle & other) 19 | { 20 | if (this != &other) { 21 | this->p_ = other.p_; 22 | this->w_ = other.w_; 23 | } 24 | return *this; 25 | } 26 | 27 | double likelihood(LikelihoodFieldMap * map, Scan & scan); 28 | bool wallConflict(LikelihoodFieldMap * map, Scan & scan, double threshold, bool replace); 29 | Pose p_; 30 | double w_; 31 | 32 | private: 33 | bool isPenetrating( 34 | double ox, double oy, double range, uint16_t direction, LikelihoodFieldMap * map, 35 | double & hit_lx, double & hit_ly); 36 | 37 | bool checkWallConflict( 38 | LikelihoodFieldMap * map, double ox, double oy, double range, uint16_t direction, 39 | double threshold, bool replace); 40 | 41 | void sensorReset( 42 | double ox, double oy, double range1, uint16_t direction1, double hit_lx1, double hit_ly1, 43 | double range2, uint16_t direction2, double hit_lx2, double hit_ly2); 44 | }; 45 | 46 | } // namespace emcl2 47 | 48 | #endif // EMCL2__PARTICLE_H_ 49 | -------------------------------------------------------------------------------- /include/emcl2/Mcl.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #ifndef EMCL2__MCL_H_ 5 | #define EMCL2__MCL_H_ 6 | 7 | #include "emcl2/LikelihoodFieldMap.h" 8 | #include "emcl2/OdomModel.h" 9 | #include "emcl2/Particle.h" 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace emcl2 20 | { 21 | class Mcl 22 | { 23 | public: 24 | Mcl() {} 25 | Mcl( 26 | const Pose & p, int num, const Scan & scan, const std::shared_ptr & odom_model, 27 | const std::shared_ptr & map); 28 | ~Mcl(); 29 | 30 | std::vector particles_; 31 | double alpha_; 32 | 33 | void sensorUpdate(double lidar_x, double lidar_y, double lidar_t, bool inv); 34 | void motionUpdate(double x, double y, double t); 35 | 36 | void initialize(double x, double y, double t); 37 | 38 | void setScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg); 39 | void meanPose( 40 | double & x_mean, double & y_mean, double & t_mean, double & x_var, double & y_var, 41 | double & t_var, double & xy_cov, double & yt_cov, double & tx_cov); 42 | 43 | void simpleReset(void); 44 | 45 | static double cos_[(1 << 16)]; 46 | static double sin_[(1 << 16)]; 47 | 48 | protected: 49 | Pose * last_odom_; 50 | Pose * prev_odom_; 51 | 52 | Scan scan_; 53 | int processed_seq_; 54 | 55 | double normalizeAngle(double t); 56 | void resampling(void); 57 | double normalizeBelief(void); 58 | void resetWeight(void); 59 | 60 | std::shared_ptr odom_model_; 61 | std::shared_ptr map_; 62 | }; 63 | 64 | } // namespace emcl2 65 | 66 | #endif // EMCL2__MCL_H_ 67 | -------------------------------------------------------------------------------- /test/test.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash -evx 2 | 3 | ros2 daemon stop 4 | ros2 daemon start 5 | 6 | xvfb-run --auto-servernum -s "-screen 0 1400x900x24" ros2 launch emcl2 test.launch.xml & 7 | sleep 30 8 | 9 | ### ESTIMATION RECOVERY TEST ### 10 | # Publish initial pose 11 | ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped " 12 | header: 13 | stamp: 14 | sec: 0 15 | nanosec: 0 16 | frame_id: 'map' 17 | pose: 18 | pose: 19 | position: {x: -2.5, y: 0.0, z: 0.0} 20 | orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0} 21 | covariance: [ 22 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 23 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 24 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 26 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 27 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" | head -n 5 28 | 29 | # Check if position is within a threshold 30 | ros2 topic echo /mcl_pose --csv | 31 | head -n 1000 | 32 | awk -F',' '{print $4" "$5} 33 | sqrt( ($4+2.0)^2 + ($5+0.5)^2 ) < 0.2 {printf "\033[42m%s\033[m\n", "ESTIMATION RECOVERY TEST OK";exit(0)} 34 | NR==1000{printf "\033[41m%s\033[m\n", "ESTIMATION RECOVERY TEST TIMEOUT";exit(1)}' 35 | 36 | if [ "$?" -ne 0 ]; then 37 | exit 1 38 | fi 39 | 40 | ros2 service call /global_costmap/clear_entirely_global_costmap nav2_msgs/srv/ClearEntireCostmap request:\ {}\ 41 | 42 | sleep 20 43 | 44 | ### NAVIGATION TEST ### 45 | # Publish nav2 goal 46 | ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose " 47 | pose: 48 | header: 49 | stamp: 50 | sec: 0 51 | frame_id: 'map' 52 | pose: 53 | position: { x: -0.5, y: -0.5, z: 0.0} 54 | orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0}" & 55 | 56 | # Check if position is within a threshold 57 | ros2 topic echo /mcl_pose --csv | 58 | head -n 1000 | 59 | awk -F',' '{print $4" "$5} 60 | sqrt( ($4+0.5)^2 + ($5+0.5)^2 ) < 0.3 {printf "\033[42m%s\033[m\n", "NAVIGATION TEST OK";exit(0)} 61 | NR==1000{printf "\033[41m%s\033[m\n", "NAVIGATION TEST TIMEOUT";exit(1)}' 62 | 63 | RESULT=$? 64 | 65 | exit $RESULT 66 | -------------------------------------------------------------------------------- /test/test_gui.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash -evx 2 | 3 | ros2 launch emcl2 test.launch.xml & 4 | sleep 50 5 | 6 | ### ESTIMATION RECOVERY TEST ### 7 | # Publish initial pose 8 | ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped " 9 | header: 10 | stamp: 11 | sec: 0 12 | nanosec: 0 13 | frame_id: '' 14 | pose: 15 | pose: 16 | position: {x: -2.5, y: 0.0, z: 0.0} 17 | orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0} 18 | covariance: [ 19 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 20 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 21 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 23 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 24 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" | head -n 10 25 | 26 | # Check if position is within a threshold 27 | ros2 topic echo /mcl_pose --csv | 28 | head -n 1000 | 29 | awk -F',' '{print $4" "$5} 30 | sqrt( ($4+2.0)^2 + ($5+0.5)^2 ) < 0.2 {printf "\033[42m%s\033[m\n", "ESTIMATION RECOVERY TEST OK";exit(0)} 31 | NR==1000{printf "\033[41m%s\033[m\n", "ESTIMATION RECOVERY TEST TIMEOUT";exit(1)}' 32 | 33 | if [ "$?" -ne 0 ]; then 34 | ps aux | grep ros | grep -v grep | awk '{ print "kill -9", $2 }' | sh 35 | killall -9 gzclient gzserver rviz2 36 | exit 1 37 | fi 38 | 39 | ros2 service call /global_costmap/clear_entirely_global_costmap nav2_msgs/srv/ClearEntireCostmap request:\ {}\ 40 | 41 | sleep 10 42 | 43 | ### NAVIGATION TEST ### 44 | # Publish nav2 goal 45 | ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose " 46 | pose: 47 | header: 48 | stamp: 49 | sec: 0 50 | frame_id: 'map' 51 | pose: 52 | position: { x: -0.5, y: -0.5, z: 0.0} 53 | orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0}" & 54 | 55 | # Check if position is within a threshold 56 | ros2 topic echo /mcl_pose --csv | 57 | head -n 1000 | 58 | awk -F',' '{print $4" "$5} 59 | sqrt( ($4+0.5)^2 + ($5+0.5)^2 ) < 0.3 {printf "\033[42m%s\033[m\n", "NAVIGATION TEST OK";exit(0)} 60 | NR==1000{printf "\033[41m%s\033[m\n", "NAVIGATION TEST TIMEOUT";exit(1)}' 61 | 62 | RESULT=$? 63 | 64 | ps aux | grep ros | grep -v grep | awk '{ print "kill -9", $2 }' | sh 65 | killall -9 gzclient gzserver rviz2 66 | 67 | exit $RESULT 68 | -------------------------------------------------------------------------------- /src/Pose.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #include "emcl2/Pose.h" 5 | 6 | #include 7 | #include 8 | 9 | namespace emcl2 10 | { 11 | Pose::Pose(double x, double y, double t) { set(x, y, t); } 12 | 13 | Pose::Pose(const Pose & other) { set(other); } 14 | 15 | void Pose::set(double x, double y, double t) 16 | { 17 | x_ = x; 18 | y_ = y; 19 | t_ = t; 20 | } 21 | 22 | void Pose::set(const Pose & p) 23 | { 24 | x_ = p.x_; 25 | y_ = p.y_; 26 | t_ = p.t_; 27 | } 28 | 29 | std::string Pose::to_s(void) 30 | { 31 | std::stringstream s; 32 | s << "x:" << x_ << "\ty:" << y_ << "\tt:" << t_; 33 | return s.str(); 34 | } 35 | 36 | void Pose::normalizeAngle(void) 37 | { 38 | while (t_ > M_PI) { 39 | t_ -= 2 * M_PI; 40 | } 41 | while (t_ < -M_PI) { 42 | t_ += 2 * M_PI; 43 | } 44 | } 45 | 46 | Pose Pose::operator-(const Pose & p) const 47 | { 48 | Pose ans(x_ - p.x_, y_ - p.y_, t_ - p.t_); 49 | ans.normalizeAngle(); 50 | 51 | return ans; 52 | } 53 | 54 | Pose Pose::operator=(const Pose & p) 55 | { 56 | if (this != &p) { 57 | x_ = p.x_; 58 | y_ = p.y_; 59 | t_ = p.t_; 60 | } 61 | return *this; 62 | } 63 | 64 | void Pose::move(double length, double direction, double rotation, double fw_noise, double rot_noise) 65 | { 66 | x_ += (length + fw_noise) * cos(direction + rot_noise + t_); 67 | y_ += (length + fw_noise) * sin(direction + rot_noise + t_); 68 | t_ += rotation + rot_noise; 69 | normalizeAngle(); 70 | } 71 | 72 | bool Pose::nearlyZero(void) { return fabs(x_) < 0.001 && fabs(y_) < 0.001 && fabs(t_) < 0.001; } 73 | 74 | uint16_t Pose::get16bitRepresentation(void) 75 | { 76 | int tmp = t_ / M_PI * (1 << 15); 77 | while (tmp < 0) { 78 | tmp += (1 << 16); 79 | } 80 | while (tmp >= (1 << 16)) { 81 | tmp -= (1 << 16); 82 | } 83 | 84 | return (uint16_t)tmp; 85 | } 86 | 87 | uint16_t Pose::get16bitRepresentation(double t) 88 | { 89 | int tmp = t / M_PI * (1 << 15); 90 | while (tmp < 0) { 91 | tmp += (1 << 16); 92 | } 93 | while (tmp >= (1 << 16)) { 94 | tmp -= (1 << 16); 95 | } 96 | 97 | return (uint16_t)tmp; 98 | } 99 | 100 | } // namespace emcl2 101 | -------------------------------------------------------------------------------- /launch/emcl2.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch.actions import DeclareLaunchArgument, GroupAction 7 | from launch.substitutions import LaunchConfiguration, TextSubstitution 8 | from launch_ros.actions import Node, SetParameter 9 | 10 | 11 | def generate_launch_description(): 12 | params_file = LaunchConfiguration('params_file') 13 | map_yaml_file = LaunchConfiguration('map') 14 | use_sim_time = LaunchConfiguration('use_sim_time') 15 | 16 | declare_map_yaml = DeclareLaunchArgument( 17 | 'map', 18 | default_value='', 19 | description='Full path to map yaml file to load') 20 | declare_use_sim_time = DeclareLaunchArgument( 21 | 'use_sim_time', 22 | default_value='false', 23 | description='Use simulation (Gazebo) clock if true') 24 | declare_params_file = DeclareLaunchArgument( 25 | 'params_file', 26 | default_value=[ 27 | TextSubstitution(text=os.path.join( 28 | get_package_share_directory('emcl2'), 'config', '')), 29 | TextSubstitution(text='emcl2.param.yaml')], 30 | description='emcl2 param file path') 31 | 32 | lifecycle_nodes = ['map_server'] 33 | 34 | launch_node = GroupAction( 35 | actions=[ 36 | SetParameter('use_sim_time', use_sim_time), 37 | Node( 38 | package='nav2_map_server', 39 | executable='map_server', 40 | name='map_server', 41 | parameters=[{'yaml_filename': map_yaml_file}], 42 | output='screen'), 43 | Node( 44 | name='emcl2', 45 | package='emcl2', 46 | executable='emcl2_node', 47 | parameters=[params_file], 48 | output='screen'), 49 | Node( 50 | package='nav2_lifecycle_manager', 51 | executable='lifecycle_manager', 52 | name='lifecycle_manager_localization', 53 | output='screen', 54 | parameters=[{'autostart': True}, 55 | {'node_names': lifecycle_nodes}]) 56 | ] 57 | ) 58 | 59 | ld = LaunchDescription() 60 | ld.add_action(declare_map_yaml) 61 | ld.add_action(declare_use_sim_time) 62 | ld.add_action(declare_params_file) 63 | 64 | ld.add_action(launch_node) 65 | 66 | return ld 67 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: test 2 | 3 | on: 4 | push: 5 | branches: 6 | - main 7 | paths-ignore: 8 | - '**.md' 9 | pull_request: 10 | branches: 11 | - main 12 | paths-ignore: 13 | - '**.md' 14 | 15 | env: 16 | RETRY-TEST-NUM: 10 17 | 18 | jobs: 19 | test: 20 | runs-on: ubuntu-22.04 21 | timeout-minutes: 5 22 | container: osrf/ros:humble-desktop-full 23 | steps: 24 | - uses: actions/checkout@v2 25 | - name: Apt update install 26 | run: | 27 | sudo apt update -y 28 | sudo apt install -y \ 29 | psmisc \ 30 | rsync \ 31 | xvfb \ 32 | ros-$ROS_DISTRO-navigation2 \ 33 | ros-$ROS_DISTRO-nav2-bringup \ 34 | ros-$ROS_DISTRO-turtlebot3-gazebo 35 | - name: Build 36 | run: | 37 | mkdir -p /root/ros2_ws/src/emcl2_ros2/ 38 | rsync -av ./ /root/ros2_ws/src/emcl2_ros2/ 39 | cd /root/ros2_ws 40 | source /opt/ros/$ROS_DISTRO/setup.bash 41 | rosdep update 42 | rosdep install -i -y --from-path src --rosdistro $ROS_DISTRO 43 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release 44 | shell: bash 45 | - name: Exec 46 | run: | 47 | source /root/ros2_ws/install/setup.bash 48 | ros2 run emcl2 test.bash 49 | shell: bash 50 | 51 | retry: 52 | runs-on: ubuntu-22.04 53 | needs: test 54 | if: ${{ always() && needs.test.result == 'failure' }} 55 | steps: 56 | - uses: actions/github-script@v6 57 | with: 58 | github-token: ${{ secrets.GITHUB_TOKEN }} 59 | script: | 60 | async function run() { 61 | const { owner, repo } = context.repo; 62 | const workflow_id = 'rerun.yml'; 63 | const ref = ('${{ github.head_ref }}' === '' || '${{ github.head_ref }}' === 'main') ? 'main' : '${{ github.head_ref }}'; 64 | const run_id = context.runId; 65 | 66 | try { 67 | const workflow_run = await github.rest.actions.getWorkflowRun({ 68 | owner, 69 | repo, 70 | run_id 71 | }); 72 | 73 | if (workflow_run.data.run_attempt < ${{ env.RETRY-TEST-NUM }}) { 74 | console.log('Triggering workflow dispatch...'); 75 | await github.rest.actions.createWorkflowDispatch({ 76 | owner, 77 | repo, 78 | workflow_id, 79 | ref 80 | }); 81 | } else { 82 | console.log('Conditions not met for re-dispatch.'); 83 | } 84 | } catch (error) { 85 | console.error('Failed to fetch workflow run or dispatch:', error); 86 | } 87 | } 88 | run(); 89 | -------------------------------------------------------------------------------- /src/LikelihoodFieldMap.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: BSD-3-Clause 3 | 4 | #include "emcl2/LikelihoodFieldMap.h" 5 | 6 | #include "emcl2/Pose.h" 7 | 8 | #include 9 | #include 10 | 11 | namespace emcl2 12 | { 13 | LikelihoodFieldMap::LikelihoodFieldMap( 14 | const nav_msgs::msg::OccupancyGrid & map, double likelihood_range) 15 | { 16 | width_ = map.info.width; 17 | height_ = map.info.height; 18 | 19 | origin_x_ = map.info.origin.position.x; 20 | origin_y_ = map.info.origin.position.y; 21 | 22 | resolution_ = map.info.resolution; 23 | 24 | for (int x = 0; x < width_; x++) { 25 | likelihoods_.push_back(new uint8_t[height_]); 26 | 27 | for (int y = 0; y < height_; y++) { 28 | likelihoods_[x][y] = 0; 29 | } 30 | } 31 | 32 | for (int x = 0; x < width_; x++) { 33 | for (int y = 0; y < height_; y++) { 34 | int v = map.data[x + y * width_]; 35 | if (v > 50) { 36 | setLikelihood(x, y, likelihood_range); 37 | } else if (0 <= v && v <= 50) { 38 | free_cells_.push_back(std::pair(x, y)); 39 | } 40 | } 41 | } 42 | 43 | //normalize(); 44 | } 45 | 46 | LikelihoodFieldMap::~LikelihoodFieldMap() 47 | { 48 | for (auto & e : likelihoods_) { 49 | delete[] e; 50 | } 51 | } 52 | 53 | uint8_t LikelihoodFieldMap::likelihood(double x, double y) 54 | { 55 | int ix = static_cast(floor((x - origin_x_) / resolution_)); 56 | int iy = static_cast(floor((y - origin_y_) / resolution_)); 57 | 58 | if (ix < 0 || iy < 0 || ix >= width_ || iy >= height_) { 59 | return 0.0; 60 | } 61 | 62 | return likelihoods_[ix][iy]; 63 | } 64 | 65 | void LikelihoodFieldMap::setLikelihood(int x, int y, double range) 66 | { 67 | int cell_num = static_cast(ceil(range / resolution_)); 68 | std::vector weights; 69 | for (int i = 0; i <= cell_num; i++) { 70 | weights.push_back(static_cast(255*(1.0 - static_cast(i) / cell_num))); 71 | } 72 | 73 | for (int i = -cell_num; i <= cell_num; i++) { 74 | for (int j = -cell_num; j <= cell_num; j++) { 75 | if (i + x >= 0 && j + y >= 0 && i + x < width_ && j + y < height_) { 76 | likelihoods_[i + x][j + y] = std::max( 77 | likelihoods_[i + x][j + y], 78 | std::min(weights[abs(i)], weights[abs(j)])); 79 | } 80 | } 81 | } 82 | } 83 | 84 | /* 85 | void LikelihoodFieldMap::normalize(void) 86 | { 87 | uint8_t maximum = 0; 88 | for (int x = 0; x < width_; x++) { 89 | for (int y = 0; y < height_; y++) { 90 | maximum = std::max(likelihoods_[x][y], maximum); 91 | } 92 | } 93 | 94 | for (int x = 0; x < width_; x++) { 95 | for (int y = 0; y < height_; y++) { 96 | likelihoods_[x][y] /= maximum; 97 | } 98 | } 99 | } 100 | */ 101 | 102 | void LikelihoodFieldMap::drawFreePoses(int num, std::vector & result) 103 | { 104 | std::random_device seed_gen; 105 | std::mt19937 engine{seed_gen()}; 106 | std::vector> chosen_cells; 107 | 108 | sample(free_cells_.begin(), free_cells_.end(), back_inserter(chosen_cells), num, engine); 109 | 110 | for (auto & c : chosen_cells) { 111 | Pose p; 112 | p.x_ = c.first * resolution_ + resolution_ * rand() / RAND_MAX + origin_x_; 113 | p.y_ = c.second * resolution_ + resolution_ * rand() / RAND_MAX + origin_y_; 114 | p.t_ = 2 * M_PI * rand() / RAND_MAX - M_PI; 115 | result.push_back(p); 116 | } 117 | } 118 | 119 | } // namespace emcl2 120 | -------------------------------------------------------------------------------- /src/ExpResetMcl2.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #include "emcl2/ExpResetMcl2.h" 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | namespace emcl2 14 | { 15 | ExpResetMcl2::ExpResetMcl2( 16 | const Pose & p, int num, const Scan & scan, const std::shared_ptr & odom_model, 17 | const std::shared_ptr & map, double alpha_th, 18 | double expansion_radius_position, double expansion_radius_orientation, double extraction_rate, 19 | double range_threshold, bool sensor_reset) 20 | : Mcl::Mcl(p, num, scan, odom_model, map), 21 | alpha_threshold_(alpha_th), 22 | expansion_radius_position_(expansion_radius_position), 23 | expansion_radius_orientation_(expansion_radius_orientation), 24 | extraction_rate_(extraction_rate), 25 | range_threshold_(range_threshold), 26 | sensor_reset_(sensor_reset) 27 | { 28 | } 29 | 30 | ExpResetMcl2::~ExpResetMcl2() {} 31 | 32 | void ExpResetMcl2::sensorUpdate(double lidar_x, double lidar_y, double lidar_t, bool inv) 33 | { 34 | Scan scan; 35 | scan = scan_; 36 | 37 | scan.lidar_pose_x_ = lidar_x; 38 | scan.lidar_pose_y_ = lidar_y; 39 | scan.lidar_pose_yaw_ = lidar_t; 40 | 41 | double origin = inv ? scan.angle_max_ : scan.angle_min_; 42 | int sgn = inv ? -1 : 1; 43 | for(size_t i = 0; i < scan.ranges_.size() ; i++) { 44 | scan.directions_16bit_.push_back(Pose::get16bitRepresentation( 45 | origin + sgn * i * scan.angle_increment_)); 46 | } 47 | 48 | double valid_pct = 0.0; 49 | int valid_beams = scan.countValidBeams(&valid_pct); 50 | if (valid_beams == 0) { 51 | return; 52 | } 53 | 54 | for (auto & p : particles_) { 55 | p.w_ *= p.likelihood(map_.get(), scan); 56 | } 57 | 58 | alpha_ = nonPenetrationRate(static_cast(particles_.size() * extraction_rate_), map_.get(), scan); 59 | RCLCPP_INFO(rclcpp::get_logger("emcl2_node"), "ALPHA: %f / %f", alpha_, alpha_threshold_); 60 | if (alpha_ < alpha_threshold_) { 61 | RCLCPP_INFO(rclcpp::get_logger("emcl2_node"), "RESET"); 62 | expansionReset(); 63 | for (auto & p : particles_) { 64 | p.w_ *= p.likelihood(map_.get(), scan); 65 | } 66 | } 67 | 68 | if (normalizeBelief() > 0.000001) { 69 | resampling(); 70 | } else { 71 | resetWeight(); 72 | } 73 | 74 | processed_seq_ = scan_.seq_; 75 | } 76 | 77 | double ExpResetMcl2::nonPenetrationRate(int skip, LikelihoodFieldMap * map, Scan & scan) 78 | { 79 | static uint16_t shift = 0; 80 | int counter = 0; 81 | int penetrating = 0; 82 | for (size_t i = shift % skip; i < particles_.size(); i += skip) { 83 | counter++; 84 | if (particles_[i].wallConflict(map, scan, range_threshold_, sensor_reset_)) { 85 | penetrating++; 86 | } 87 | } 88 | shift++; 89 | 90 | std::cout << penetrating << " " << counter << std::endl; 91 | return static_cast((counter - penetrating)) / counter; 92 | } 93 | 94 | void ExpResetMcl2::expansionReset(void) 95 | { 96 | for (auto & p : particles_) { 97 | double length = 98 | 2 * (static_cast(rand()) / RAND_MAX - 0.5) * expansion_radius_position_; 99 | double direction = 2 * (static_cast(rand()) / RAND_MAX - 0.5) * M_PI; 100 | 101 | p.p_.x_ += length * cos(direction); 102 | p.p_.y_ += length * sin(direction); 103 | p.p_.t_ += 2 * (static_cast(rand()) / RAND_MAX - 0.5) * 104 | expansion_radius_orientation_; 105 | p.w_ = 1.0 / particles_.size(); 106 | } 107 | } 108 | 109 | } // namespace emcl2 110 | -------------------------------------------------------------------------------- /include/emcl2/emcl2_node.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | // CAUTION: Some lines came from amcl (LGPL). 4 | 5 | #ifndef EMCL2__EMCL2_NODE_H_ 6 | #define EMCL2__EMCL2_NODE_H_ 7 | 8 | #include "emcl2/ExpResetMcl2.h" 9 | #include "emcl2/LikelihoodFieldMap.h" 10 | #include "emcl2/OdomModel.h" 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace emcl2 30 | { 31 | class EMcl2Node : public rclcpp::Node 32 | { 33 | public: 34 | EMcl2Node(); 35 | ~EMcl2Node(); 36 | 37 | void loop(void); 38 | int getOdomFreq(void); 39 | 40 | private: 41 | std::shared_ptr pf_; 42 | 43 | rclcpp::Publisher::SharedPtr particlecloud_pub_; 44 | rclcpp::Publisher::SharedPtr pose_pub_; 45 | rclcpp::Publisher::SharedPtr alpha_pub_; 46 | rclcpp::Subscription::SharedPtr laser_scan_sub_; 47 | rclcpp::Subscription::SharedPtr 48 | initial_pose_sub_; 49 | rclcpp::Subscription::SharedPtr map_sub_; 50 | 51 | // ros::ServiceServer global_loc_srv_; 52 | rclcpp::Service::SharedPtr global_loc_srv_; 53 | rclcpp::Time scan_time_stamp_; 54 | 55 | std::string footprint_frame_id_; 56 | std::string global_frame_id_; 57 | std::string odom_frame_id_; 58 | std::string scan_frame_id_; 59 | std::string base_frame_id_; 60 | 61 | std::shared_ptr tfb_; 62 | std::shared_ptr tfl_; 63 | std::shared_ptr tf_; 64 | 65 | tf2::Transform latest_tf_; 66 | 67 | rclcpp::Clock ros_clock_; 68 | 69 | int odom_freq_; 70 | bool init_pf_; 71 | bool init_request_; 72 | bool initialpose_receive_; 73 | bool simple_reset_request_; 74 | bool scan_receive_; 75 | bool map_receive_; 76 | double init_x_, init_y_, init_t_; 77 | double transform_tolerance_; 78 | 79 | void publishPose( 80 | double x, double y, double t, double x_dev, double y_dev, double t_dev, double xy_cov, 81 | double yt_cov, double tx_cov); 82 | void publishOdomFrame(double x, double y, double t); 83 | void publishParticles(void); 84 | bool getOdomPose(double & x, double & y, double & yaw); // same name is found in amcl 85 | bool getLidarPose(double & x, double & y, double & yaw, bool & inv); 86 | void receiveMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); 87 | 88 | void declareParameter(); 89 | 90 | void initCommunication(void); 91 | void initTF(); 92 | void initPF(void); 93 | std::shared_ptr initMap(void); 94 | std::shared_ptr initOdometry(void); 95 | 96 | nav_msgs::msg::OccupancyGrid map_; 97 | 98 | void cbScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg); 99 | // bool cbSimpleReset(std_srvs::Empty::Request & req, std_srvs::Empty::Response & res); 100 | bool cbSimpleReset( 101 | const std_srvs::srv::Empty::Request::ConstSharedPtr, 102 | std_srvs::srv::Empty::Response::SharedPtr); 103 | void initialPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr 104 | msg); // same name is found in amcl 105 | }; 106 | 107 | } // namespace emcl2 108 | 109 | #endif // EMCL2__EMCL2_NODE_H_ 110 | -------------------------------------------------------------------------------- /src/Particle.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | 4 | #include "emcl2/Particle.h" 5 | 6 | #include "emcl2/Mcl.h" 7 | 8 | #include 9 | 10 | namespace emcl2 11 | { 12 | Particle::Particle(double x, double y, double t, double w) : p_(x, y, t) { w_ = w; } 13 | 14 | double Particle::likelihood(LikelihoodFieldMap * map, Scan & scan) 15 | { 16 | uint16_t t = p_.get16bitRepresentation(); 17 | double lidar_x = 18 | p_.x_ + scan.lidar_pose_x_ * Mcl::cos_[t] - scan.lidar_pose_y_ * Mcl::sin_[t]; 19 | double lidar_y = 20 | p_.y_ + scan.lidar_pose_x_ * Mcl::sin_[t] + scan.lidar_pose_y_ * Mcl::cos_[t]; 21 | uint16_t lidar_yaw = Pose::get16bitRepresentation(scan.lidar_pose_yaw_); 22 | 23 | double ans = 0.0; 24 | for (size_t i = 0; i < scan.ranges_.size(); i += scan.scan_increment_) { 25 | if (!scan.valid(scan.ranges_[i])) { 26 | continue; 27 | } 28 | uint16_t a = scan.directions_16bit_[i] + t + lidar_yaw; 29 | double lx = lidar_x + scan.ranges_[i] * Mcl::cos_[a]; 30 | double ly = lidar_y + scan.ranges_[i] * Mcl::sin_[a]; 31 | 32 | ans += map->likelihood(lx, ly); 33 | } 34 | return ans; 35 | } 36 | 37 | bool Particle::wallConflict(LikelihoodFieldMap * map, Scan & scan, double threshold, bool replace) 38 | { 39 | uint16_t t = p_.get16bitRepresentation(); 40 | double lidar_x = 41 | p_.x_ + scan.lidar_pose_x_ * Mcl::cos_[t] - scan.lidar_pose_y_ * Mcl::sin_[t]; 42 | double lidar_y = 43 | p_.y_ + scan.lidar_pose_x_ * Mcl::sin_[t] + scan.lidar_pose_y_ * Mcl::cos_[t]; 44 | uint16_t lidar_yaw = Pose::get16bitRepresentation(scan.lidar_pose_yaw_); 45 | 46 | std::vector order; 47 | if (rand() % 2) { 48 | for (size_t i = 0; i < scan.ranges_.size(); i += scan.scan_increment_) { 49 | order.push_back(i); 50 | } 51 | } else { 52 | for (int i = scan.ranges_.size() - 1; i >= 0; i -= scan.scan_increment_) { 53 | order.push_back(i); 54 | } 55 | } 56 | 57 | int hit_counter = 0; 58 | for (int i : order) { 59 | if (!scan.valid(scan.ranges_[i])) { 60 | continue; 61 | } 62 | 63 | double range = scan.ranges_[i]; 64 | uint16_t a = scan.directions_16bit_[i] + t + lidar_yaw; 65 | 66 | double hit_lx = 0.0, hit_ly = 0.0; 67 | double hit_lx1 = 0.0, hit_ly1 = 0.0, r1 = 0.0; 68 | uint16_t a1 = 0; 69 | if (isPenetrating(lidar_x, lidar_y, range, a, map, hit_lx, hit_ly)) { 70 | if (hit_counter == 0) { 71 | hit_lx1 = hit_lx; 72 | hit_ly1 = hit_ly; 73 | r1 = range; 74 | a1 = a; 75 | } 76 | 77 | hit_counter++; 78 | } else { 79 | hit_counter = 0; 80 | } 81 | 82 | if (hit_counter * scan.angle_increment_ >= threshold) { 83 | if (replace) { 84 | sensorReset( 85 | lidar_x, lidar_y, r1, a1, hit_lx1, hit_ly1, range, a, hit_lx, 86 | hit_ly); 87 | } 88 | return true; 89 | } 90 | } 91 | return false; 92 | } 93 | 94 | bool Particle::isPenetrating( 95 | double ox, double oy, double range, uint16_t direction, LikelihoodFieldMap * map, double & hit_lx, 96 | double & hit_ly) 97 | { 98 | bool hit = false; 99 | for (double d = map->resolution_; d < range; d += map->resolution_) { 100 | double lx = ox + d * Mcl::cos_[direction]; 101 | double ly = oy + d * Mcl::sin_[direction]; 102 | 103 | if ((!hit) && map->likelihood(lx, ly) == 255) { 104 | hit = true; 105 | hit_lx = lx; 106 | hit_ly = ly; 107 | } else if (hit && map->likelihood(lx, ly) == 0.0) { // openspace after hit 108 | return true; // penetration 109 | } 110 | } 111 | return false; 112 | } 113 | 114 | void Particle::sensorReset( 115 | double ox, double oy, double range1, uint16_t direction1, double hit_lx1, double hit_ly1, 116 | double range2, uint16_t direction2, double hit_lx2, double hit_ly2) 117 | { 118 | double p1_x = ox + range1 * Mcl::cos_[direction1]; 119 | double p1_y = oy + range1 * Mcl::sin_[direction1]; 120 | double p2_x = ox + range2 * Mcl::cos_[direction2]; 121 | double p2_y = oy + range2 * Mcl::sin_[direction2]; 122 | 123 | double cx = (hit_lx1 + hit_lx2) / 2; 124 | double cy = (hit_ly1 + hit_ly2) / 2; 125 | 126 | p_.x_ -= (p1_x + p2_x) / 2 - cx; 127 | p_.y_ -= (p1_y + p2_y) / 2 - cy; 128 | 129 | double theta_delta = 130 | atan2(p2_y - p1_y, p2_x - p1_x) - atan2(hit_ly2 - hit_ly1, hit_lx2 - hit_lx1); 131 | 132 | // double d = std::sqrt((p_.x_ - cx)*(p_.x_ - cx) + (p_.y_ - cy)*(p_.y_ - cy)); 133 | 134 | // double theta = atan2(p_.y_ - cy, p_.x_ - cx) - theta_delta; 135 | // p_.x_ = cx + d * std::cos(theta); 136 | // p_.y_ = cy + d * std::cos(theta); 137 | 138 | p_.t_ -= theta_delta; 139 | } 140 | 141 | } // namespace emcl2 142 | -------------------------------------------------------------------------------- /src/Mcl.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | #include "emcl2/Mcl.h" 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace emcl2 13 | { 14 | Mcl::Mcl( 15 | const Pose & p, int num, const Scan & scan, const std::shared_ptr & odom_model, 16 | const std::shared_ptr & map) 17 | : last_odom_(NULL), prev_odom_(NULL) 18 | { 19 | odom_model_ = move(odom_model); 20 | map_ = move(map); 21 | scan_ = scan; 22 | 23 | if (num <= 0) { 24 | RCLCPP_ERROR(rclcpp::get_logger("emcl2_node"), "NO PARTICLE"); 25 | } 26 | 27 | Particle particle(p.x_, p.y_, p.t_, 1.0 / num); 28 | for (int i = 0; i < num; i++) { 29 | particles_.push_back(particle); 30 | } 31 | 32 | processed_seq_ = -1; 33 | alpha_ = 1.0; 34 | 35 | for (int i = 0; i < (1 << 16); i++) { 36 | cos_[i] = cos(M_PI * i / (1 << 15)); 37 | sin_[i] = sin(M_PI * i / (1 << 15)); 38 | } 39 | } 40 | 41 | Mcl::~Mcl() 42 | { 43 | delete last_odom_; 44 | delete prev_odom_; 45 | } 46 | 47 | void Mcl::resampling(void) 48 | { 49 | std::vector accum; 50 | accum.push_back(particles_[0].w_); 51 | for (size_t i = 1; i < particles_.size(); i++) { 52 | accum.push_back(accum.back() + particles_[i].w_); 53 | } 54 | 55 | std::vector old(particles_); 56 | 57 | double start = static_cast(rand()) / (RAND_MAX * particles_.size()); 58 | double step = 1.0 / particles_.size(); 59 | 60 | std::vector chosen; 61 | 62 | size_t tick = 0; 63 | for (size_t i = 0; i < particles_.size(); i++) { 64 | while (accum[tick] <= start + i * step) { 65 | tick++; 66 | if (tick == particles_.size()) { 67 | RCLCPP_ERROR(rclcpp::get_logger("emcl2_node"), "RESAMPLING FAILED"); 68 | exit(1); 69 | } 70 | } 71 | chosen.push_back(tick); 72 | } 73 | 74 | for (size_t i = 0; i < particles_.size(); i++) { 75 | particles_[i] = old[chosen[i]]; 76 | } 77 | } 78 | 79 | void Mcl::sensorUpdate(double lidar_x, double lidar_y, double lidar_t, bool inv) 80 | { 81 | if (processed_seq_ == scan_.seq_) { 82 | return; 83 | } 84 | 85 | Scan scan; 86 | int seq = -1; 87 | while (seq != scan_.seq_) { // trying to copy the latest scan before next 88 | seq = scan_.seq_; 89 | scan = scan_; 90 | } 91 | 92 | scan.lidar_pose_x_ = lidar_x; 93 | scan.lidar_pose_y_ = lidar_y; 94 | scan.lidar_pose_yaw_ = lidar_t; 95 | 96 | int i = 0; 97 | if (!inv) { 98 | for ([[maybe_unused]] auto & _ : scan.ranges_) { 99 | scan.directions_16bit_.push_back(Pose::get16bitRepresentation( 100 | scan.angle_min_ + (i++) * scan.angle_increment_)); 101 | } 102 | } else { 103 | for ([[maybe_unused]] auto & _ : scan.ranges_) { 104 | scan.directions_16bit_.push_back(Pose::get16bitRepresentation( 105 | scan.angle_max_ - (i++) * scan.angle_increment_)); 106 | } 107 | } 108 | 109 | double valid_pct = 0.0; 110 | int valid_beams = scan.countValidBeams(&valid_pct); 111 | if (valid_beams == 0) { 112 | return; 113 | } 114 | 115 | for (auto & p : particles_) { 116 | p.w_ *= p.likelihood(map_.get(), scan); 117 | } 118 | 119 | if (normalizeBelief() > 0.000001) { 120 | resampling(); 121 | } else { 122 | resetWeight(); 123 | } 124 | 125 | processed_seq_ = scan_.seq_; 126 | } 127 | 128 | void Mcl::motionUpdate(double x, double y, double t) 129 | { 130 | if (last_odom_ == NULL) { 131 | last_odom_ = new Pose(x, y, t); 132 | prev_odom_ = new Pose(x, y, t); 133 | return; 134 | } else { 135 | last_odom_->set(x, y, t); 136 | } 137 | 138 | Pose d = *last_odom_ - *prev_odom_; 139 | if (d.nearlyZero()) { 140 | return; 141 | } 142 | 143 | double fw_length = sqrt(d.x_ * d.x_ + d.y_ * d.y_); 144 | double fw_direction = atan2(d.y_, d.x_) - prev_odom_->t_; 145 | 146 | odom_model_->setDev(fw_length, d.t_); 147 | 148 | for (auto & p : particles_) { 149 | p.p_.move( 150 | fw_length, fw_direction, d.t_, odom_model_->drawFwNoise(), 151 | odom_model_->drawRotNoise()); 152 | } 153 | 154 | prev_odom_->set(*last_odom_); 155 | } 156 | 157 | void Mcl::meanPose( 158 | double & x_mean, double & y_mean, double & t_mean, double & x_dev, double & y_dev, double & t_dev, 159 | double & xy_cov, double & yt_cov, double & tx_cov) 160 | { 161 | double x, y, t, t2; 162 | x = y = t = t2 = 0.0; 163 | for (const auto & p : particles_) { 164 | x += p.p_.x_; 165 | y += p.p_.y_; 166 | t += p.p_.t_; 167 | t2 += normalizeAngle(p.p_.t_ + M_PI); 168 | } 169 | 170 | x_mean = x / particles_.size(); 171 | y_mean = y / particles_.size(); 172 | t_mean = t / particles_.size(); 173 | double t2_mean = t2 / particles_.size(); 174 | 175 | double xx, yy, tt, tt2; 176 | xx = yy = tt = tt2 = 0.0; 177 | for (const auto & p : particles_) { 178 | xx += pow(p.p_.x_ - x_mean, 2); 179 | yy += pow(p.p_.y_ - y_mean, 2); 180 | tt += pow(p.p_.t_ - t_mean, 2); 181 | tt2 += pow(normalizeAngle(p.p_.t_ + M_PI) - t2_mean, 2); 182 | } 183 | 184 | if (tt > tt2) { 185 | tt = tt2; 186 | t_mean = normalizeAngle(t2_mean - M_PI); 187 | } 188 | 189 | x_dev = xx / (particles_.size() - 1); 190 | y_dev = yy / (particles_.size() - 1); 191 | t_dev = tt / (particles_.size() - 1); 192 | 193 | double xy, yt, tx; 194 | xy = yt = tx = 0.0; 195 | for (const auto & p : particles_) { 196 | xy += (p.p_.x_ - x_mean) * (p.p_.y_ - y_mean); 197 | yt += (p.p_.y_ - y_mean) * (normalizeAngle(p.p_.t_ - t_mean)); 198 | tx += (p.p_.x_ - x_mean) * (normalizeAngle(p.p_.t_ - t_mean)); 199 | } 200 | 201 | xy_cov = xy / (particles_.size() - 1); 202 | yt_cov = yt / (particles_.size() - 1); 203 | tx_cov = tx / (particles_.size() - 1); 204 | } 205 | 206 | double Mcl::normalizeAngle(double t) 207 | { 208 | while (t > M_PI) { 209 | t -= 2 * M_PI; 210 | } 211 | while (t < -M_PI) { 212 | t += 2 * M_PI; 213 | } 214 | 215 | return t; 216 | } 217 | 218 | void Mcl::setScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg) 219 | { 220 | if (msg->ranges.size() != scan_.ranges_.size()) { 221 | scan_.ranges_.resize(msg->ranges.size()); 222 | } 223 | 224 | scan_.seq_ = msg->header.stamp.sec; 225 | for (size_t i = 0; i < msg->ranges.size(); i++) { 226 | scan_.ranges_[i] = msg->ranges[i]; 227 | } 228 | 229 | scan_.angle_min_ = msg->angle_min; 230 | scan_.angle_max_ = msg->angle_max; 231 | scan_.angle_increment_ = msg->angle_increment; 232 | scan_.range_min_ = msg->range_min; 233 | scan_.range_max_ = msg->range_max; 234 | } 235 | 236 | double Mcl::normalizeBelief(void) 237 | { 238 | double sum = 0.0; 239 | for (const auto & p : particles_) { 240 | sum += p.w_; 241 | } 242 | 243 | if (sum < 0.000000000001) { 244 | return sum; 245 | } 246 | 247 | for (auto & p : particles_) { 248 | p.w_ /= sum; 249 | } 250 | 251 | return sum; 252 | } 253 | 254 | void Mcl::resetWeight(void) 255 | { 256 | for (auto & p : particles_) { 257 | p.w_ = 1.0 / particles_.size(); 258 | } 259 | } 260 | 261 | void Mcl::initialize(double x, double y, double t) 262 | { 263 | Pose new_pose(x, y, t); 264 | for (auto & p : particles_) { 265 | p.p_ = new_pose; 266 | } 267 | 268 | resetWeight(); 269 | } 270 | 271 | void Mcl::simpleReset(void) 272 | { 273 | std::vector poses; 274 | map_->drawFreePoses(particles_.size(), poses); 275 | 276 | for (size_t i = 0; i < poses.size(); i++) { 277 | particles_[i].p_ = poses[i]; 278 | particles_[i].w_ = 1.0 / particles_.size(); 279 | } 280 | } 281 | 282 | double Mcl::cos_[(1 << 16)]; 283 | double Mcl::sin_[(1 << 16)]; 284 | 285 | } // namespace emcl2 286 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # emcl2_ros2: mcl with expansion resetting (version 2) 2 | 3 | [![test](https://github.com/CIT-Autonomous-Robot-Lab/emcl2_ros2/actions/workflows/test.yml/badge.svg)](https://github.com/CIT-Autonomous-Robot-Lab/emcl2_ros2/actions/workflows/test.yml) 4 | 5 | Emcl is an alternative Monte Carlo localization (MCL) package to amcl (http://wiki.ros.org/amcl). Differently from amcl, KLD-sampling and adaptive MCL are not implemented. Instead, the expansion resetting and other features are implemented[^1][^2]. 6 | 7 | This package is ROS 2 version of [ryuichiueda/emcl2](https://github.com/ryuichiueda/emcl2). 8 | 9 | ## ROS 2 version 10 | 11 | * ROS 2 Humble Hawksbill 12 | 13 | ## quick start 14 | 15 | ### Install & Build 16 | ``` 17 | mkdir ros2_ws && cd ros2_ws 18 | git clone git@github.com:CIT-Autonomous-Robot-Lab/emcl2_ros2.git ./src/emcl2_ros2 19 | rosdep update 20 | rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 21 | sudo apt install -y ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-turtlebot3-gazebo 22 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release 23 | source install/setup.bash 24 | ``` 25 | 26 | ### Try emcl2 in simulator 27 | 28 | You may wait Gazebo to be initilalized. You can also use `./test/demo.bash`, in which the following procedure is written. 29 | 30 | ``` 31 | export TURTLEBOT3_MODEL=burger 32 | ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py 33 | ros2 launch nav2_bringup rviz_launch.py 34 | ros2 launch emcl2 emcl2.launch.py params_file:=$(ros2 pkg prefix --share emcl2)/config/emcl2_quick_start.param.yaml map:=$(ros2 pkg prefix --share nav2_bringup)/maps/turtlebot3_world.yaml use_sim_time:=true 35 | ros2 launch nav2_bringup navigation_launch.py use_sim_time:=true 36 | ``` 37 | 38 | ## demo movies 39 | 40 | [![](https://img.youtube.com/vi/dqS7KgGxwBs/0.jpg)](https://www.youtube.com/watch?v=dqS7KgGxwBs) 41 | 42 | [![](https://img.youtube.com/vi/n9tzKY6ua_o/0.jpg)](https://www.youtube.com/watch?v=n9tzKY6ua_o) 43 | 44 | ## Nodes 45 | 46 | ### emcl2_node 47 | 48 | This node calculates the alpha value with a different algorithm than `emcl_node` in [ryuichiueda/emcl](https://github.com/ryuichiueda/emcl). This node counts the particles that make lasers penetrate occupancy cells. Specifically, this node chooses some particles at a rate of `extraction_rate` and checks each of them with the following procedure: 49 | 50 | * maps a set of laser scan on the occupancy grid map based on the pose of the particle 51 | * judges the pose of the particle as wrong if all of lasers in a `range_threshold`[rad] range penatrate occupancy grids 52 | 53 | If the rate of the wrong particles is greater than `alpha_threshold`, the node invokes a reset. 54 | 55 | This node also has a sensor resetting algorithm. When `sensor_reset` is true, a particle with laser penetration is dragged back from occupied cells. 56 | 57 | #### Subscribed Topics 58 | 59 | | Name | Type | Description | 60 | | :---------- | :-------------------------------------- | :-------------------------------- | 61 | | `map` | [`nav_msgs/OccupancyGrid`](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/OccupancyGrid.html) | occupancy grid map | 62 | | `scan` | [`sensor_msgs/LaserScan`](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/LaserScan.html) | laser scans | 63 | | `tf` | [`tf/tfMessage`](http://docs.ros.org/en/noetic/api/tf/html/msg/tfMessage.html) | transforms | 64 | | `initialpose` | [`geometry_msgs/PoseWithCovarianceStamped`](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html) | pose of particles for replacement | 65 | 66 | #### Published Topics 67 | 68 | | Name | Type | Description | 69 | | ------------- | --------------------------------------- | --------------------------------------------------------------------------------------- | 70 | | `mcl_pose` | [`geometry_msgs/PoseWithCovarianceStamped`](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html) | the mean pose of the particles with covariance | 71 | | `particlecloud` | [`geometry_msgs/PoseArray`](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseArray.html) | poses of the particles | 72 | | `tf` | [`tf/tfMessage`](http://docs.ros.org/en/noetic/api/tf/html/msg/tfMessage.html) | the transform from odom (which can be remapped via the odom_frame_id parameter) to map | 73 | | `alpha` | [`std_msgs/Float32`](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html) | marginal likelihood of particles after sensor update | 74 | 75 | #### Parameters 76 | 77 | | Name | Type | Default | Description | 78 | |-------------------------------|---------|------------|--------------------------------------------------------------------| 79 | | `odom_freq` | `int` | 20 [Hz] | frequency of odometry update | 80 | | `num_particles` | `int` | 500 | number of particles | 81 | | `odom_frame_id` | `string` | "odom" | the frame for odometry | 82 | | `footprint_frame_id` | `string` | "base_footprint" | the frame of the localized robot's base | 83 | | `base_frame_id` | `string` | "base_link" | the frame of the robot's base. It is used for calculating the position and orientation of the LiDAR | 84 | | `global_frame_id` | `string` | "map" | the frame for localization | 85 | | `initial_pose_x` | `double` | 0.0 [m] | initial x coordinate of particles | 86 | | `initial_pose_y` | `double` | 0.0 [m] | initial y coordinate of particles | 87 | | `initial_pose_a` | `double` | 0.0 [rad] | initial yaw coordinate of particles | 88 | | `odom_fw_dev_per_fw` | `double` | 0.19 [m/m] | standard deviation of forward motion noise by forward motion | 89 | | `odom_fw_dev_per_rot` | `double` | 0.0001 [m/rad] | standard deviation of forward motion noise by rotational motion | 90 | | `odom_rot_dev_per_fw` | `double` | 0.13 [rad/m] | standard deviation of rotational motion noise by forward motion | 91 | | `odom_rot_dev_per_rot` | `double` | 0.2 [rad/rad] | standard deviation of rotational motion noise by rotational motion | 92 | | `laser_likelihood_max_dist` | `double` | 0.2 meters | maximum distance to inflate occupied cells on the likelihood field map | 93 | | `scan_increment` | `int` | 1 | increment number when beams are picked from their sequence | 94 | | `alpha_threshold` | `double` | 0.5 | threshold of the alpha value for expansion resetting | 95 | | `open_space_threshold` | `double` | 0.05 | threshold of the valid beam rate for expansion resetting | 96 | | `expansion_radius_position` | `double` | 0.1 [m] | maximum change of the position on the xy-plane when the reset replaces a particle | 97 | | `expansion_radius_orientation` | `double` | 0.2 [rad] | maximum change of the yaw angle when the reset replaces a particle | 98 | | `extraction_rate` | `double` | 0.1 | rate of particles that are checked by the node | 99 | | `range_threshold` | `double` | 0.3 [rad] | threshold of the range of lasers | 100 | | `sensor_reset` | `bool` | true | flag for sensor resettings | 101 | 102 | 103 | 104 | The followings have never been implemented yet. 105 | 106 | | Name | Type | Default | Description | 107 | |-------------------|--------|------------------|-----------------------------------------------------------| 108 | | `laser_min_range` | `double` | 0.0 [m] | threshold for discarding scans whose ranges are smaller than this value | 109 | | `laser_max_range` | `double` | 100000000.0 [m] | threshold for discarding scans whose ranges are larger than this value | 110 | 111 | ## To do 112 | 113 | * Implement service 114 | * Fix a bug that caused particles to survive 115 | 116 | ## citation 117 | 118 | [^1]: R. Ueda: "[Syokai Kakuritsu Robotics (lecture note on probabilistic robotics)](https://www.amazon.co.jp/dp/B082SN3VTD)," Kodansya, 2019. 119 | 120 | [^2]: R. Ueda, T. Arai, K. Sakamoto, T. Kikuchi, S. Kamiya: Expansion resetting for recovery from fatal error in Monte Carlo localization - comparison with sensor resetting methods, IEEE/RSJ IROS, pp.2481-2486, 2004. 121 | -------------------------------------------------------------------------------- /src/emcl2_node.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2022 Ryuichi Ueda ryuichiueda@gmail.com 2 | // SPDX-License-Identifier: LGPL-3.0-or-later 3 | // CAUTION: Some lines came from amcl (LGPL). 4 | 5 | #include "emcl2/emcl2_node.h" 6 | 7 | #include "emcl2/LikelihoodFieldMap.h" 8 | #include "emcl2/OdomModel.h" 9 | #include "emcl2/Pose.h" 10 | #include "emcl2/Scan.h" 11 | 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace emcl2 31 | { 32 | EMcl2Node::EMcl2Node() 33 | : Node("emcl2_node"), 34 | ros_clock_(RCL_SYSTEM_TIME), 35 | init_pf_(false), 36 | init_request_(false), 37 | simple_reset_request_(false), 38 | scan_receive_(false), 39 | map_receive_(false) 40 | { 41 | // declare ros parameters 42 | declareParameter(); 43 | initCommunication(); 44 | } 45 | 46 | EMcl2Node::~EMcl2Node() {} 47 | 48 | void EMcl2Node::declareParameter() 49 | { 50 | this->declare_parameter("global_frame_id", std::string("map")); 51 | this->declare_parameter("footprint_frame_id", std::string("base_footprint")); 52 | this->declare_parameter("odom_frame_id", std::string("odom")); 53 | this->declare_parameter("base_frame_id", std::string("base_link")); 54 | 55 | this->declare_parameter("odom_freq", 20); 56 | this->declare_parameter("transform_tolerance", 0.2); 57 | 58 | this->declare_parameter("laser_min_range", 0.0); 59 | this->declare_parameter("laser_max_range", 100000000.0); 60 | this->declare_parameter("scan_increment", 1); 61 | 62 | this->declare_parameter("initial_pose_x", 0.0); 63 | this->declare_parameter("initial_pose_y", 0.0); 64 | this->declare_parameter("initial_pose_a", 0.0); 65 | 66 | this->declare_parameter("num_particles", 500); 67 | this->declare_parameter("alpha_threshold", 0.5); 68 | this->declare_parameter("expansion_radius_position", 0.1); 69 | this->declare_parameter("expansion_radius_orientation", 0.2); 70 | this->declare_parameter("extraction_rate", 0.1); 71 | this->declare_parameter("range_threshold", 0.1); 72 | this->declare_parameter("sensor_reset", false); 73 | 74 | this->declare_parameter("odom_fw_dev_per_fw", 0.19); 75 | this->declare_parameter("odom_fw_dev_per_rot", 0.0001); 76 | this->declare_parameter("odom_rot_dev_per_fw", 0.13); 77 | this->declare_parameter("odom_rot_dev_per_rot", 0.2); 78 | 79 | this->declare_parameter("laser_likelihood_max_dist", 0.2); 80 | } 81 | 82 | void EMcl2Node::initCommunication(void) 83 | { 84 | particlecloud_pub_ = create_publisher("particlecloud", 2); 85 | pose_pub_ = create_publisher("mcl_pose", 2); 86 | alpha_pub_ = create_publisher("alpha", 2); 87 | 88 | laser_scan_sub_ = create_subscription( 89 | "scan", 2, std::bind(&EMcl2Node::cbScan, this, std::placeholders::_1)); 90 | initial_pose_sub_ = create_subscription( 91 | "initialpose", 2, 92 | std::bind(&EMcl2Node::initialPoseReceived, this, std::placeholders::_1)); 93 | map_sub_ = create_subscription( 94 | "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), 95 | std::bind(&EMcl2Node::receiveMap, this, std::placeholders::_1)); 96 | 97 | global_loc_srv_ = create_service( 98 | "global_localization", 99 | std::bind(&EMcl2Node::cbSimpleReset, this, std::placeholders::_1, std::placeholders::_2)); 100 | 101 | this->get_parameter("global_frame_id", global_frame_id_); 102 | this->get_parameter("footprint_frame_id", footprint_frame_id_); 103 | this->get_parameter("odom_frame_id", odom_frame_id_); 104 | this->get_parameter("base_frame_id", base_frame_id_); 105 | 106 | this->get_parameter("odom_freq", odom_freq_); 107 | 108 | this->get_parameter("transform_tolerance", transform_tolerance_); 109 | } 110 | 111 | void EMcl2Node::initTF(void) 112 | { 113 | tfb_.reset(); 114 | tfl_.reset(); 115 | tf_.reset(); 116 | 117 | tf_ = std::make_shared(get_clock()); 118 | auto timer_interface = std::make_shared( 119 | get_node_base_interface(), get_node_timers_interface(), 120 | create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false)); 121 | tf_->setCreateTimerInterface(timer_interface); 122 | tfl_ = std::make_shared(*tf_); 123 | tfb_ = std::make_shared(shared_from_this()); 124 | latest_tf_ = tf2::Transform::getIdentity(); 125 | } 126 | 127 | void EMcl2Node::initPF(void) 128 | { 129 | std::shared_ptr map = std::move(initMap()); 130 | std::shared_ptr om = std::move(initOdometry()); 131 | 132 | Scan scan; 133 | this->get_parameter("laser_min_range", scan.range_min_); 134 | this->get_parameter("laser_max_range", scan.range_max_); 135 | this->get_parameter("scan_increment", scan.scan_increment_); 136 | 137 | Pose init_pose; 138 | this->get_parameter("initial_pose_x", init_pose.x_); 139 | this->get_parameter("initial_pose_y", init_pose.y_); 140 | this->get_parameter("initial_pose_a", init_pose.t_); 141 | 142 | int num_particles; 143 | double alpha_th; 144 | double ex_rad_pos, ex_rad_ori; 145 | this->get_parameter("num_particles", num_particles); 146 | this->get_parameter("alpha_threshold", alpha_th); 147 | this->get_parameter("expansion_radius_position", ex_rad_pos); 148 | this->get_parameter("expansion_radius_orientation", ex_rad_ori); 149 | 150 | double extraction_rate, range_threshold; 151 | bool sensor_reset = false; 152 | this->get_parameter("extraction_rate", extraction_rate); 153 | this->get_parameter("range_threshold", range_threshold); 154 | this->get_parameter("sensor_reset", sensor_reset); 155 | 156 | pf_.reset(new ExpResetMcl2( 157 | init_pose, num_particles, scan, om, map, alpha_th, ex_rad_pos, ex_rad_ori, 158 | extraction_rate, range_threshold, sensor_reset)); 159 | 160 | init_pf_ = true; 161 | } 162 | 163 | std::shared_ptr EMcl2Node::initOdometry(void) 164 | { 165 | double ff, fr, rf, rr; 166 | this->get_parameter("odom_fw_dev_per_fw", ff); 167 | this->get_parameter("odom_fw_dev_per_rot", fr); 168 | this->get_parameter("odom_rot_dev_per_fw", rf); 169 | this->get_parameter("odom_rot_dev_per_rot", rr); 170 | return std::shared_ptr(new OdomModel(ff, fr, rf, rr)); 171 | } 172 | 173 | std::shared_ptr EMcl2Node::initMap(void) 174 | { 175 | double likelihood_range; 176 | this->get_parameter("laser_likelihood_max_dist", likelihood_range); 177 | 178 | return std::shared_ptr(new LikelihoodFieldMap(map_, likelihood_range)); 179 | } 180 | 181 | void EMcl2Node::receiveMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) 182 | { 183 | map_ = *msg; 184 | map_receive_ = true; 185 | RCLCPP_INFO(get_logger(), "Received map."); 186 | initPF(); 187 | initTF(); 188 | } 189 | 190 | void EMcl2Node::cbScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg) 191 | { 192 | if (init_pf_) { 193 | scan_receive_ = true; 194 | scan_time_stamp_ = msg->header.stamp; 195 | scan_frame_id_ = msg->header.frame_id; 196 | pf_->setScan(msg); 197 | } 198 | } 199 | 200 | void EMcl2Node::initialPoseReceived( 201 | const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) 202 | { 203 | RCLCPP_INFO(get_logger(), "Run receiveInitialPose"); 204 | if (!initialpose_receive_) { 205 | if (scan_receive_ && map_receive_) { 206 | init_x_ = msg->pose.pose.position.x; 207 | init_y_ = msg->pose.pose.position.y; 208 | init_t_ = tf2::getYaw(msg->pose.pose.orientation); 209 | pf_->initialize(init_x_, init_y_, init_t_); 210 | initialpose_receive_ = true; 211 | } else { 212 | if (!scan_receive_) { 213 | RCLCPP_WARN( 214 | get_logger(), 215 | "Not yet received scan. Therefore, MCL cannot be initiated."); 216 | } 217 | if (!map_receive_) { 218 | RCLCPP_WARN( 219 | get_logger(), 220 | "Not yet received map. Therefore, MCL cannot be initiated."); 221 | } 222 | } 223 | } else { 224 | init_request_ = true; 225 | init_x_ = msg->pose.pose.position.x; 226 | init_y_ = msg->pose.pose.position.y; 227 | init_t_ = tf2::getYaw(msg->pose.pose.orientation); 228 | } 229 | } 230 | 231 | void EMcl2Node::loop(void) 232 | { 233 | if (init_request_) { 234 | pf_->initialize(init_x_, init_y_, init_t_); 235 | init_request_ = false; 236 | } else if (simple_reset_request_) { 237 | pf_->simpleReset(); 238 | simple_reset_request_ = false; 239 | } 240 | 241 | if (init_pf_) { 242 | double x, y, t; 243 | if (!getOdomPose(x, y, t)) { 244 | RCLCPP_INFO(get_logger(), "can't get odometry info"); 245 | return; 246 | } 247 | pf_->motionUpdate(x, y, t); 248 | 249 | double lx, ly, lt; 250 | bool inv; 251 | if (!getLidarPose(lx, ly, lt, inv)) { 252 | RCLCPP_INFO(get_logger(), "can't get lidar pose info"); 253 | return; 254 | } 255 | 256 | pf_->sensorUpdate(lx, ly, lt, inv); 257 | 258 | double x_var, y_var, t_var, xy_cov, yt_cov, tx_cov; 259 | pf_->meanPose(x, y, t, x_var, y_var, t_var, xy_cov, yt_cov, tx_cov); 260 | 261 | publishOdomFrame(x, y, t); 262 | publishPose(x, y, t, x_var, y_var, t_var, xy_cov, yt_cov, tx_cov); 263 | publishParticles(); 264 | 265 | std_msgs::msg::Float32 alpha_msg; 266 | alpha_msg.data = static_cast(pf_->alpha_); 267 | alpha_pub_->publish(alpha_msg); 268 | } else { 269 | if (!scan_receive_) { 270 | RCLCPP_WARN( 271 | get_logger(), 272 | "Not yet received scan. Therefore, MCL cannot be initiated."); 273 | } 274 | if (!map_receive_) { 275 | RCLCPP_WARN( 276 | get_logger(), 277 | "Not yet received map. Therefore, MCL cannot be initiated."); 278 | } 279 | } 280 | } 281 | 282 | void EMcl2Node::publishPose( 283 | double x, double y, double t, double x_dev, double y_dev, double t_dev, double xy_cov, 284 | double yt_cov, double tx_cov) 285 | { 286 | geometry_msgs::msg::PoseWithCovarianceStamped p; 287 | p.header.frame_id = global_frame_id_; 288 | p.header.stamp = ros_clock_.now(); 289 | p.pose.pose.position.x = x; 290 | p.pose.pose.position.y = y; 291 | p.pose.covariance[6 * 0 + 0] = x_dev; 292 | p.pose.covariance[6 * 1 + 1] = y_dev; 293 | p.pose.covariance[6 * 2 + 2] = t_dev; 294 | p.pose.covariance[6 * 0 + 1] = xy_cov; 295 | p.pose.covariance[6 * 1 + 0] = xy_cov; 296 | p.pose.covariance[6 * 0 + 2] = tx_cov; 297 | p.pose.covariance[6 * 2 + 0] = tx_cov; 298 | p.pose.covariance[6 * 1 + 2] = yt_cov; 299 | p.pose.covariance[6 * 2 + 1] = yt_cov; 300 | 301 | tf2::Quaternion q; 302 | q.setRPY(0, 0, t); 303 | tf2::convert(q, p.pose.pose.orientation); 304 | 305 | pose_pub_->publish(p); 306 | } 307 | 308 | void EMcl2Node::publishOdomFrame(double x, double y, double t) 309 | { 310 | geometry_msgs::msg::PoseStamped odom_to_map; 311 | try { 312 | tf2::Quaternion q; 313 | q.setRPY(0, 0, t); 314 | tf2::Transform tmp_tf(q, tf2::Vector3(x, y, 0.0)); 315 | 316 | geometry_msgs::msg::PoseStamped tmp_tf_stamped; 317 | tmp_tf_stamped.header.frame_id = footprint_frame_id_; 318 | tmp_tf_stamped.header.stamp = scan_time_stamp_; 319 | tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose); 320 | 321 | tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_); 322 | } catch (tf2::TransformException & e) { 323 | RCLCPP_DEBUG(get_logger(), "Failed to subtract base to odom transform"); 324 | return; 325 | } 326 | tf2::convert(odom_to_map.pose, latest_tf_); 327 | auto stamp = tf2_ros::fromMsg(scan_time_stamp_); 328 | tf2::TimePoint transform_expiration = stamp + tf2::durationFromSec(transform_tolerance_); 329 | 330 | geometry_msgs::msg::TransformStamped tmp_tf_stamped; 331 | tmp_tf_stamped.header.frame_id = global_frame_id_; 332 | tmp_tf_stamped.header.stamp = tf2_ros::toMsg(transform_expiration); 333 | tmp_tf_stamped.child_frame_id = odom_frame_id_; 334 | tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform); 335 | 336 | tfb_->sendTransform(tmp_tf_stamped); 337 | } 338 | 339 | void EMcl2Node::publishParticles(void) 340 | { 341 | geometry_msgs::msg::PoseArray cloud_msg; 342 | cloud_msg.header.stamp = ros_clock_.now(); 343 | cloud_msg.header.frame_id = global_frame_id_; 344 | cloud_msg.poses.resize(pf_->particles_.size()); 345 | 346 | for (size_t i = 0; i < pf_->particles_.size(); i++) { 347 | cloud_msg.poses[i].position.x = pf_->particles_[i].p_.x_; 348 | cloud_msg.poses[i].position.y = pf_->particles_[i].p_.y_; 349 | cloud_msg.poses[i].position.z = 0; 350 | 351 | tf2::Quaternion q; 352 | q.setRPY(0, 0, pf_->particles_[i].p_.t_); 353 | tf2::convert(q, cloud_msg.poses[i].orientation); 354 | } 355 | particlecloud_pub_->publish(cloud_msg); 356 | } 357 | 358 | bool EMcl2Node::getOdomPose(double & x, double & y, double & yaw) 359 | { 360 | geometry_msgs::msg::PoseStamped ident; 361 | ident.header.frame_id = footprint_frame_id_; 362 | ident.header.stamp = rclcpp::Time(0); 363 | tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); 364 | 365 | geometry_msgs::msg::PoseStamped odom_pose; 366 | try { 367 | this->tf_->transform(ident, odom_pose, odom_frame_id_); 368 | } catch (tf2::TransformException & e) { 369 | RCLCPP_WARN( 370 | get_logger(), "Failed to compute odom pose, skipping scan (%s)", e.what()); 371 | return false; 372 | } 373 | x = odom_pose.pose.position.x; 374 | y = odom_pose.pose.position.y; 375 | yaw = tf2::getYaw(odom_pose.pose.orientation); 376 | 377 | return true; 378 | } 379 | 380 | bool EMcl2Node::getLidarPose(double & x, double & y, double & yaw, bool & inv) 381 | { 382 | geometry_msgs::msg::PoseStamped ident; 383 | ident.header.frame_id = scan_frame_id_; 384 | ident.header.stamp = ros_clock_.now(); 385 | tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); 386 | 387 | geometry_msgs::msg::PoseStamped lidar_pose; 388 | try { 389 | this->tf_->transform(ident, lidar_pose, base_frame_id_); 390 | } catch (tf2::TransformException & e) { 391 | RCLCPP_WARN( 392 | get_logger(), "Failed to compute lidar pose, skipping scan (%s)", e.what()); 393 | return false; 394 | } 395 | 396 | x = lidar_pose.pose.position.x; 397 | y = lidar_pose.pose.position.y; 398 | 399 | double roll, pitch; 400 | tf2::getEulerYPR(lidar_pose.pose.orientation, yaw, pitch, roll); 401 | inv = (fabs(pitch) > M_PI / 2 || fabs(roll) > M_PI / 2) ? true : false; 402 | 403 | return true; 404 | } 405 | 406 | int EMcl2Node::getOdomFreq(void) { return odom_freq_; } 407 | 408 | bool EMcl2Node::cbSimpleReset( 409 | const std_srvs::srv::Empty::Request::ConstSharedPtr, std_srvs::srv::Empty::Response::SharedPtr) 410 | { 411 | return simple_reset_request_ = true; 412 | } 413 | 414 | } // namespace emcl2 415 | 416 | int main(int argc, char ** argv) 417 | { 418 | rclcpp::init(argc, argv); 419 | auto node = std::make_shared(); 420 | rclcpp::Rate loop_rate(node->getOdomFreq()); 421 | while (rclcpp::ok()) { 422 | node->loop(); 423 | rclcpp::spin_some(node); 424 | loop_rate.sleep(); 425 | } 426 | rclcpp::shutdown(); 427 | return 0; 428 | } 429 | --------------------------------------------------------------------------------