├── README.md
├── src
    ├── main.cpp
    └── neo_localization_node.cpp
├── LICENSE
├── package.xml
├── .github
    └── workflows
    │   └── main.yml
├── CMakeLists.txt
├── launch
    ├── test_setup.yaml
    └── test_setup.launch.py
└── include
    └── neo_localization2
        ├── utils
            ├── Convert.hpp
            ├── Util.hpp
            └── Matrix.hpp
        ├── solver
            └── Solver.hpp
        ├── neo_localization_node.hpp
        └── map
            └── GridMap.hpp


/README.md:
--------------------------------------------------------------------------------
1 | # neo_localization2
2 | 
3 | Please visit [our documentation](https://neobotix-docs.de/ros/packages/neo_localization.html) to know more about the setup and the usage of this package.
4 | 
5 | 


--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
 1 | #include <memory>
 2 | #include "neo_localization2/neo_localization_node.hpp"
 3 | #include "rclcpp/rclcpp.hpp"
 4 | 
 5 | int main(int argc, char ** argv)
 6 | {
 7 |   rclcpp::init(argc, argv);
 8 |   auto node = std::make_shared<neo_localization2::NeoLocalizationNode>();
 9 |   rclcpp::spin(node->get_node_base_interface());
10 |   rclcpp::shutdown();
11 |   return 0;
12 | }
13 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2020 neobotix gmbh
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |     <name>neo_localization2</name>
 5 |     <version>1.0.3</version>
 6 |     <description>
 7 | 	Platform localization node based on laser sensors and a grid map.
 8 |     </description>
 9 |     <author>Max Wittal</author>
10 |     <maintainer email="ros@neobotix.de">Neobotix GmbH</maintainer>
11 |     <maintainer email="padmanabhan@neobotix.de">Pradheep Padmanabhan</maintainer>
12 |     <license>MIT</license>
13 |     <url>https://github.com/neobotix/neo_localization2</url>
14 | 
15 |     <buildtool_depend>ament_cmake</buildtool_depend>
16 |     <build_depend>nav2_common</build_depend>
17 | 
18 |     <depend>nav2_util</depend>
19 |     <depend>rclcpp</depend>
20 |     <depend>rclcpp_lifecycle</depend>
21 |     <depend>rclcpp_components</depend>
22 |     <depend>tf2</depend>
23 |     <depend>tf2_eigen</depend>
24 |     <depend>tf2_ros</depend>
25 |     <depend>tf2_sensor_msgs</depend>
26 |     <depend>tf2_geometry_msgs</depend>
27 |     <depend>geometry_msgs</depend>
28 |     <depend>nav_msgs</depend>
29 |     <depend>sensor_msgs</depend>
30 |     <depend>std_srvs</depend>
31 |     <depend>angles</depend>
32 |     <depend>nav2_ros_common</depend>
33 | 
34 |     <export>
35 |         <build_type>ament_cmake</build_type>
36 |     </export>
37 | 
38 | </package>
39 | 


--------------------------------------------------------------------------------
/.github/workflows/main.yml:
--------------------------------------------------------------------------------
 1 | # This is a basic workflow to help you get started with Actions
 2 | 
 3 | name: neobotix-CI
 4 | 
 5 | # Controls when the workflow will run
 6 | on:
 7 |   # Triggers the workflow on push or pull request events but only for the "rolling" branch
 8 |   push:
 9 |     branches: [ "rolling" ]
10 |   pull_request:
11 |     branches: [ "rolling" ]
12 |   schedule: #trigger for every friday at 5:30 UTC
13 |     - cron: '30 5 * * 5'
14 | 
15 |   # Allows you to run this workflow manually from the Actions tab
16 |   workflow_dispatch:
17 | 
18 | # A workflow run is made up of one or more jobs that can run sequentially or in parallel
19 | jobs:
20 |   # This workflow contains a single job called "build"
21 |   build:
22 |     # The type of runner that the job will run on
23 |     runs-on: ubuntu-latest
24 |     container:
25 |       image: ubuntu:noble
26 | 
27 |     # Steps represent a sequence of tasks that will be executed as part of the job
28 |     steps:
29 |       - uses: actions/checkout@v2
30 |       - uses: ros-tooling/setup-ros@0.7.7
31 |         with:
32 |           use-ros2-testing: false
33 |       - uses: ros-tooling/action-ros-ci@0.3.13
34 |         with:
35 |           target-ros2-distro: rolling
36 |           package-name: neo_localization2
37 |           skip-tests: true
38 |           colcon-defaults: |
39 |             {
40 |               "build": {
41 |                 "symlink-install": true
42 |               }
43 |             }
44 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 3.5)
  2 | project(neo_localization2)
  3 | 
  4 | find_package(ament_cmake REQUIRED)
  5 | find_package(nav_msgs REQUIRED)
  6 | find_package(nav2_common REQUIRED)
  7 | find_package(nav2_util REQUIRED)
  8 | find_package(rclcpp REQUIRED)
  9 | find_package(rclcpp_components REQUIRED)
 10 | find_package(rclcpp_lifecycle REQUIRED)
 11 | find_package(geometry_msgs REQUIRED)
 12 | find_package(tf2 REQUIRED)
 13 | find_package(tf2_eigen REQUIRED)
 14 | find_package(tf2_ros REQUIRED)
 15 | find_package(tf2_sensor_msgs REQUIRED)
 16 | find_package(tf2_geometry_msgs REQUIRED)
 17 | find_package(ament_cmake REQUIRED)
 18 | find_package(sensor_msgs REQUIRED)
 19 | find_package(angles REQUIRED)
 20 | find_package(nav2_ros_common REQUIRED)
 21 | 
 22 | nav2_package()
 23 | 
 24 | set(executable_name neo_localization)
 25 | set(library_name ${executable_name}_core)
 26 | 
 27 | # Main library
 28 | add_library(${library_name} SHARED
 29 |   src/neo_localization_node.cpp
 30 | )
 31 | 
 32 | target_include_directories(${library_name}
 33 |   PUBLIC
 34 |   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
 35 |   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 36 | )
 37 | 
 38 | target_link_libraries(${library_name} PUBLIC
 39 |   ${geometry_msgs_TARGETS}
 40 |   nav2_util::nav2_util_core
 41 |   nav2_ros_common::nav2_ros_common
 42 |   angles::angles
 43 |   ${sensor_msgs_TARGETS}
 44 |   ${std_srvs_TARGETS}
 45 |   rclcpp::rclcpp
 46 |   rclcpp_lifecycle::rclcpp_lifecycle
 47 |   ${nav_msgs_TARGETS}
 48 |   tf2_ros::tf2_ros
 49 |   tf2::tf2
 50 |   ${nav2_msgs_TARGETS}
 51 | )
 52 | 
 53 | target_link_libraries(${library_name} PRIVATE
 54 |   rclcpp_components::component
 55 | )
 56 | 
 57 | # Main executable
 58 | add_executable(${executable_name}
 59 |   src/main.cpp
 60 | )
 61 | 
 62 | target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp)
 63 | 
 64 | rclcpp_components_register_nodes(${library_name} "neo_localization2::NeoLocalizationNode")
 65 | 
 66 | install(
 67 |   TARGETS ${library_name} 
 68 |   ARCHIVE DESTINATION lib
 69 |   LIBRARY DESTINATION lib
 70 |   RUNTIME DESTINATION bin
 71 | )
 72 | 
 73 | install(TARGETS ${executable_name}
 74 |   RUNTIME DESTINATION lib/${PROJECT_NAME}
 75 | )
 76 | 
 77 | install(DIRECTORY include/
 78 |   DESTINATION include/${PROJECT_NAME}
 79 | )
 80 | 
 81 | if(BUILD_TESTING)
 82 |   find_package(ament_lint_auto REQUIRED)
 83 |   set(ament_cmake_copyright_FOUND TRUE)
 84 |   set(ament_cmake_cpplint_FOUND TRUE)
 85 |   ament_lint_auto_find_test_dependencies()
 86 | endif()
 87 | 
 88 | ament_export_include_directories("include/${PROJECT_NAME}")
 89 | ament_export_libraries(${library_name})
 90 | ament_export_dependencies(
 91 |   geometry_msgs
 92 |   nav_msgs
 93 |   nav2_msgs
 94 |   nav2_util
 95 |   rclcpp
 96 |   rclcpp_lifecycle
 97 |   sensor_msgs
 98 |   std_srvs
 99 |   tf2
100 |   tf2_ros
101 |   tf2_sensor_msgs
102 |   tf2_geometry_msgs
103 |   tf2_eigen
104 |   angles
105 |   nav2_ros_common
106 | )
107 | 
108 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
109 | 
110 | ament_package()
111 | 


--------------------------------------------------------------------------------
/launch/test_setup.yaml:
--------------------------------------------------------------------------------
 1 | neo_localization2_node:
 2 |   ros__parameters:
 3 |     base_frame: "base_link"
 4 |     odom_frame: "odom"
 5 |     # exponential low pass gain for localization update (0 to 1)
 6 |     #   (higher gain means odometry is less used / relied on)
 7 |     update_gain: 0.5
 8 | 
 9 |     # time based confidence gain when in 2D / 1D mode
10 |     confidence_gain: 0.01
11 | 
12 |     # how many particles (samples) to spread (per update)
13 |     sample_rate: 10
14 | 
15 |     # localization update rate [1/s]
16 |     loc_update_rate: 10
17 | 
18 |     # map tile update rate [1/s]
19 |     map_update_rate: 0.5
20 | 
21 |     # map tile size in pixels
22 |     map_size: 1000
23 | 
24 |     # how often to downscale (half) the original map
25 |     map_downscale: 0
26 | 
27 |     # how many 3x3 gaussian smoothing iterations are applied to the map
28 |     num_smooth: 5
29 | 
30 |     # minimum score for valid localization (otherwise 0D mode)
31 |     #    higher values make it go into 0D mode earlier
32 |     min_score: 0.2
33 | 
34 |     # odometry error in x and y [m/m] [1]
35 |     #    how fast to increase particle spread when in 1D / 0D mode
36 |     odometry_std_xy: 0.01
37 | 
38 |     # odometry error in yaw angle [rad/rad] [1]
39 |     #    how fast to increase particle spread when in 0D mode
40 |     odometry_std_yaw: 0.01
41 | 
42 |     # minimum particle spread in x and y [m]
43 |     min_sample_std_xy: 0.025
44 | 
45 |     # minimum particle spread in yaw angle [rad]
46 |     min_sample_std_yaw: 0.025
47 | 
48 |     # initial/maximum particle spread in x and y [m]
49 |     max_sample_std_xy: 0.5
50 | 
51 |     # initial/maximum particle spread in yaw angle [rad]
52 |     max_sample_std_yaw: 0.5
53 | 
54 |     # threshold for 1D / 2D position decision making (minimum average second order gradient)
55 |     #   if worst gradient direction is below this value we go into 1D mode
56 |     #   if both gradient directions are below we may go into 0D mode, depending on disable_threshold
57 |     #   higher values will make it go into 1D / 0D mode earlier
58 |     constrain_threshold: 0.1
59 | 
60 |     # threshold for 1D / 2D decision making (with or without orientation)
61 |     #   higher values will make it go into 1D mode earlier
62 |     constrain_threshold_yaw: 0.2
63 | 
64 |     # minimum number of points per update
65 |     min_points: 20
66 | 
67 |     # solver update gain, lower gain = more stability / slower convergence
68 |     solver_gain: 0.1
69 | 
70 |     # solver update damping, higher damping = more stability / slower convergence
71 |     solver_damping: 1000.0
72 | 
73 |     # number of gauss-newton iterations per sample per scan
74 |     solver_iterations: 20
75 | 
76 |     # maximum wait for getting transforms [s]
77 |     transform_timeout: 0.2
78 | 
79 |     # if to broadcast map frame
80 |     broadcast_tf: true
81 |     # Scan topic
82 |     scan_topic: scan
83 |     # Initial Pose topic
84 |     initialpose: initialpose
85 |     # Map Tile topic
86 |     map_tile: map_tile
87 |     # Map Pose topic
88 |     map_pose: map_pose
89 |     # particle_cloud topic
90 |     particle_cloud: particlecloud
91 |     # amcl_pose topic
92 |     amcl_pose: amcl_pose
93 |     # for debugging
94 |     broadcast_info: false
95 | 
96 |     set_initial_pose: true
97 |     initia_pose.x: 0.0
98 |     initia_pose.y: 0.0
99 |     initia_pose.yaw: 0.0


--------------------------------------------------------------------------------
/include/neo_localization2/utils/Convert.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | MIT License
  3 | 
  4 | Copyright (c) 2020 neobotix gmbh
  5 | 
  6 | Permission is hereby granted, free of charge, to any person obtaining a copy
  7 | of this software and associated documentation files (the "Software"), to deal
  8 | in the Software without restriction, including without limitation the rights
  9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 10 | copies of the Software, and to permit persons to whom the Software is
 11 | furnished to do so, subject to the following conditions:
 12 | 
 13 | The above copyright notice and this permission notice shall be included in all
 14 | copies or substantial portions of the Software.
 15 | 
 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 22 | SOFTWARE.
 23 |  */
 24 | 
 25 | #ifndef NEO_LOCALIZATION2__UTILS__CONVERT_HPP_
 26 | #define NEO_LOCALIZATION2__UTILS__CONVERT_HPP_
 27 | 
 28 | #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> 
 29 | #include <tf2/transform_datatypes.hpp>
 30 | #include <tf2/LinearMath/Quaternion.hpp>
 31 | #include <tf2/utils.hpp>
 32 | #include "rclcpp/rclcpp.hpp"
 33 | #include <tf2/LinearMath/Transform.hpp>
 34 | #include <nav_msgs/msg/occupancy_grid.hpp>
 35 | #include "neo_localization2/utils/Matrix.hpp"
 36 | #include "neo_localization2/map/GridMap.hpp"
 37 | 
 38 | namespace neo_localization2
 39 | {
 40 | 
 41 | /*
 42 |  * Converts ROS 3D Transform to a 2.5D matrix.
 43 |  */
 44 | inline
 45 | Matrix<double, 4, 4> convert_transform_25(const tf2::Transform& trans)
 46 | {
 47 |   Matrix<double, 4, 4> res;
 48 |   res(0, 0) = trans.getBasis()[0][0];
 49 |   res(1, 0) = trans.getBasis()[1][0];
 50 |   res(0, 1) = trans.getBasis()[0][1];
 51 |   res(1, 1) = trans.getBasis()[1][1];
 52 |   res(0, 3) = trans.getOrigin()[0];
 53 |   res(1, 3) = trans.getOrigin()[1];
 54 |   res(2, 3) = tf2::getYaw(trans.getRotation());
 55 |   res(2, 2) = 1;
 56 |   res(3, 3) = 1;
 57 |   return res;
 58 | }
 59 | 
 60 | /*
 61 |  * Converts ROS 3D Transform to a 3D matrix.
 62 |  */
 63 | inline
 64 | Matrix<double, 4, 4> convert_transform_3(const tf2::Transform& trans)
 65 | {
 66 |   Matrix<double, 4, 4> res;
 67 |   for(int j = 0; j < 3; ++j) {
 68 |     for(int i = 0; i < 3; ++i) {
 69 |       res(i, j) = trans.getBasis()[i][j];
 70 |     }
 71 |   }
 72 |   res(0, 3) = trans.getOrigin()[0];
 73 |   res(1, 3) = trans.getOrigin()[1];
 74 |   res(2, 3) = trans.getOrigin()[2];
 75 |   res(3, 3) = 1;
 76 |   return res;
 77 | }
 78 | 
 79 | /*
 80 |  * Converts a grid map to a ROS occupancy map.
 81 |  */
 82 | inline
 83 | std::shared_ptr<nav_msgs::msg::OccupancyGrid> convert_to_ros( std::shared_ptr<GridMap<float>> map,
 84 |                         Matrix<double, 3, 1> origin,
 85 |                         rclcpp::Time m_tCurrentTimeStamp)
 86 | {
 87 |   auto grid = std::make_shared<nav_msgs::msg::OccupancyGrid>();
 88 |   tf2::Quaternion q;
 89 |   grid->header.stamp = m_tCurrentTimeStamp;
 90 |   grid->info.resolution = map->scale();
 91 |   grid->info.width = map->size_x();
 92 |   grid->info.height = map->size_y();
 93 |   grid->info.origin.position.x = origin[0];
 94 |   grid->info.origin.position.y = origin[1];
 95 |   q.setRPY(0, 0, origin[2]);
 96 |   grid->info.origin.orientation = tf2::toMsg(q);
 97 |   grid->data.resize(map->num_cells());
 98 |   for(int y = 0; y < map->size_y(); ++y) {
 99 |     for(int x = 0; x < map->size_x(); ++x) {
100 |       grid->data[y * map->size_x() + x] = (*map)(x, y) * 100.f;
101 |     }
102 |   }
103 |   return grid;
104 | }
105 | 
106 | /*
107 |  * Converts a grid map to a binary (-1, 0 or 100) ROS occupancy map.
108 |  */
109 | inline
110 | std::shared_ptr<nav_msgs::msg::OccupancyGrid> convert_to_ros_binary(  std::shared_ptr<GridMap<float>> map,
111 |                           Matrix<double, 3, 1> origin,
112 |                           float threshold,
113 |                           rclcpp::Time m_tCurrentTimeStamp)
114 | {
115 |   static const float coeff_55[5][5] = {
116 |       {0.00118231, 0.01357, 0.0276652, 0.01357, 0.00118231},
117 |       {0.01357, 0.06814, -0, 0.06814, 0.01357},
118 |       {0.0276652, -0, -0.50349, -0, 0.0276652},
119 |       {0.01357, 0.06814, -0, 0.06814, 0.01357},
120 |       {0.00118231, 0.01357, 0.0276652, 0.01357, 0.00118231},
121 |   };
122 | 
123 |   auto grid = std::make_shared<nav_msgs::msg::OccupancyGrid>();
124 |   grid->header.stamp = m_tCurrentTimeStamp;
125 |   grid->info.resolution = map->scale();
126 |   grid->info.width = map->size_x();
127 |   grid->info.height = map->size_y();
128 |   grid->info.origin.position.x = origin[0];
129 |   grid->info.origin.position.y = origin[1];
130 |   tf2::Quaternion q;
131 |   q.setRPY(0, 0, origin[2]);
132 |   grid->info.origin.orientation = tf2::toMsg(q);
133 |   grid->data.resize(map->num_cells());
134 |   for(int y = 0; y < map->size_y(); ++y) {
135 |     for(int x = 0; x < map->size_x(); ++x) {
136 |       // compute LoG filter
137 |       float sum = 0;
138 |       for(int j = -2; j <= 2; ++j) {
139 |         const int y_ = std::min(std::max(y + j, 0), map->size_y() - 1);
140 |         for(int i = -2; i <= 2; ++i) {
141 |           const int x_ = std::min(std::max(x + i, 0), map->size_x() - 1);
142 |           sum += coeff_55[j+2][i+2] * (*map)(x_, y_);
143 |         }
144 |       }
145 |       if(-1 * sum > threshold) {
146 |         grid->data[y * map->size_x() + x] = 100;
147 |       } else if((*map)(x, y) < 0) {
148 |         grid->data[y * map->size_x() + x] = -1;
149 |       } else {
150 |         grid->data[y * map->size_x() + x] = 0;
151 |       }
152 |     }
153 |   }
154 |   return grid;
155 | }
156 | 
157 | }
158 | 
159 | #endif //NEO_LOCALIZATION2__UTILS__CONVERT_HPP_


--------------------------------------------------------------------------------
/launch/test_setup.launch.py:
--------------------------------------------------------------------------------
  1 | import os
  2 | 
  3 | from ament_index_python.packages import get_package_share_directory
  4 | from launch import LaunchDescription
  5 | from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
  6 | from launch.conditions import IfCondition
  7 | from launch.substitutions import (LaunchConfiguration,
  8 |                                   PythonExpression)
  9 | from launch_ros.actions import LoadComposableNodes, Node, PushROSNamespace, SetParameter
 10 | from launch_ros.descriptions import ComposableNode, ParameterFile
 11 | from nav2_common.launch import LaunchConfigAsBool, RewrittenYaml
 12 | 
 13 | 
 14 | def generate_launch_description() -> LaunchDescription:
 15 |     # Get the launch directory
 16 |     bringup_dir = get_package_share_directory('neo_localization2')
 17 | 
 18 |     namespace = LaunchConfiguration('namespace')
 19 |     use_sim_time = LaunchConfigAsBool('use_sim_time')
 20 |     autostart = LaunchConfigAsBool('autostart')
 21 |     params_file = LaunchConfiguration('params_file')
 22 |     use_composition = LaunchConfigAsBool('use_composition')
 23 |     container_name = LaunchConfiguration('container_name')
 24 |     container_name_full = (namespace, '/', container_name)
 25 |     use_respawn = LaunchConfigAsBool('use_respawn')
 26 |     log_level = LaunchConfiguration('log_level')
 27 | 
 28 |     lifecycle_nodes = ['neo_localization2_node']
 29 | 
 30 |     # Map fully qualified names to relative ones so the node's namespace can be prepended.
 31 |     remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
 32 | 
 33 |     configured_params = ParameterFile(
 34 |         RewrittenYaml(
 35 |             source_file=params_file,
 36 |             root_key=namespace,
 37 |             param_rewrites={},
 38 |             convert_types=True,
 39 |         ),
 40 |         allow_substs=True,
 41 |     )
 42 | 
 43 |     stdout_linebuf_envvar = SetEnvironmentVariable(
 44 |         'RCUTILS_LOGGING_BUFFERED_STREAM', '1'
 45 |     )
 46 | 
 47 |     declare_namespace_cmd = DeclareLaunchArgument(
 48 |         'namespace', default_value='', description='Top-level namespace'
 49 |     )
 50 | 
 51 |     declare_use_sim_time_cmd = DeclareLaunchArgument(
 52 |         'use_sim_time',
 53 |         default_value='false',
 54 |         description='Use simulation (Gazebo) clock if true',
 55 |     )
 56 | 
 57 |     declare_params_file_cmd = DeclareLaunchArgument(
 58 |         'params_file',
 59 |         default_value=os.path.join(bringup_dir, 'launch', 'test_setup.yaml'),
 60 |         description='Full path to the ROS2 parameters file to use for launch neo_localization2 node',
 61 |     )
 62 | 
 63 |     declare_autostart_cmd = DeclareLaunchArgument(
 64 |         'autostart',
 65 |         default_value='true',
 66 |         description='Automatically startup the nav2 stack',
 67 |     )
 68 | 
 69 |     declare_use_composition_cmd = DeclareLaunchArgument(
 70 |         'use_composition',
 71 |         default_value='False',
 72 |         description='Use composed bringup if True',
 73 |     )
 74 | 
 75 |     declare_container_name_cmd = DeclareLaunchArgument(
 76 |         'container_name',
 77 |         default_value='nav2_container',
 78 |         description='the name of container that nodes will load in if use composition',
 79 |     )
 80 | 
 81 |     declare_use_respawn_cmd = DeclareLaunchArgument(
 82 |         'use_respawn',
 83 |         default_value='False',
 84 |         description='Whether to respawn if a node crashes. Applied when composition is disabled.',
 85 |     )
 86 | 
 87 |     declare_log_level_cmd = DeclareLaunchArgument(
 88 |         'log_level', default_value='info', description='log level'
 89 |     )
 90 | 
 91 |     load_nodes = GroupAction(
 92 |         condition=IfCondition(PythonExpression(['not ', use_composition])),
 93 |         actions=[
 94 |             PushROSNamespace(namespace),
 95 |             SetParameter('use_sim_time', use_sim_time),
 96 |             Node(
 97 |                 package='neo_localization2',
 98 |                 executable='neo_localization',
 99 |                 name='neo_localization2_node',
100 |                 output='screen',
101 |                 respawn=use_respawn,
102 |                 respawn_delay=2.0,
103 |                 parameters=[configured_params],
104 |                 arguments=['--ros-args', '--log-level', log_level],
105 |                 remappings=remappings,
106 |             ),
107 |             Node(
108 |                 package='nav2_lifecycle_manager',
109 |                 executable='lifecycle_manager',
110 |                 name='lifecycle_manager_localization',
111 |                 output='screen',
112 |                 arguments=['--ros-args', '--log-level', log_level],
113 |                 parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}],
114 |             ),
115 |         ],
116 |     )
117 | 
118 |     load_composable_nodes = GroupAction(
119 |         condition=IfCondition(use_composition),
120 |         actions=[
121 |             LoadComposableNodes(
122 |                 target_container=container_name_full,
123 |                 composable_node_descriptions=[
124 |                     ComposableNode(
125 |                         package='neo_localization2',
126 |                         plugin='neo_localization2::NeoLocalizationNode',
127 |                         name='neo_localization2_node',
128 |                         parameters=[configured_params],
129 |                         remappings=remappings,
130 |                     ),
131 |                     ComposableNode(
132 |                         package='nav2_lifecycle_manager',
133 |                         plugin='nav2_lifecycle_manager::LifecycleManager',
134 |                         name='lifecycle_manager_localization',
135 |                         parameters=[
136 |                             {'autostart': autostart, 'node_names': lifecycle_nodes}
137 |                         ],
138 |                     ),
139 |                 ],
140 |             ),
141 |         ],
142 |     )
143 | 
144 |     # Create the launch description and populate
145 |     ld = LaunchDescription()
146 | 
147 |     # Set environment variables
148 |     ld.add_action(stdout_linebuf_envvar)
149 | 
150 |     # Declare the launch options
151 |     ld.add_action(declare_namespace_cmd)
152 |     ld.add_action(declare_use_sim_time_cmd)
153 |     ld.add_action(declare_params_file_cmd)
154 |     ld.add_action(declare_autostart_cmd)
155 |     ld.add_action(declare_use_composition_cmd)
156 |     ld.add_action(declare_container_name_cmd)
157 |     ld.add_action(declare_use_respawn_cmd)
158 |     ld.add_action(declare_log_level_cmd)
159 | 
160 |     # Add the actions to launch all of the localiztion nodes
161 |     ld.add_action(load_nodes)
162 |     ld.add_action(load_composable_nodes)
163 | 
164 |     return ld
165 | 


--------------------------------------------------------------------------------
/include/neo_localization2/utils/Util.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | MIT License
  3 | 
  4 | Copyright (c) 2020 neobotix gmbh
  5 | 
  6 | Permission is hereby granted, free of charge, to any person obtaining a copy
  7 | of this software and associated documentation files (the "Software"), to deal
  8 | in the Software without restriction, including without limitation the rights
  9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 10 | copies of the Software, and to permit persons to whom the Software is
 11 | furnished to do so, subject to the following conditions:
 12 | 
 13 | The above copyright notice and this permission notice shall be included in all
 14 | copies or substantial portions of the Software.
 15 | 
 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 22 | SOFTWARE.
 23 |  */
 24 | 
 25 | #ifndef NEO_LOCALIZATION2__UTILS__UTIL_HPP_
 26 | #define NEO_LOCALIZATION2__UTILS__UTIL_HPP_
 27 | 
 28 | #include <vector>
 29 | #include "neo_localization2/utils/Matrix.hpp"
 30 | 
 31 | namespace neo_localization2
 32 | {
 33 | 
 34 | template<typename T>
 35 | Matrix<T, 3, 3> rotate2_z(T rad) {
 36 |   return {std::cos(rad), -std::sin(rad), 0,
 37 |       std::sin(rad), std::cos(rad), 0,
 38 |       0, 0, 1};
 39 | }
 40 | 
 41 | template<typename T>
 42 | Matrix<T, 3, 3> translate2(T x, T y) {
 43 |   return {1, 0, x,
 44 |       0, 1, y,
 45 |       0, 0, 1};
 46 | }
 47 | 
 48 | /*
 49 |  * Creates a 2D (x, y, yaw) transformation matrix.
 50 |  */
 51 | template<typename T>
 52 | Matrix<T, 3, 3> transform2(T x, T y, T rad) {
 53 |   return translate2(x, y) * rotate2_z(rad);
 54 | }
 55 | 
 56 | /*
 57 |  * Creates a 2D (x, y, yaw) transformation matrix.
 58 |  */
 59 | template<typename T>
 60 | Matrix<T, 3, 3> transform2(const Matrix<T, 3, 1>& pose) {
 61 |   return translate2(pose[0], pose[1]) * rotate2_z(pose[2]);
 62 | }
 63 | 
 64 | /*
 65 |  * Creates a 2.5D (x, y, yaw) rotation matrix.
 66 |  */
 67 | template<typename T>
 68 | Matrix<T, 4, 4> rotate25_z(T rad) {
 69 |   return {std::cos(rad), -std::sin(rad), 0, 0,
 70 |       std::sin(rad), std::cos(rad), 0, 0,
 71 |       0, 0, 1, rad,
 72 |       0, 0, 0, 1};
 73 | }
 74 | 
 75 | /*
 76 |  * Creates a 3D rotation matrix for a yaw rotation around Z axis.
 77 |  */
 78 | template<typename T>
 79 | Matrix<T, 4, 4> rotate3_z(T rad) {
 80 |   return {std::cos(rad), -std::sin(rad), 0, 0,
 81 |       std::sin(rad), std::cos(rad), 0, 0,
 82 |       0, 0, 1, 0,
 83 |       0, 0, 0, 1};
 84 | }
 85 | 
 86 | /*
 87 |  * Creates a 2.5D (x, y, yaw) translation matrix.
 88 |  */
 89 | template<typename T>
 90 | Matrix<T, 4, 4> translate25(T x, T y) {
 91 |   return {1, 0, 0, x,
 92 |       0, 1, 0, y,
 93 |       0, 0, 1, 0,
 94 |       0, 0, 0, 1};
 95 | }
 96 | 
 97 | /*
 98 |  * Creates a 2.5D (x, y, yaw) transformation matrix.
 99 |  */
100 | template<typename T>
101 | Matrix<T, 4, 4> transform25(T x, T y, T rad) {
102 |   return translate25(x, y) * rotate25_z(rad);
103 | }
104 | 
105 | /*
106 |  * Creates a 2.5D (x, y, yaw) transformation matrix.
107 |  */
108 | template<typename T>
109 | Matrix<T, 4, 4> transform25(const Matrix<T, 3, 1>& pose) {
110 |   return translate25(pose[0], pose[1]) * rotate25_z(pose[2]);
111 | }
112 | 
113 | /*
114 |  * Computes 1D variance.
115 |  */
116 | template<typename T>
117 | T compute_variance(const std::vector<double>& values, T& mean)
118 | {
119 |   if(values.size() < 2) {
120 |     throw std::logic_error("values.size() < 2");
121 |   }
122 |   mean = 0;
123 |   for(auto v : values) {
124 |     mean += v;
125 |   }
126 |   mean /= T(values.size());
127 | 
128 |   double var = 0;
129 |   for(auto v : values) {
130 |     var += std::pow(v - mean, 2);
131 |   }
132 |   var /= T(values.size() - 1);
133 |   return var;
134 | }
135 | 
136 | /*
137 |  * Computes ND covariance matrix.
138 |  */
139 | template<typename T, size_t N, size_t M>
140 | Matrix<T, N, N> compute_covariance(const std::vector<Matrix<T, M, 1>>& points, Matrix<T, N, 1>& mean)
141 | {
142 |   if(M < N) {
143 |     throw std::logic_error("M < N");
144 |   }
145 |   if(points.size() < 2) {
146 |     throw std::logic_error("points.size() < 2");
147 |   }
148 |   mean = Matrix<T, N, 1>();
149 |   for(auto point : points) {
150 |     mean += point.template get<N, 1>();
151 |   }
152 |   mean /= T(points.size());
153 | 
154 |   Matrix<T, N, N> mat;
155 |   for(auto point : points) {
156 |     for(unsigned long j = 0; j < N; ++j) {
157 |       for(unsigned long i = 0; i < N; ++i) {
158 |         mat(i, j) += (point[i] - mean[i]) * (point[j] - mean[j]);
159 |       }
160 |     }
161 |   }
162 |   mat /= T(points.size() - 1);
163 |   return mat;
164 | }
165 | 
166 | /*
167 |  * Computes 1D variance along a 2D axis (given by direction unit vector), around given mean position.
168 |  */
169 | template<typename T, size_t N>
170 | T compute_variance_along_direction_2( const std::vector<Matrix<T, N, 1>>& points,
171 |                     const Matrix<T, 2, 1>& mean,
172 |                     const Matrix<T, 2, 1>& direction)
173 | {
174 |   if(N < 2) {
175 |     throw std::logic_error("N < 2");
176 |   }
177 |   if(points.size() < 2) {
178 |     throw std::logic_error("points.size() < 2");
179 |   }
180 |   double var = 0;
181 |   for(auto point : points)
182 |   {
183 |     const auto delta = Matrix<T, 2, 1>{point[0], point[1]} - mean;
184 |     var += std::pow(direction.dot(delta), 2);
185 |   }
186 |   var /= T(points.size() - 1);
187 |   return var;
188 | }
189 | 
190 | // See: http://croninprojects.org/Vince/Geodesy/FindingEigenvectors.pdf
191 | // See: http://people.math.harvard.edu/~knill/teaching/math21b2004/exhibits/2dmatrices/index.html
192 | // See: http://math.colgate.edu/~wweckesser/math312Spring06/handouts/IMM_2x2linalg.pdf
193 | // Returns eigenvalues in descending order (with matching eigenvector order)
194 | template<typename T>
195 | Matrix<T, 2, 1> compute_eigenvectors_2( const Matrix<T, 2, 2>& mat,
196 |                     std::array<Matrix<T, 2, 1>, 2>& eigen_vectors)
197 | {
198 |   Matrix<T, 2, 1> eigen_values;
199 |   const T tmp_0 = std::sqrt(std::pow(mat(0, 0) + mat(1, 1), T(2)) - T(4) * (mat(0, 0) * mat(1, 1) - mat(1, 0) * mat(0, 1)));
200 |   eigen_values[0] = (mat(0, 0) + mat(1, 1) + tmp_0) / T(2);
201 |   eigen_values[1] = (mat(0, 0) + mat(1, 1) - tmp_0) / T(2);
202 | 
203 |   if(std::abs(eigen_values[0] - eigen_values[1]) > 1e-6)
204 |   {
205 |     for(int i = 0; i < 2; ++i)
206 |     {
207 |       const Matrix<T, 2, 1> vector_a {-1 * mat(0, 1), mat(0, 0) - eigen_values[i]};
208 |       const Matrix<T, 2, 1> vector_b {mat(1, 1) - eigen_values[i], -1 * mat(1, 0)};
209 | 
210 |       if(vector_a.norm() > vector_b.norm()) {
211 |         eigen_vectors[i] = vector_a;
212 |       } else{
213 |         eigen_vectors[i] = vector_b;
214 |       }
215 |       eigen_vectors[i].normalize();
216 |     }
217 |     if(eigen_values[1] > eigen_values[0]) {
218 |       std::swap(eigen_values[0], eigen_values[1]);
219 |       std::swap(eigen_vectors[0], eigen_vectors[1]);
220 |     }
221 |   } else {
222 |     eigen_vectors[0] = Matrix<T, 2, 1>{1, 0};
223 |     eigen_vectors[1] = Matrix<T, 2, 1>{0, 1};
224 |   }
225 |   return eigen_values;
226 | }
227 | 
228 | }
229 | 
230 | #endif // NEO_LOCALIZATION2__UTILS__UTIL_HPP_ 
231 | 


--------------------------------------------------------------------------------
/include/neo_localization2/solver/Solver.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | MIT License
  3 | 
  4 | Copyright (c) 2020 neobotix gmbh
  5 | 
  6 | Permission is hereby granted, free of charge, to any person obtaining a copy
  7 | of this software and associated documentation files (the "Software"), to deal
  8 | in the Software without restriction, including without limitation the rights
  9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 10 | copies of the Software, and to permit persons to whom the Software is
 11 | furnished to do so, subject to the following conditions:
 12 | 
 13 | The above copyright notice and this permission notice shall be included in all
 14 | copies or substantial portions of the Software.
 15 | 
 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 22 | SOFTWARE.
 23 | */
 24 | 
 25 | #ifndef NEO_LOCALIZATION2__SOLVER__SOLVER_HPP_
 26 | #define NEO_LOCALIZATION2__SOLVER__SOLVER_HPP_
 27 | 
 28 | #include <vector>
 29 | #include "neo_localization2/map/GridMap.hpp"
 30 | 
 31 | namespace neo_localization2
 32 | {
 33 | 
 34 | struct scan_point_t
 35 | {
 36 |   float x = 0;    // [m]
 37 |   float y = 0;    // [m]
 38 | };
 39 | 
 40 | struct scan_point_ex_t : public scan_point_t
 41 | {
 42 |   float w = 1;    // weight [1]
 43 |   int layer = 0;    // layer index
 44 | };
 45 | 
 46 | 
 47 | class Solver {
 48 | public:
 49 |   double pose_x = 0;          // initial guess / output grid pose
 50 |   double pose_y = 0;          // initial guess / output grid pose
 51 |   double pose_yaw = 0;        // initial guess / output grid pose
 52 | 
 53 |   double gain = 0.1;          // how fast to converge (0 to 1)
 54 |   double damping = 1;         // numerical hessian damping
 55 |   double r_norm = 0;          // current error norm
 56 | 
 57 |   Matrix<double, 3, 1> G;       // gradient vector
 58 |   Matrix<double, 3, 3> H;       // Hessian matrix
 59 | 
 60 |   template<typename T>
 61 |   void solve( const GridMap<T>& grid,
 62 |         const std::vector<scan_point_t>& points)
 63 |   {
 64 |     reset();
 65 | 
 66 |     // compute transformation matrix first
 67 |     const Matrix<double, 3, 3> P = transform2(pose_x, pose_y, pose_yaw);
 68 | 
 69 |     for(const auto& point : points)
 70 |     {
 71 |       // transform sensor point to grid coordinates
 72 |       const auto q = (P * Matrix<double, 3, 1>{point.x, point.y, 1}).project();
 73 |       const float grid_x = grid.world_to_grid(q[0]);
 74 |       const float grid_y = grid.world_to_grid(q[1]);
 75 | 
 76 |       // compute error based on grid
 77 |       const float r_i = grid.bilinear_lookup(grid_x, grid_y);
 78 |       r_norm += r_i * r_i;
 79 | 
 80 |       // compute error gradient based on grid
 81 |       float dx, dy;
 82 |       grid.calc_gradient(grid_x, grid_y, dx, dy);
 83 | 
 84 |       integrate(point.x, point.y, r_i, dx, dy);
 85 |     }
 86 | 
 87 |     // we want average r_norm
 88 |     r_norm = sqrt(r_norm / points.size());
 89 | 
 90 |     update();
 91 |   }
 92 | 
 93 |   template<typename T>
 94 |   void solve( const MultiGridMap<T>& multi_grid,
 95 |         const std::vector<scan_point_ex_t>& points)
 96 |   {
 97 |     reset();
 98 | 
 99 |     // compute transformation matrix first
100 |     const Matrix<double, 3, 3> P = transform2(pose_x, pose_y, pose_yaw);
101 | 
102 |     for(const auto& point : points)
103 |     {
104 |       auto& grid = multi_grid.layers[point.layer];
105 | 
106 |       // transform sensor point to grid coordinates
107 |       const auto q = (P * Matrix<double, 3, 1>{point.x, point.y, 1}).project();
108 |       const float grid_x = grid.world_to_grid(q[0]);
109 |       const float grid_y = grid.world_to_grid(q[1]);
110 | 
111 |       // compute error based on grid
112 |       const float r_i = grid.bilinear_lookup(grid_x, grid_y);
113 |       r_norm += r_i * r_i;
114 | 
115 |       // compute error gradient based on grid
116 |       float dx, dy;
117 |       grid.calc_gradient(grid_x, grid_y, dx, dy);
118 | 
119 |       integrate(point.x, point.y, r_i, dx, dy);
120 |     }
121 | 
122 |     // we want average r_norm
123 |     r_norm = sqrt(r_norm / points.size());
124 | 
125 |     update();
126 |   }
127 | 
128 | protected:
129 |   void reset()
130 |   {
131 |     G = Matrix<double, 3, 1>();
132 |     H = Matrix<double, 3, 3>();
133 |     r_norm = 0;
134 |   }
135 | 
136 |   void integrate(const float p_x, const float p_y, const float r_i, const float dx, const float dy)
137 |   {
138 |     const float J_x = dx * 1.f;
139 |     const float J_y = dy * 1.f;
140 |     const float J_yaw =   dx * (-sinf(pose_yaw) * p_x - cosf(pose_yaw) * p_y)
141 |               + dy * ( cosf(pose_yaw) * p_x - sinf(pose_yaw) * p_y);
142 | 
143 |     // direct gradient vector summation
144 |     G[0] += J_x * r_i;
145 |     G[1] += J_y * r_i;
146 |     G[2] += J_yaw * r_i;
147 | 
148 |     // direct Hessian matrix summation
149 |     H(0, 0) += J_x * J_x;
150 |     H(1, 1) += J_y * J_y;
151 |     H(2, 2) += J_yaw * J_yaw;
152 | 
153 |     H(0, 1) += J_x * J_y;
154 |     H(1, 0) += J_x * J_y;
155 | 
156 |     H(0, 2) += J_x * J_yaw;
157 |     H(2, 0) += J_x * J_yaw;
158 | 
159 |     H(1, 2) += J_y * J_yaw;
160 |     H(2, 1) += J_y * J_yaw;
161 |   }
162 | 
163 |   void update()
164 |   {
165 |     // add Hessian damping
166 |     H(0, 0) += damping;
167 |     H(1, 1) += damping;
168 |     H(2, 2) += damping;
169 | 
170 |     // solve Gauss-Newton step
171 |     const auto X = H.inverse() * G;
172 | 
173 |     // apply new solution with a gain (optimize max. r_norm)
174 |     pose_x += gain * X[0];
175 |     pose_y += gain * X[1];
176 |     pose_yaw += gain * X[2];
177 |   }
178 | 
179 | };
180 | 
181 | 
182 | /*
183 |  * Computes a "virtual" covariance matrix based on second order gradients.
184 |  * A higher covariance means a larger gradient, so the meaning of "covariance" is inverted here.
185 |  * A higher gradient is better for localization accuracy.
186 |  */
187 | inline
188 | Matrix<double, 3, 3> compute_virtual_scan_covariance_xyw( std::shared_ptr<const GridMap<float>> grid,
189 |                               const std::vector<scan_point_t>& points,
190 |                               const Matrix<double, 3, 1>& pose)
191 | {
192 |   Matrix<double, 3, 3> var_xyw;
193 |   const Matrix<double, 3, 3> P = transform2(pose);  // pre-compute transformation matrix
194 | 
195 |   for(const auto& point : points)
196 |   {
197 |     // transform sensor point to grid coordinates
198 |     const auto q = (P * Matrix<double, 3, 1>{point.x, point.y, 1}).project();
199 |     const float grid_x = grid->world_to_grid(q[0]);
200 |     const float grid_y = grid->world_to_grid(q[1]);
201 | 
202 |     float ddx, ddy;
203 |     grid->calc_gradient2(grid_x, grid_y, ddx, ddy);
204 | 
205 |     const float dir = pose[2] + atan2f(point.y, point.x);
206 |     const float length = hypotf(point.x, point.y);
207 |     const float ddyaw = (sinf(dir) * ddx + cosf(dir) * ddy) * length;
208 | 
209 |     var_xyw(0, 0) += ddx * ddx;
210 |     var_xyw(1, 0) += ddx * ddy;
211 |     var_xyw(0, 1) += ddy * ddx;
212 |     var_xyw(1, 1) += ddy * ddy;
213 |     var_xyw(2, 2) += ddyaw * ddyaw;
214 |   }
215 |   var_xyw *= 1. / points.size();
216 |   return var_xyw;
217 | }
218 | 
219 | }
220 | 
221 | #endif
222 | 
223 | 


--------------------------------------------------------------------------------
/include/neo_localization2/neo_localization_node.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef NEO_LOCALIZATION2__NEO_LOCALIZATION_NODE_HPP_
  2 | #define NEO_LOCALIZATION2__NEO_LOCALIZATION_NODE_HPP_
  3 | 
  4 | #include "neo_localization2/utils/Util.hpp"
  5 | #include "neo_localization2/utils/Convert.hpp"
  6 | #include "neo_localization2/solver/Solver.hpp"
  7 | #include "neo_localization2/map/GridMap.hpp"
  8 | #include "nav2_ros_common/lifecycle_node.hpp"
  9 | #include "nav2_ros_common/node_utils.hpp"
 10 | #include "pluginlib/class_loader.hpp"
 11 | #include "rclcpp/node_options.hpp"
 12 | #include "angles/angles.h"
 13 | #include <tf2_ros/transform_listener.hpp>
 14 | #include <tf2_ros/transform_broadcaster.hpp>
 15 | #include <tf2/LinearMath/Transform.hpp>
 16 | #include <tf2_ros/buffer.hpp>
 17 | 
 18 | #include <nav_msgs/msg/odometry.hpp>
 19 | #include <nav_msgs/msg/occupancy_grid.hpp>
 20 | #include <sensor_msgs/msg/laser_scan.hpp>
 21 | #include <geometry_msgs/msg/quaternion.h>
 22 | #include <geometry_msgs/msg/pose_array.hpp>
 23 | #include <geometry_msgs/msg/transform_stamped.hpp>
 24 | #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 25 | #include <std_msgs/msg/bool.hpp>
 26 | #include <chrono>
 27 | #include <memory>
 28 | #include <mutex>
 29 | #include <thread>
 30 | #include <random>
 31 | #include <cmath>
 32 | #include <array>
 33 | #include <tf2_ros/create_timer_ros.hpp>
 34 | 
 35 | /*
 36 |  * Coordinate systems:
 37 |  * - Sensor in [meters, rad], aka. "laserX"
 38 |  * - Base Link in [meters, rad], aka. "base_link"
 39 |  * - Odometry in [meters, rad], aka. "odom"
 40 |  * - Map in [meters, rad], aka. "map"
 41 |  * - World Grid in [meters, rad], aka. "world"
 42 |  * - Tile Grid in [meters, rad], aka. "grid"
 43 |  * - World Grid in [pixels]
 44 |  * - Tile Grid in [pixels]
 45 |  *
 46 |  */
 47 | 
 48 | namespace neo_localization2
 49 | {
 50 | /*
 51 |  * @class NeoLocalizationNode
 52 |  * @brief ROS wrapper for NeoLocalization
 53 |  */
 54 | class NeoLocalizationNode: public nav2::LifecycleNode
 55 | {
 56 | public:
 57 | 
 58 |   /*
 59 |    * @brief NeoLocalization constructor
 60 |    * @param options Additional options to control creation of the node.
 61 |    */
 62 |   explicit NeoLocalizationNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 63 |   /*
 64 |    * @brief NeoLocalization destructor
 65 |    */
 66 |   ~NeoLocalizationNode();
 67 | 
 68 | protected:
 69 |   /*
 70 |    * @brief Lifecycle configure
 71 |    */
 72 |   nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
 73 |   /*
 74 |    * @brief Lifecycle activate
 75 |    */
 76 |   nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
 77 |   /*
 78 |    * @brief Lifecycle deactivate
 79 |    */
 80 |   nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
 81 |   /*
 82 |    * @brief Lifecycle cleanup
 83 |    */
 84 |   nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
 85 |   /*
 86 |    * @brief Lifecycle shutdown
 87 |    */
 88 |   nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 89 | 
 90 |   /*
 91 |    * @brief Initialize parameters
 92 |    */
 93 |   void initParameters(nav2::LifecycleNode::SharedPtr node);
 94 | 
 95 |   /*
 96 |    * @brief Get parameters
 97 |    */
 98 |   void getParameters(nav2::LifecycleNode::SharedPtr node);
 99 | 
100 |   /*
101 |    * @brief Initialize transforms
102 |    */
103 |   void initTransforms();
104 | 
105 |   /*
106 |    * @brief Initialize publishers & subscribers
107 |    */
108 |   void initPubSub();
109 | 
110 |   /*
111 |    * @brief Initialize robot namespace
112 |    */
113 |   void initNameSpace();
114 | 
115 |   /*
116 |    * Computes localization update for a single laser scan.
117 |    */
118 |   void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
119 | 
120 |   /*
121 |    * Whether just only use odom (without scan matching) or not in localization
122 |    */
123 |   void use_odom_callback(const std_msgs::msg::Bool::SharedPtr msg);
124 | 
125 |   /*
126 |    * Convert/Transform a scan from ROS format to a specified base frame.
127 |    */
128 |   std::vector<scan_point_t> convert_scan(const sensor_msgs::msg::LaserScan::SharedPtr scan, const Matrix<double, 4, 4>& odom_to_base);
129 | 
130 |   /*
131 |    * Update localization
132 |    */
133 |   void loc_update();
134 | 
135 |   /*
136 |    * Resets localization to given position.
137 |    */
138 |   void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose);
139 | 
140 |   /*
141 |    * Stores the given map.
142 |    */
143 |   void map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr ros_map);
144 | 
145 |   /*
146 |    * Extracts a new map tile around current position.
147 |    */
148 |   void update_map();
149 | 
150 |   /*
151 |    * Asynchronous map update loop, running in separate thread.
152 |    */
153 |   void update_loop();
154 | 
155 |   /*
156 |    * Publishes "map" frame on tf_->
157 |    */
158 |   void broadcast();
159 | 
160 | private:
161 |   std::mutex m_node_mutex;
162 | 
163 |   nav2::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr m_pub_map_tile;
164 |   nav2::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr m_pub_loc_pose;
165 |   nav2::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr m_pub_loc_pose_2;
166 |   nav2::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr m_pub_pose_array;
167 | 
168 |   nav2::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr m_sub_map_topic;
169 |   nav2::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr m_sub_scan_topic;
170 |   nav2::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr m_sub_pose_estimate;
171 |   nav2::Subscription<std_msgs::msg::Bool>::SharedPtr m_sub_only_use_odom;
172 |   std::shared_ptr<tf2_ros::TransformBroadcaster> m_tf_broadcaster;
173 | 
174 |   bool m_broadcast_tf = false;
175 |   bool m_initialized = false;
176 |   bool m_set_initial_pose = false;
177 |   std::string m_base_frame;
178 |   std::string m_odom_frame;
179 |   std::string m_map_frame;
180 |   std::string m_map_topic;
181 |   std::string m_scan_topic;
182 |   std::string m_initial_pose;
183 |   std::string m_map_tile;
184 |   std::string m_map_pose;
185 |   std::string m_particle_cloud;
186 |   std::string m_amcl_pose;
187 |   std::string m_ns = "";
188 | 
189 |   int m_map_size = 0;
190 |   int m_map_downscale = 0;
191 |   int m_num_smooth = 0;
192 |   int m_solver_iterations = 0;
193 |   int m_sample_rate = 0;
194 |   int m_min_points = 0;
195 |   double m_update_gain = 0;
196 |   double m_confidence_gain = 0;
197 |   double m_min_score = 0;
198 |   double m_odometry_std_xy = 0;     // odometry xy error in meter per meter driven
199 |   double m_odometry_std_yaw = 0;      // odometry yaw error in rad per rad rotated
200 |   double m_min_sample_std_xy = 0;
201 |   double m_min_sample_std_yaw = 0;
202 |   double m_max_sample_std_xy = 0;
203 |   double m_max_sample_std_yaw = 0;
204 |   double m_constrain_threshold = 0;
205 |   double m_constrain_threshold_yaw = 0;
206 |   int m_loc_update_time_ms = 0;
207 |   double m_map_update_rate = 0;
208 |   double m_transform_timeout = 0;
209 |   bool only_using_odom;
210 | 
211 |   builtin_interfaces::msg::Time m_offset_time;
212 |   double m_offset_x = 0;          // current x offset between odom and map
213 |   double m_offset_y = 0;          // current y offset between odom and map
214 |   double m_offset_yaw = 0;        // current yaw offset between odom and map
215 |   double m_sample_std_xy = 0;       // current sample spread in xy
216 |   double m_sample_std_yaw = 0;      // current sample spread in yaw
217 |   std::unique_ptr<tf2_ros::Buffer> buffer;
218 |   std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
219 | 
220 |   Matrix<double, 3, 1> m_last_odom_pose;
221 |   Matrix<double, 4, 4> m_grid_to_map;
222 |   Matrix<double, 4, 4> m_world_to_map;
223 |   std::shared_ptr<GridMap<float>> m_map;      // map tile
224 |   nav_msgs::msg::OccupancyGrid::SharedPtr m_world;    // whole map
225 |   bool map_received_ = false;
226 | 
227 |   int64_t update_counter = 0;
228 |   std::map<std::string, sensor_msgs::msg::LaserScan::SharedPtr> m_scan_buffer;
229 | 
230 |   Solver m_solver;
231 |   std::mt19937 m_generator;
232 |   std::thread m_map_update_thread;
233 |   bool m_broadcast_info;
234 |   rclcpp::TimerBase::SharedPtr m_loc_update_timer;
235 |   std::atomic<bool> stop_threads_{false};
236 |   std::mutex stop_mtx_;
237 |   std::condition_variable stop_cv_;
238 | 
239 |   /*
240 |    * @brief Callback executed when a parameter change is detected
241 |    * @param event ParameterEvent message
242 |    */
243 |   rcl_interfaces::msg::SetParametersResult
244 |   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
245 | 
246 |   // Dynamic parameters handler
247 |   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
248 | 
249 |   // Dedicated callback group and executor for services and subscriptions in AmclNode,
250 |   // in order to isolate TF timer used in message filter.
251 |   rclcpp::CallbackGroup::SharedPtr callback_group_;
252 |   rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
253 |   std::unique_ptr<nav2::NodeThread> executor_thread_;
254 | };
255 | 
256 | } //namespace neo_localization2
257 | 
258 | #endif  // NEO_LOCALIZATION2__NEO_LOCALIZATION_NODE_HPP_
259 | 


--------------------------------------------------------------------------------
/include/neo_localization2/map/GridMap.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | MIT License
  3 | 
  4 | Copyright (c) 2020 neobotix gmbh
  5 | 
  6 | Permission is hereby granted, free of charge, to any person obtaining a copy
  7 | of this software and associated documentation files (the "Software"), to deal
  8 | in the Software without restriction, including without limitation the rights
  9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 10 | copies of the Software, and to permit persons to whom the Software is
 11 | furnished to do so, subject to the following conditions:
 12 | 
 13 | The above copyright notice and this permission notice shall be included in all
 14 | copies or substantial portions of the Software.
 15 | 
 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 22 | SOFTWARE.
 23 |  */
 24 | 
 25 | #ifndef NEO_LOCALIZATION2__MAP__GRIDMAP_HPP_
 26 | #define NEO_LOCALIZATION2__MAP__GRIDMAP_HPP_
 27 | 
 28 | #include <math.h>
 29 | #include <stdint.h>
 30 | #include <string.h>
 31 | #include <stdexcept>
 32 | #include <memory>
 33 | #include <vector>
 34 | 
 35 | namespace neo_localization2
 36 | {
 37 | 
 38 | /*
 39 |  * Class for a rectangular grid map.
 40 |  */
 41 | template<typename T>
 42 | class GridMap {
 43 | public:
 44 |   /*
 45 |    * @param size_x Size of map in pixels
 46 |    * @param size_y Size of map in pixels
 47 |    * @param scale Size of one pixel in meters
 48 |    */
 49 |   GridMap(int size_x, int size_y, float scale)
 50 |     : m_size_x(size_x),
 51 |       m_size_y(size_y),
 52 |       m_scale(scale),
 53 |       m_inv_scale(1 / scale)
 54 |   {
 55 |     m_map = new T[size_t(size_x) * size_y];
 56 |   }
 57 | 
 58 |   /*
 59 |    * Deep copy constructor.
 60 |    */
 61 |   GridMap(const GridMap& other)
 62 |     : GridMap(other.m_size_x, other.m_size_y, other.m_scale)
 63 |   {
 64 |     *this = other;
 65 |   }
 66 | 
 67 |   ~GridMap()
 68 |   {
 69 |     delete [] m_map;
 70 |     m_map = 0;
 71 |   }
 72 | 
 73 |   /*
 74 |    * Deep assignment operator.
 75 |    */
 76 |   GridMap& operator=(const GridMap& other)
 77 |   {
 78 |     if(m_size_x != other.m_size_x || m_size_y != other.m_size_y) {
 79 |       throw std::logic_error("grid size mismatch");
 80 |     }
 81 |     m_scale = other.m_scale;
 82 |     m_inv_scale = other.m_inv_scale;
 83 |     ::memcpy(m_map, other.m_map, num_cells() * sizeof(T));
 84 |     return *this;
 85 |   }
 86 | 
 87 |   int size_x() const {
 88 |     return m_size_x;
 89 |   }
 90 | 
 91 |   int size_y() const {
 92 |     return m_size_y;
 93 |   }
 94 | 
 95 |   float scale() const {
 96 |     return m_scale;
 97 |   }
 98 | 
 99 |   float inv_scale() const {
100 |     return m_inv_scale;
101 |   }
102 | 
103 |   size_t num_cells() const {
104 |     return size_t(m_size_x) * m_size_y;
105 |   }
106 | 
107 |   /*
108 |    * Sets all pixels to given value.
109 |    */
110 |   void clear(const T& value) const {
111 |     for(size_t i = 0; i < num_cells(); ++i) {
112 |       m_map[i] = value;
113 |     }
114 |   }
115 | 
116 |   /*
117 |    * Access a given cell by index.
118 |    */
119 |   T& operator[](size_t index)
120 |   {
121 |     return m_map[index];
122 |   }
123 | 
124 |   const T& operator[](size_t index) const
125 |   {
126 |     return m_map[index];
127 |   }
128 | 
129 |   /*
130 |    * Access a given cell by coordinate.
131 |    */
132 |   T& operator()(int x, int y)
133 |   {
134 |     return m_map[size_t(y) * m_size_x + x];
135 |   }
136 | 
137 |   const T& operator()(int x, int y) const
138 |   {
139 |     return m_map[size_t(y) * m_size_x + x];
140 |   }
141 | 
142 |   /*
143 |    * Converts a coordinate in meters to a grid coordinate in pixels.
144 |    */
145 |   float world_to_grid(float meters) const {
146 |     return meters * m_inv_scale - 0.5f;
147 |   }
148 | 
149 |   /*
150 |    * Bilinear interpolation at given pixel position.
151 |    * A coordinate of (0, 0) gives the exact value of the first pixel.
152 |    */
153 |   float bilinear_lookup(float x, float y) const
154 |   {
155 |     const float a = x - floorf(x);
156 |     const float b = y - floorf(y);
157 | 
158 |     return bilinear_lookup_ex(x, y, a, b);
159 |   }
160 | 
161 |   /*
162 |    * Same as bilinear_lookup() but with pre-computed offsets a and b.
163 |    */
164 |   float bilinear_lookup_ex(int x, int y, float a, float b) const
165 |   {
166 |     const int x0 = (x > 0 ? x:0) < m_size_x - 1 ? (x > 0 ? x:0): m_size_x - 1;
167 |     const int x1 = x0 + 1 < m_size_x - 1 ? x0 + 1: m_size_x - 1;
168 |     const int y0 = (y > 0 ? y:0) < m_size_y - 1 ? (y > 0 ? y:0): m_size_y - 1;
169 |     const int y1 = y0 + 1 < m_size_y - 1 ? y0 + 1: m_size_y - 1;
170 | 
171 |     return    m_map[size_t(y0) * m_size_x + x0] * ((1.f - a) * (1.f - b))
172 |         + m_map[size_t(y0) * m_size_x + x1] * (a * (1.f - b))
173 |         + m_map[size_t(y1) * m_size_x + x0] * ((1.f - a) * b)
174 |         + m_map[size_t(y1) * m_size_x + x1] * (a * b);
175 |   }
176 | 
177 |   /*
178 |    * Bilinear summation at a given pixel position.
179 |    * A coordinate of (0, 0) will sum at the first pixel exclusively.
180 |    */
181 |   void bilinear_summation(float x, float y, const T& value)
182 |   {
183 |     const float a = x - floorf(x);
184 |     const float b = y - floorf(y);
185 | 
186 |     bilinear_summation_ex(x, y, a, b, value);
187 |   }
188 | 
189 |   /*
190 |    * Same as bilinear_summation() but with pre-computed offsets a and b.
191 |    */
192 |   void bilinear_summation_ex(int x, int y, float a, float b, const T& value)
193 |   {
194 |     const int x0 = (x > 0 ? x:0) < m_size_x - 1 ? (x > 0 ? x:0): m_size_x - 1;
195 |     const int x1 = x0 + 1 < m_size_x - 1 ? x0 + 1: m_size_x - 1;
196 |     const int y0 = (y > 0 ? y:0) < m_size_y - 1 ? (y > 0 ? y:0): m_size_y - 1;
197 |     const int y1 = y0 + 1 < m_size_y - 1 ? y0 + 1: m_size_y - 1;
198 | 
199 |     (*this)(x0, y0) += value * ((1.f - a) * (1.f - b));
200 |     (*this)(x1, y0) += value * (a * (1.f - b));
201 |     (*this)(x0, y1) += value * ((1.f - a) * b);
202 |     (*this)(x1, y1) += value * (a * b);
203 |   }
204 | 
205 |   /*
206 |    * Computes gauss-filtered and bilinear-interpolated first-order x and y gradient
207 |    * at given pixel position.
208 |    */
209 |   void calc_gradient(float x, float y, float& dx, float& dy) const
210 |   {
211 |     static const float coeff_33_dxy[3][3] = {
212 |         {-0.139505, 0, 0.139505},
213 |         {-0.220989, 0, 0.220989},
214 |         {-0.139505, 0, 0.139505}
215 |     };
216 | 
217 |     const int x0 = x;
218 |     const int y0 = y;
219 | 
220 |     const float a = x - floorf(x);
221 |     const float b = y - floorf(y);
222 | 
223 |     dx = 0;
224 |     dy = 0;
225 | 
226 |     for(int j = -1; j <= 1; ++j) {
227 |       for(int i = -1; i <= 1; ++i) {
228 |         const float value = bilinear_lookup_ex(x0 + i, y0 + j, a, b);
229 |         dx += coeff_33_dxy[j+1][i+1] * value;
230 |         dy += coeff_33_dxy[i+1][j+1] * value;
231 |       }
232 |     }
233 | 
234 |     dx /= 2 * m_scale;
235 |     dy /= 2 * m_scale;
236 |   }
237 | 
238 |   /*
239 |    * Computes gauss-filtered and bilinear-interpolated second-order x and y gradient
240 |    * at given pixel position.
241 |    */
242 |   void calc_gradient2(float x, float y, float& ddx, float& ddy) const
243 |   {
244 |     static const float coeff_33_ddxy[3][3] = {
245 |         {0.069752, -0.139504, 0.069752},
246 |         {0.110494, -0.220989, 0.110494},
247 |         {0.069752, -0.139504, 0.069752}
248 |     };
249 | 
250 |     const int x0 = x;
251 |     const int y0 = y;
252 | 
253 |     const float a = x - floorf(x);
254 |     const float b = y - floorf(y);
255 | 
256 |     ddx = 0;
257 |     ddy = 0;
258 | 
259 |     for(int j = -1; j <= 1; ++j) {
260 |       for(int i = -1; i <= 1; ++i) {
261 |         const float value = bilinear_lookup_ex(x0 + i, y0 + j, a, b);
262 |         ddx += coeff_33_ddxy[j+1][i+1] * value;
263 |         ddy += coeff_33_ddxy[i+1][j+1] * value;
264 |       }
265 |     }
266 | 
267 |     ddx /= 2 * m_scale;
268 |     ddy /= 2 * m_scale;
269 |   }
270 | 
271 |   /*
272 |    * Applies one smoothing iteration using a 3x3 gaussian kernel with sigma 1.
273 |    */
274 |   void smooth_33_1()
275 |   {
276 |     static const float coeff_33_1[3][3] = {
277 |         {0.077847, 0.123317, 0.077847},
278 |         {0.123317, 0.195346, 0.123317},
279 |         {0.077847, 0.123317, 0.077847}
280 |     };
281 | 
282 |     GridMap<T> tmp(m_size_x, m_size_y, m_scale);
283 | 
284 |     for(int y = 0; y < m_size_y; ++y) {
285 |       for(int x = 0; x < m_size_x; ++x) {
286 |         float sum = 0;
287 |         for(int j = -1; j <= 1; ++j) {
288 |           const int y_ = std::min(std::max(y + j, 0), m_size_y - 1);
289 |           for(int i = -1; i <= 1; ++i) {
290 |             const int x_ = std::min(std::max(x + i, 0), m_size_x - 1);
291 |             sum += coeff_33_1[j+1][i+1] * (*this)(x_, y_);
292 |           }
293 |         }
294 |         tmp(x, y) = sum;
295 |       }
296 |     }
297 |     *this = tmp;
298 |   }
299 | 
300 |   /*
301 |    * Applies one smoothing iteration using a 5x5 gaussian kernel with sigma 2.
302 |    */
303 |   void smooth_55_2()
304 |   {
305 |     static const float coeff_55_2[5][5] = {
306 |         {0.0232468, 0.033824, 0.0383276, 0.033824, 0.0232468},
307 |         {0.033824, 0.0492136, 0.0557663, 0.0492136, 0.033824},
308 |         {0.0383276, 0.0557663, 0.0631915, 0.0557663, 0.0383276},
309 |         {0.033824, 0.0492136, 0.0557663, 0.0492136, 0.033824},
310 |         {0.0232468, 0.033824, 0.0383276, 0.033824, 0.0232468}
311 |     };
312 | 
313 |     GridMap<T> tmp(m_size_x, m_size_y, m_scale);
314 | 
315 |     for(int y = 0; y < m_size_y; ++y) {
316 |       for(int x = 0; x < m_size_x; ++x) {
317 |         float sum = 0;
318 |         for(int j = -2; j <= 2; ++j) {
319 |           const int y_ = std::min(std::max(y + j, 0), m_size_y - 1);
320 |           for(int i = -2; i <= 2; ++i) {
321 |             const int x_ = std::min(std::max(x + i, 0), m_size_x - 1);
322 |             sum += coeff_55_2[j+2][i+2] * (*this)(x_, y_);
323 |           }
324 |         }
325 |         tmp(x, y) = sum;
326 |       }
327 |     }
328 |     *this = tmp;
329 |   }
330 | 
331 |   /*
332 |    * Returns a 2x downscaled map using a 4x4 gaussian filter with sigma 1.
333 |    */
334 |   std::shared_ptr<GridMap<T>> downscale()
335 |   {
336 |     static const float coeff_44_1[4][4] {
337 |       {0.0180824, 0.049153, 0.049153, 0.0180824},
338 |       {0.049153, 0.133612, 0.133612, 0.049153},
339 |       {0.049153, 0.133612, 0.133612, 0.049153},
340 |       {0.0180824, 0.049153, 0.049153, 0.0180824}
341 |     };
342 | 
343 |     auto res = std::make_shared<GridMap<T>>(m_size_x / 2, m_size_y / 2, m_scale * 2);
344 | 
345 |     for(int y = 0; y < m_size_y / 2; ++y) {
346 |       for(int x = 0; x < m_size_x / 2; ++x) {
347 |         float sum = 0;
348 |         for(int j = -1; j <= 2; ++j) {
349 |           const int y_ = std::min(std::max(y * 2 + j, 0), m_size_y - 1);
350 |           for(int i = -1; i <= 2; ++i) {
351 |             const int x_ = std::min(std::max(x * 2 + i, 0), m_size_x - 1);
352 |             sum += coeff_44_1[j+1][i+1] * (*this)(x_, y_);
353 |           }
354 |         }
355 |         (*res)(x, y) = sum;
356 |       }
357 |     }
358 |     return res;
359 |   }
360 | 
361 | private:
362 |   int m_size_x = 0;
363 |   int m_size_y = 0;
364 | 
365 |   float m_scale = 0;
366 |   float m_inv_scale = 0;
367 | 
368 |   T* m_map = 0;
369 | 
370 | };
371 | 
372 | 
373 | template<typename T>
374 | class MultiGridMap {
375 | public:
376 |   std::vector<std::shared_ptr<GridMap<T>>> layers;
377 | 
378 |   /*
379 |    * @param size_x_m Size of map in meters
380 |    * @param size_y_m Size of map in meters
381 |    * @param scale Size of one pixel in meters
382 |    */
383 |   MultiGridMap(float size_x_m, float size_y_m, float scale, int num_layers)
384 |   {
385 |     layers.resize(num_layers);
386 |     const int base_size_x = size_x_m / (scale * (1 << (num_layers - 1))) + 0.5f;
387 |     const int base_size_y = size_y_m / (scale * (1 << (num_layers - 1))) + 0.5f;
388 | 
389 |     for(int i = 0; i < num_layers; ++i) {
390 |       layers[i] = std::make_shared<GridMap<T>>(base_size_x * (1 << (num_layers - i - 1)),
391 |                            base_size_y * (1 << (num_layers - i - 1)),
392 |                            scale * (1 << i));
393 |     }
394 |   }
395 | 
396 | };
397 | 
398 | }
399 | 
400 | #endif //NEO_LOCALIZATION2__MAP__GRIDMAP_HPP_
401 | 
402 | 
403 | 


--------------------------------------------------------------------------------
/include/neo_localization2/utils/Matrix.hpp:
--------------------------------------------------------------------------------
  1 | /*********************************************************************
  2 |  * Software License Agreement (BSD License)
  3 |  *
  4 |  *  Copyright (c) 2016, Neobotix GmbH
  5 |  *  All rights reserved.
  6 |  *
  7 |  *  Redistribution and use in source and binary forms, with or without
  8 |  *  modification, are permitted provided that the following conditions
  9 |  *  are met:
 10 |  *
 11 |  *   * Redistributions of source code must retain the above copyright
 12 |  *     notice, this list of conditions and the following disclaimer.
 13 |  *   * Redistributions in binary form must reproduce the above
 14 |  *     copyright notice, this list of conditions and the following
 15 |  *     disclaimer in the documentation and/or other materials provided
 16 |  *     with the distribution.
 17 |  *   * Neither the name of the Neobotix nor the names of its
 18 |  *     contributors may be used to endorse or promote products derived
 19 |  *     from this software without specific prior written permission.
 20 |  *
 21 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 22 |  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 23 |  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 24 |  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 25 |  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 26 |  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 27 |  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 28 |  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 29 |  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 30 |  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 31 |  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 32 |  *  POSSIBILITY OF SUCH DAMAGE.
 33 |  *********************************************************************/
 34 | 
 35 | #ifndef INCLUDE_MATH_MATRIX_H_
 36 | #define INCLUDE_MATH_MATRIX_H_
 37 | 
 38 | #include <cmath>
 39 | #include <cstdint>
 40 | #include <ostream>
 41 | #include <array>
 42 | #include <stdexcept>
 43 | #include <initializer_list>
 44 | 
 45 | namespace neo_localization2
 46 | {
 47 | 
 48 | template<typename T>
 49 | class MatrixX;
 50 | 
 51 | struct NO_INIT {};
 52 | 
 53 | template<typename T, size_t Rows, size_t Cols>
 54 | class Matrix;
 55 | 
 56 | template<typename T, size_t Rows, size_t Cols>
 57 | Matrix<T, Rows, Cols> inverse(const Matrix<T, Rows, Cols>& mat);
 58 | 
 59 | template<typename T>
 60 | Matrix<T, 1, 1> inverse(const Matrix<T, 1, 1>& m);
 61 | 
 62 | template<typename T>
 63 | Matrix<T, 2, 2> inverse(const Matrix<T, 2, 2>& m);
 64 | 
 65 | template<typename T>
 66 | Matrix<T, 3, 3> inverse(const Matrix<T, 3, 3>& m);
 67 | 
 68 | template<typename T>
 69 | Matrix<T, 4, 4> inverse(const Matrix<T, 4, 4>& m);
 70 | 
 71 | 
 72 | template<typename T, size_t Rows, size_t Cols>
 73 | class Matrix {
 74 | public:
 75 | 	typedef T Scalar;
 76 | 
 77 | 	/*
 78 | 	 * Default constructor initializes with all zeros.
 79 | 	 */
 80 | 	Matrix() : data({}) {}
 81 | 
 82 | 	/*
 83 | 	 * Special constructor, does not initialize data.
 84 | 	 */
 85 | 	Matrix(NO_INIT) {}
 86 | 
 87 | 	/*
 88 | 	 * Generic copy constructor.
 89 | 	 */
 90 | 	template<typename S>
 91 | 	Matrix(const Matrix<S, Rows, Cols>& mat) {
 92 | 		for(size_t i = 0; i < size(); ++i) {
 93 | 			data[i] = mat.data[i];
 94 | 		}
 95 | 	}
 96 | 
 97 | 	/*
 98 | 	 * Generic copy constructor for a MatrixX.
 99 | 	 */
100 | 	template<typename S>
101 | 	Matrix(const MatrixX<S>& mat);
102 | 
103 | 	/*
104 | 	 * Initialize with column major list (ie. human readable form).
105 | 	 */
106 | 	Matrix(const std::initializer_list<T>& list) {
107 | 		if(list.size() != Rows * Cols) {
108 | 			throw std::logic_error("list.size() != Rows * Cols");
109 | 		}
110 | 		size_t i = 0;
111 | 		for(const T& v : list) {
112 | 			(*this)(i / Cols, i % Cols) = v;
113 | 			i++;
114 | 		}
115 | 	}
116 | 
117 | 	/*
118 | 	 * Returns identity matrix.
119 | 	 */
120 | 	static Matrix Identity() {
121 | 		if(Rows != Cols) {
122 | 			throw std::logic_error("Rows != Cols");
123 | 		}
124 | 		Matrix res;
125 | 		for(size_t i = 0; i < Rows; ++i) {
126 | 			res(i, i) = 1;
127 | 		}
128 | 		return res;
129 | 	}
130 | 
131 | 	T* get_data() {
132 | 		return &data[0];
133 | 	}
134 | 
135 | 	const T* get_data() const {
136 | 		return &data[0];
137 | 	}
138 | 
139 | 	T& operator()(size_t i, size_t j) {
140 | 		return data[j * Rows + i];
141 | 	}
142 | 
143 | 	const T& operator()(size_t i, size_t j) const {
144 | 		return data[j * Rows + i];
145 | 	}
146 | 
147 | 	T& operator[](size_t i) {
148 | 		return data[i];
149 | 	}
150 | 
151 | 	const T& operator[](size_t i) const {
152 | 		return data[i];
153 | 	}
154 | 
155 | 	Matrix<T, Cols, Rows> transpose() const {
156 | 		Matrix<T, Cols, Rows> res;
157 | 		for(size_t j = 0; j < Cols; ++j) {
158 | 			for(size_t i = 0; i < Rows; ++i) {
159 | 				res(j, i) = (*this)(i, j);
160 | 			}
161 | 		}
162 | 		return res;
163 | 	}
164 | 
165 | 	T squared_norm() const {
166 | 		T sum = 0;
167 | 		for(T v : data) {
168 | 			sum += v*v;
169 | 		}
170 | 		return sum;
171 | 	}
172 | 
173 | 	T norm() const {
174 | 		return std::sqrt(squared_norm());
175 | 	}
176 | 
177 | 	size_t rows() const {
178 | 		return Rows;
179 | 	}
180 | 
181 | 	size_t cols() const {
182 | 		return Cols;
183 | 	}
184 | 
185 | 	size_t size() const {
186 | 		return Rows * Cols;
187 | 	}
188 | 
189 | 	Matrix inverse() const {
190 | 		if(Rows != Cols) {
191 | 			throw std::logic_error("Rows != Cols");
192 | 		}
193 | 		return neo_localization2::inverse<T>(*this);
194 | 	}
195 | 
196 | 	template<size_t N, size_t M = 1>
197 | 	Matrix<T, N, M> get(size_t i_0 = 0, size_t j_0 = 0) const {
198 | 		Matrix<T, N, M> res;
199 | 		for(size_t j = 0; j < M; ++j) {
200 | 			for(size_t i = 0; i < N; ++i) {
201 | 				res(i, j) = (*this)(i + i_0, j + j_0);
202 | 			}
203 | 		}
204 | 		return res;
205 | 	}
206 | 
207 | 	Matrix<T, Rows+1, 1> extend() const {
208 | 		if(Cols != 1) {
209 | 			throw std::logic_error("Cols != 1");
210 | 		}
211 | 		Matrix<T, Rows+1, 1> res;
212 | 		for(size_t i = 0; i < Rows; ++i) {
213 | 			res[i] = data[i];
214 | 		}
215 | 		res[Rows] = 1;
216 | 		return res;
217 | 	}
218 | 
219 | 	Matrix<T, Rows-1, 1> project() const {
220 | 		if(Cols != 1) {
221 | 			throw std::logic_error("Cols != 1");
222 | 		}
223 | 		auto res = get<Rows-1>();
224 | 		if(data[Rows-1] != 1) {
225 | 			res *= T(1) / data[Rows-1];
226 | 		}
227 | 		return res;
228 | 	}
229 | 
230 | 	void normalize() {
231 | 		(*this) *= (T(1) / norm());
232 | 	}
233 | 
234 | 	Matrix normalized() const {
235 | 		return (*this) * (T(1) / norm());
236 | 	}
237 | 
238 | 	T dot(const Matrix& B) const {
239 | 		T res = 0;
240 | 		for(size_t i = 0; i < size(); ++i) {
241 | 			res += data[i] * B[i];
242 | 		}
243 | 		return res;
244 | 	}
245 | 
246 | 	template<size_t N>
247 | 	Matrix<T, Rows, N> operator*(const Matrix<T, Cols, N>& B) const {
248 | 		Matrix<T, Rows, N> C;
249 | 		for(size_t i = 0; i < Rows; ++i) {
250 | 			for(size_t j = 0; j < N; ++j) {
251 | 				for(size_t k = 0; k < Cols; ++k) {
252 | 					C(i, j) += (*this)(i, k) * B(k, j);
253 | 				}
254 | 			}
255 | 		}
256 | 		return C;
257 | 	}
258 | 
259 | 	template<size_t N>
260 | 	Matrix& operator*=(const Matrix<T, Cols, N>& B) {
261 | 		*this = *this * B;
262 | 		return *this;
263 | 	}
264 | 
265 | 	Matrix& operator*=(const T& value) {
266 | 		for(size_t i = 0; i < Rows * Cols; ++i) {
267 | 			(*this)[i] *= value;
268 | 		}
269 | 		return *this;
270 | 	}
271 | 
272 | 	Matrix operator*(const T& value) const {
273 | 		Matrix C = *this;
274 | 		C *= value;
275 | 		return C;
276 | 	}
277 | 
278 | 	Matrix& operator/=(const T& value) {
279 | 		for(size_t i = 0; i < Rows * Cols; ++i) {
280 | 			(*this)[i] /= value;
281 | 		}
282 | 		return *this;
283 | 	}
284 | 
285 | 	Matrix operator/(const T& value) const {
286 | 		Matrix C = *this;
287 | 		C /= value;
288 | 		return C;
289 | 	}
290 | 
291 | 	Matrix& operator+=(const Matrix& B) {
292 | 		for(size_t i = 0; i < Rows * Cols; ++i) {
293 | 			(*this)[i] += B[i];
294 | 		}
295 | 		return *this;
296 | 	}
297 | 
298 | 	Matrix operator+(const Matrix& B) const {
299 | 		Matrix C = *this;
300 | 		C += B;
301 | 		return C;
302 | 	}
303 | 
304 | 	Matrix& operator-=(const Matrix& B) {
305 | 		for(size_t i = 0; i < Rows * Cols; ++i) {
306 | 			(*this)[i] -= B[i];
307 | 		}
308 | 		return *this;
309 | 	}
310 | 
311 | 	Matrix operator-(const Matrix& B) const {
312 | 		Matrix C = *this;
313 | 		C -= B;
314 | 		return C;
315 | 	}
316 | 
317 | 	bool operator==(const Matrix& B) const {
318 | 		return data == B.data;
319 | 	}
320 | 
321 | 	bool operator!=(const Matrix& B) const {
322 | 		return data != B.data;
323 | 	}
324 | 
325 | 	std::ostream& print(std::ostream& out, const std::string& name) const {
326 | 		out << name << " = [" << std::endl;
327 | 		for(size_t i = 0; i < Rows; ++i) {
328 | 			if(i > 0) {
329 | 				out << "," << std::endl;
330 | 			}
331 | 			out << "[";
332 | 			for(size_t j = 0; j < Cols; ++j) {
333 | 				if(j > 0) {
334 | 					out << ", ";
335 | 				}
336 | 				out << (*this)(i, j);
337 | 			}
338 | 			out << "]";
339 | 		}
340 | 		out << "]" << std::endl;
341 | 		return out;
342 | 	}
343 | 
344 | 	std::array<T, Rows * Cols> data;
345 | 
346 | };
347 | 
348 | 
349 | template<typename T, size_t Rows, size_t Cols>
350 | Matrix<T, Rows, Cols> inverse(const Matrix<T, Rows, Cols>& mat) {
351 | 	throw std::logic_error("not implemented");
352 | }
353 | 
354 | template<typename T>
355 | Matrix<T, 1, 1> inverse(const Matrix<T, 1, 1>& m) {
356 | 	return m * (T(1) / m[0]);
357 | }
358 | 
359 | template<typename T>
360 | Matrix<T, 2, 2> inverse(const Matrix<T, 2, 2>& m) {
361 | 	const T det = (m(0,0) * m(1, 1)) - (m(0, 1) * m(1, 0));
362 | 	if(det == 0) {
363 | 		throw std::runtime_error("inverse(): determinant = 0");
364 | 	}
365 | 	Matrix<T, 2, 2> tmp;
366 | 	tmp(0, 0) = m(1, 1);
367 | 	tmp(1, 0) = -m(1, 0);
368 | 	tmp(0, 1) = -m(0, 1);
369 | 	tmp(1, 1) = m(0, 0);
370 | 	return tmp * (T(1) / det);
371 | }
372 | 
373 | template<typename T>
374 | Matrix<T, 3, 3> inverse(const Matrix<T, 3, 3>& m) {
375 | 	const T det = m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) -
376 | 				  m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
377 | 				  m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));
378 | 	if(det == 0) {
379 | 		throw std::runtime_error("inverse(): determinant = 0");
380 | 	}
381 | 	Matrix<T, 3, 3> tmp;
382 | 	tmp(0, 0) = m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2);
383 | 	tmp(0, 1) = m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2);
384 | 	tmp(0, 2) = m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1);
385 | 	tmp(1, 0) = m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2);
386 | 	tmp(1, 1) = m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0);
387 | 	tmp(1, 2) = m(1, 0) * m(0, 2) - m(0, 0) * m(1, 2);
388 | 	tmp(2, 0) = m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1);
389 | 	tmp(2, 1) = m(2, 0) * m(0, 1) - m(0, 0) * m(2, 1);
390 | 	tmp(2, 2) = m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1);
391 | 	return tmp * (T(1) / det);
392 | }
393 | 
394 | template<typename T>
395 | Matrix<T, 4, 4> inverse(const Matrix<T, 4, 4>& m) {
396 | 	Matrix<T, 4, 4> tmp;
397 | 	tmp[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15]
398 | 			+ m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
399 | 
400 | 	tmp[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15]
401 | 			- m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
402 | 
403 | 	tmp[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15]
404 | 			+ m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
405 | 
406 | 	tmp[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14]
407 | 			- m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
408 | 
409 | 	tmp[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15]
410 | 			- m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
411 | 
412 | 	tmp[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15]
413 | 			+ m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
414 | 
415 | 	tmp[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15]
416 | 			- m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
417 | 
418 | 	tmp[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14]
419 | 			+ m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
420 | 
421 | 	tmp[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15]
422 | 			+ m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
423 | 
424 | 	tmp[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15]
425 | 			- m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
426 | 
427 | 	tmp[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15]
428 | 			+ m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
429 | 
430 | 	tmp[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14]
431 | 			- m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
432 | 
433 | 	tmp[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11]
434 | 			- m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
435 | 
436 | 	tmp[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11]
437 | 			+ m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
438 | 
439 | 	tmp[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11]
440 | 			- m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
441 | 
442 | 	tmp[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10]
443 | 			+ m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
444 | 
445 | 	const T det = m[0] * tmp[0] + m[1] * tmp[4] + m[2] * tmp[8] + m[3] * tmp[12];
446 | 	if(det == 0) {
447 | 		throw std::runtime_error("inverse(): determinant = 0");
448 | 	}
449 | 	return tmp * (T(1) / det);
450 | }
451 | 
452 | }
453 | 
454 | #endif /* INCLUDE_MATH_MATRIX_H_ */
455 | 


--------------------------------------------------------------------------------
/src/neo_localization_node.cpp:
--------------------------------------------------------------------------------
  1 | #include "neo_localization2/neo_localization_node.hpp"
  2 | 
  3 | using namespace std::chrono_literals;
  4 | using std::placeholders::_1;
  5 | using std::placeholders::_2;
  6 | using nav2::declare_parameter_if_not_declared;
  7 | using rcl_interfaces::msg::ParameterType;
  8 | 
  9 | 
 10 | namespace neo_localization2
 11 | {
 12 | 
 13 | NeoLocalizationNode::NeoLocalizationNode(const rclcpp::NodeOptions & options)
 14 | : nav2::LifecycleNode("neo_localization", "", options)
 15 | {
 16 |   RCLCPP_INFO(get_logger(), "Creating");
 17 | }
 18 | 
 19 | NeoLocalizationNode::~NeoLocalizationNode()
 20 | {
 21 |   stop_threads_ = true;
 22 |   if (m_map_update_thread.joinable()) {
 23 |     m_map_update_thread.join();
 24 |   }
 25 |   if (m_loc_update_timer) {
 26 |     m_loc_update_timer->cancel();
 27 |     m_loc_update_timer.reset();
 28 |   }
 29 | }
 30 | 
 31 | nav2::CallbackReturn NeoLocalizationNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
 32 | {
 33 |   RCLCPP_INFO(get_logger(), "Configuring");
 34 |   auto node = shared_from_this();
 35 |   
 36 |   callback_group_ = create_callback_group(
 37 |     rclcpp::CallbackGroupType::MutuallyExclusive, false);
 38 |   initParameters(node);
 39 |   getParameters(node);
 40 |   initTransforms();
 41 |   initPubSub();
 42 |   initNameSpace();
 43 | 
 44 |   if (!dyn_params_handler_) {
 45 |     // Add callback for dynamic parameters
 46 |     dyn_params_handler_ = this->add_on_set_parameters_callback(
 47 |       std::bind(&NeoLocalizationNode::dynamicParametersCallback, this, _1));
 48 |   }
 49 | 
 50 |   executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
 51 |   executor_->add_callback_group(callback_group_, get_node_base_interface());
 52 |   executor_thread_ = std::make_unique<nav2::NodeThread>(executor_);
 53 | 
 54 |   return nav2::CallbackReturn::SUCCESS;
 55 | }
 56 | 
 57 | nav2::CallbackReturn NeoLocalizationNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
 58 | {
 59 |   RCLCPP_INFO(get_logger(), "Activating");
 60 | 
 61 |   // Lifecycle publishers must be explicitly activated
 62 |   m_pub_map_tile->on_activate();
 63 |   m_pub_loc_pose->on_activate();
 64 |   m_pub_loc_pose_2->on_activate();
 65 |   m_pub_pose_array->on_activate();
 66 | 
 67 |   auto node = shared_from_this();
 68 | 
 69 |   //Init timer
 70 |   if (!m_loc_update_timer) {
 71 |     m_loc_update_timer = create_wall_timer(std::chrono::milliseconds(m_loc_update_time_ms), std::bind(&NeoLocalizationNode::loc_update, this));
 72 |   } else {
 73 |     m_loc_update_timer->reset();
 74 |   }
 75 | 
 76 |   if (m_map_update_thread.joinable()) {
 77 |     stop_threads_ = true;
 78 |     stop_cv_.notify_all();
 79 |     m_map_update_thread.join();
 80 |   }
 81 | 
 82 |   // Init map update thread
 83 |   stop_threads_ = false;
 84 |   m_map_update_thread = std::thread(&NeoLocalizationNode::update_loop, this);
 85 | 
 86 |   // create bond connection
 87 |   createBond();
 88 | 
 89 |   return nav2::CallbackReturn::SUCCESS;
 90 | }
 91 | 
 92 | nav2::CallbackReturn NeoLocalizationNode::on_deactivate(const rclcpp_lifecycle::State &)
 93 | {
 94 |   RCLCPP_INFO(get_logger(), "Deactivating");
 95 | 
 96 |   // stop timer
 97 |   if (m_loc_update_timer) {
 98 |     m_loc_update_timer->cancel();
 99 |     m_loc_update_timer.reset();  
100 |   }
101 | 
102 |   // ask background loop to stop, but DO NOT join here
103 |   stop_threads_ = true;
104 |   stop_cv_.notify_all();
105 | 
106 |   if (m_map_update_thread.joinable()) {
107 |     m_map_update_thread.join();
108 |   }
109 |   
110 |   // Now end the bond cleanly
111 |   destroyBond();
112 | 
113 |   // deactivate lifecycle pubs
114 |   m_pub_map_tile->on_deactivate();
115 |   m_pub_loc_pose->on_deactivate();
116 |   m_pub_loc_pose_2->on_deactivate();
117 |   m_pub_pose_array->on_deactivate();
118 | 
119 |   return nav2::CallbackReturn::SUCCESS;
120 | }
121 | 
122 | nav2::CallbackReturn NeoLocalizationNode::on_cleanup(const rclcpp_lifecycle::State &)
123 | {
124 |   RCLCPP_INFO(get_logger(), "Cleaning up");
125 | 
126 |   stop_threads_ = true;
127 |   stop_cv_.notify_all();
128 | 
129 |   if (m_map_update_thread.joinable()) {
130 |     m_map_update_thread.join();
131 |   }
132 | 
133 |   // now it’s safe to remove callbacks and free resources
134 |   if (dyn_params_handler_) {
135 |     remove_on_set_parameters_callback(dyn_params_handler_.get());
136 |     dyn_params_handler_.reset();
137 |   }
138 | 
139 |   map_received_ = false;
140 |   m_initialized = false;
141 |   m_scan_buffer.clear();
142 | 
143 |   executor_thread_.reset();
144 | 
145 |   m_map.reset(); 
146 |   m_world.reset();
147 |   transform_listener_.reset();  
148 |   buffer.reset();
149 |   m_tf_broadcaster.reset();
150 | 
151 |   m_pub_map_tile.reset();
152 |   m_pub_loc_pose.reset();
153 |   m_pub_loc_pose_2.reset();
154 |   m_pub_pose_array.reset();
155 | 
156 |   m_sub_pose_estimate.reset();
157 |   m_sub_scan_topic.reset();
158 |   m_sub_map_topic.reset();
159 |   m_sub_only_use_odom.reset();
160 | 
161 | 
162 |   return nav2::CallbackReturn::SUCCESS;
163 | }
164 | 
165 | 
166 | nav2::CallbackReturn NeoLocalizationNode::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
167 | {
168 |   RCLCPP_INFO(get_logger(), "Shutting down");
169 |   return nav2::CallbackReturn::SUCCESS;
170 | }
171 | 
172 | void NeoLocalizationNode::initParameters(nav2::LifecycleNode::SharedPtr node)
173 | {
174 |   // Declare parameters
175 |   declare_parameter_if_not_declared(node, "base_frame", rclcpp::ParameterValue("base_link")); //"Which frame to use for the robot base");
176 |   declare_parameter_if_not_declared(node, "odom_frame", rclcpp::ParameterValue("odom")); //"The name of the odom coordinate frame (local localization)");
177 |   declare_parameter_if_not_declared(node, "map_frame", rclcpp::ParameterValue("map")); //"The name of the coordinate frame published by the localization system");
178 |   declare_parameter_if_not_declared(node, "update_gain", rclcpp::ParameterValue(0.5)); //"Exponential low pass gain for localization update (0 to 1)");
179 |   declare_parameter_if_not_declared(node, "confidence_gain", rclcpp::ParameterValue(0.01)); //"Time based confidence gain when in 2D / 1D mode");
180 |   declare_parameter_if_not_declared(node, "sample_rate", rclcpp::ParameterValue(10)); //"How many particles (samples) to spread (per update)");
181 |   declare_parameter_if_not_declared(node, "loc_update_rate", rclcpp::ParameterValue(100)); //"Localization update rate [ms]");
182 |   declare_parameter_if_not_declared(node, "map_update_rate", rclcpp::ParameterValue(0.5)); //"Map tile update rate [1/s]");
183 |   declare_parameter_if_not_declared(node, "map_size", rclcpp::ParameterValue(1000)); //"Map tile size in pixels");
184 |   declare_parameter_if_not_declared(node, "map_downscale", rclcpp::ParameterValue(0)); //"How often to downscale (half) the original map");
185 |   declare_parameter_if_not_declared(node, "num_smooth", rclcpp::ParameterValue(0)); //"How many 3x3 gaussian smoothing iterations are applied to the map");
186 |   declare_parameter_if_not_declared(node, "min_score", rclcpp::ParameterValue(0.2)); //"Minimum score for valid localization (otherwise 0D mode)");
187 |   declare_parameter_if_not_declared(node, "odometry_std_xy", rclcpp::ParameterValue(0.01)); //"Odometry error in x and y [m/m] (how fast to increase particle spread when in 1D / 0D mode)");
188 |   declare_parameter_if_not_declared(node, "odometry_std_yaw", rclcpp::ParameterValue(0.01)); //"Odometry error in yaw angle [rad/rad] (how fast to increase particle spread when in 0D mode)");
189 |   declare_parameter_if_not_declared(node, "min_sample_std_xy", rclcpp::ParameterValue(0.025)); //"Minimum particle spread in x and y [m]");
190 |   declare_parameter_if_not_declared(node, "min_sample_std_yaw", rclcpp::ParameterValue(0.025)); //"Minimum particle spread in yaw angle [rad]");
191 |   declare_parameter_if_not_declared(node, "max_sample_std_xy", rclcpp::ParameterValue(0.5)); //"Initial/maximum particle spread in x and y [m]");
192 |   declare_parameter_if_not_declared(node, "max_sample_std_yaw", rclcpp::ParameterValue(0.5)); //"Initial/maximum particle spread in yaw angle [rad]");
193 |   declare_parameter_if_not_declared(node, "constrain_threshold", rclcpp::ParameterValue(0.1)); //"Threshold for 1D / 2D position decision making (minimum average second order gradient)");
194 |   declare_parameter_if_not_declared(node, "constrain_threshold_yaw", rclcpp::ParameterValue(0.2)); //"Threshold for 1D / 2D decision making (with or without orientation)");
195 |   declare_parameter_if_not_declared(node, "min_points", rclcpp::ParameterValue(20)); //"Minimum number of points per update");
196 |   declare_parameter_if_not_declared(node, "solver_gain", rclcpp::ParameterValue(0.1)); //"Solver update gain, lower gain = more stability / slower convergence");
197 |   declare_parameter_if_not_declared(node, "solver_damping", rclcpp::ParameterValue(1000.0)); //"Solver update damping, higher damping = more stability / slower convergence");
198 |   declare_parameter_if_not_declared(node, "solver_iterations", rclcpp::ParameterValue(20)); //"Number of gauss-newton iterations per sample per scan");
199 |   declare_parameter_if_not_declared(node, "transform_timeout", rclcpp::ParameterValue(0.2)); //"Maximum wait for getting transforms [s]");
200 |   declare_parameter_if_not_declared(node, "broadcast_tf", rclcpp::ParameterValue(true)); //"Whether broadcast tf or not");
201 |   declare_parameter_if_not_declared(node, "map_topic", rclcpp::ParameterValue("map")); //"Name of map topic");
202 |   declare_parameter_if_not_declared(node, "scan_topic", rclcpp::ParameterValue("scan")); //"Name of scan topic");
203 |   declare_parameter_if_not_declared(node, "initialpose", rclcpp::ParameterValue("initialpose")); //"Name of initial pose topic");
204 |   declare_parameter_if_not_declared(node, "map_tile", rclcpp::ParameterValue("map_tile")); //"Name of map tile topic");
205 |   declare_parameter_if_not_declared(node, "map_pose", rclcpp::ParameterValue("map_pose")); //"Name of map pose topic");
206 |   declare_parameter_if_not_declared(node, "particle_cloud", rclcpp::ParameterValue("particlecloud")); //"Name of particle_cloud topic");
207 |   declare_parameter_if_not_declared(node, "amcl_pose", rclcpp::ParameterValue("amcl_pose")); //"Name of amcl_pose topic");
208 |   declare_parameter_if_not_declared(node, "broadcast_info", rclcpp::ParameterValue(false)); //"Broadcast info for debugging");
209 |   declare_parameter_if_not_declared(node, "set_initial_pose", rclcpp::ParameterValue(true)); //"Whether auto set initial pose or not");
210 |   declare_parameter_if_not_declared(node, "initial_pose.x", rclcpp::ParameterValue(0.0)); //"Initial pose x");
211 |   declare_parameter_if_not_declared(node, "initial_pose.y", rclcpp::ParameterValue(0.0)); //"Initial pose y");
212 |   declare_parameter_if_not_declared(node, "initial_pose.yaw", rclcpp::ParameterValue(0.0)); //"Initial pose yaw");
213 | }
214 | 
215 | void NeoLocalizationNode::getParameters(nav2::LifecycleNode::SharedPtr node)
216 | {
217 |   // Get parameters
218 |   node->get_parameter("base_frame", m_base_frame);
219 |   node->get_parameter("odom_frame", m_odom_frame);
220 |   node->get_parameter("map_frame", m_map_frame);
221 |   node->get_parameter("update_gain", m_update_gain);
222 |   node->get_parameter("confidence_gain", m_confidence_gain);
223 |   node->get_parameter("sample_rate", m_sample_rate);
224 |   node->get_parameter("loc_update_rate", m_loc_update_time_ms);
225 |   node->get_parameter("map_update_rate", m_map_update_rate);
226 |   node->get_parameter("map_size", m_map_size);
227 |   node->get_parameter("map_downscale", m_map_downscale);
228 |   node->get_parameter("num_smooth", m_num_smooth);
229 |   node->get_parameter("min_score", m_min_score);
230 |   node->get_parameter("odometry_std_xy", m_odometry_std_xy);
231 |   node->get_parameter("odometry_std_yaw", m_odometry_std_yaw);
232 |   node->get_parameter("min_sample_std_xy", m_min_sample_std_xy);
233 |   node->get_parameter("min_sample_std_yaw", m_min_sample_std_yaw);
234 |   node->get_parameter("max_sample_std_xy", m_max_sample_std_xy);
235 |   node->get_parameter("max_sample_std_yaw", m_max_sample_std_yaw);
236 |   node->get_parameter("constrain_threshold", m_constrain_threshold);
237 |   node->get_parameter("constrain_threshold_yaw", m_constrain_threshold_yaw);
238 |   node->get_parameter("min_points", m_min_points);
239 |   node->get_parameter("solver_gain", m_solver.gain);
240 |   node->get_parameter("solver_damping", m_solver.damping);
241 |   node->get_parameter("solver_iterations", m_solver_iterations);
242 |   node->get_parameter("transform_timeout", m_transform_timeout);
243 |   node->get_parameter("broadcast_tf", m_broadcast_tf);
244 |   node->get_parameter("map_topic", m_map_topic);
245 |   node->get_parameter("scan_topic", m_scan_topic);
246 |   node->get_parameter("initialpose", m_initial_pose);
247 |   node->get_parameter("map_tile", m_map_tile);
248 |   node->get_parameter("map_pose", m_map_pose);
249 |   node->get_parameter("particle_cloud", m_particle_cloud);
250 |   node->get_parameter("amcl_pose", m_amcl_pose);
251 |   node->get_parameter("broadcast_info", m_broadcast_info);
252 |   node->get_parameter("set_initial_pose", m_set_initial_pose);
253 |   node->get_parameter("initial_pose.x", m_offset_x);
254 |   node->get_parameter("initial_pose.y", m_offset_y);
255 |   node->get_parameter("initial_pose.yaw", m_offset_yaw);
256 | }
257 | 
258 | void NeoLocalizationNode::initTransforms()
259 | {
260 |   m_tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
261 |   buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
262 |   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer);
263 | }
264 | 
265 | void NeoLocalizationNode::initPubSub()
266 | {
267 |   // Init Subscribers
268 |   m_sub_scan_topic = create_subscription<sensor_msgs::msg::LaserScan>(m_scan_topic, std::bind(&NeoLocalizationNode::scan_callback, this, _1), nav2::qos::SensorDataQoS());
269 |   m_sub_map_topic = create_subscription<nav_msgs::msg::OccupancyGrid>("/map", std::bind(&NeoLocalizationNode::map_callback, this, _1), nav2::qos::LatchedSubscriptionQoS(1));
270 |   m_sub_pose_estimate = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(m_initial_pose, std::bind(&NeoLocalizationNode::pose_callback, this, _1), nav2::qos::StandardTopicQoS(1));
271 |   m_sub_only_use_odom = create_subscription<std_msgs::msg::Bool>("/global_costmap/binary_state", std::bind(&NeoLocalizationNode::use_odom_callback, this, _1), nav2::qos::LatchedSubscriptionQoS(10));
272 | 
273 |   // Init Publishers
274 |   m_pub_map_tile = create_publisher<nav_msgs::msg::OccupancyGrid>(m_map_tile, nav2::qos::StandardTopicQoS(1));
275 |   m_pub_loc_pose = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(m_amcl_pose, nav2::qos::LatchedPublisherQoS(1));
276 |   m_pub_loc_pose_2 = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(m_map_pose, nav2::qos::StandardTopicQoS(10));
277 |   m_pub_pose_array = create_publisher<geometry_msgs::msg::PoseArray>(m_particle_cloud, nav2::qos::StandardTopicQoS(10));
278 | }
279 | 
280 | void NeoLocalizationNode::initNameSpace()
281 | {
282 |   std::string robot_namespace(this->get_namespace());
283 |   // removing the unnecessary "/" from the namespace
284 |   robot_namespace.erase(std::remove(robot_namespace.begin(), robot_namespace.end(), '/'), 
285 |   robot_namespace.end());
286 |   only_using_odom = false;
287 |   m_base_frame = robot_namespace + m_base_frame;
288 |   m_odom_frame = robot_namespace + m_odom_frame;
289 | }
290 | 
291 | /*
292 |  * Computes localization update for a single laser scan.
293 |  */
294 | void NeoLocalizationNode::scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr scan)
295 | {
296 | 
297 |   std::lock_guard<std::mutex> lock(m_node_mutex);
298 | 
299 |   if(!map_received_) {
300 |     RCLCPP_INFO_ONCE(this->get_logger(), "no map");
301 |     return;
302 |   }
303 |   RCLCPP_INFO_ONCE(this->get_logger(), "map_received");
304 |   scan->header.frame_id = m_ns + scan->header.frame_id;
305 |   m_scan_buffer[scan->header.frame_id] = scan;
306 | }
307 | 
308 | /*
309 |  * Whether to only use odom (no scan_matching) or not
310 |  */
311 | void NeoLocalizationNode::use_odom_callback(const std_msgs::msg::Bool::SharedPtr msg)
312 | {
313 |   std::lock_guard<std::mutex> lock(m_node_mutex);
314 |   if (msg->data == true) {
315 |     only_using_odom = true;
316 |   }
317 |   else {
318 |     only_using_odom = false;
319 |   }
320 | }
321 | 
322 | /*
323 |  * Convert/Transform a scan from ROS format to a specified base frame.
324 |  */
325 | std::vector<scan_point_t> NeoLocalizationNode::convert_scan(const sensor_msgs::msg::LaserScan::SharedPtr scan, const Matrix<double, 4, 4>& odom_to_base)
326 | {
327 |   std::vector<scan_point_t> points;
328 |   tf2::Stamped<tf2::Transform> base_to_odom;
329 |   tf2::Stamped<tf2::Transform> sensor_to_base;
330 |   try {
331 |     auto tempTransform = buffer->lookupTransform(m_base_frame, scan->header.frame_id, tf2::TimePointZero);
332 |     tf2::fromMsg(tempTransform, sensor_to_base);
333 | 
334 |   } catch(const std::exception& ex) {
335 |     RCLCPP_WARN_STREAM(this->get_logger(), "NeoLocalizationNode: lookupTransform(scan->header.frame_id, m_base_frame) failed: " << ex.what());
336 |     return points;
337 |   }
338 |   try {
339 |     auto tempTransform = buffer->lookupTransform(m_odom_frame, m_base_frame, scan->header.stamp, tf2::durationFromSec(m_transform_timeout));
340 |     tf2::fromMsg(tempTransform, base_to_odom);
341 |     } catch(const std::exception& ex) {
342 |     RCLCPP_WARN_STREAM(this->get_logger(), "NeoLocalizationNode: lookupTransform(m_base_frame, m_odom_frame) failed: " << ex.what());
343 |     return points;
344 |   }
345 |   
346 |   const Matrix<double, 4, 4> S = convert_transform_3(sensor_to_base);
347 |   const Matrix<double, 4, 4> L = convert_transform_25(base_to_odom);
348 | 
349 |   // precompute transformation matrix from sensor to requested base
350 |   const Matrix<double, 4, 4> T = odom_to_base * L * S;
351 | 
352 |   for(size_t i = 0; i < scan->ranges.size(); ++i)
353 |   {
354 |     if(scan->ranges[i] <= scan->range_min || scan->ranges[i] >= scan->range_max) {
355 |       continue; // no actual measurement
356 |     }
357 | 
358 |     // transform sensor points into base coordinate system
359 |     const Matrix<double, 3, 1> scan_pos = (T * rotate3_z<double>(scan->angle_min + i * scan->angle_increment)
360 |                         * Matrix<double, 4, 1>{scan->ranges[i], 0, 0, 1}).project();
361 |     scan_point_t point;
362 |     point.x = scan_pos[0];
363 |     point.y = scan_pos[1];
364 |     points.emplace_back(point);
365 |   }
366 |   return points;
367 | }
368 | 
369 | /*
370 |  * Update localization
371 |  */
372 | void NeoLocalizationNode::loc_update()
373 | {
374 |   std::lock_guard<std::mutex> lock(m_node_mutex);
375 |   if(!map_received_ || m_scan_buffer.empty() || !m_initialized) {
376 |     return;
377 |   }
378 | 
379 |   tf2::Stamped<tf2::Transform> base_to_odom;
380 | 
381 |   try {
382 |     auto tempTransform = buffer->lookupTransform(m_odom_frame, m_base_frame, tf2::TimePointZero);
383 |     tf2::fromMsg(tempTransform, base_to_odom);
384 |   } catch(const std::exception& ex) {
385 |     RCLCPP_WARN_STREAM(this->get_logger(), "NeoLocalizationNode: lookup Transform(m_base_frame, m_odom_frame) failed: " << ex.what());
386 |     return;
387 |   }
388 |   
389 |   tf2::Transform base_to_odom_ws(base_to_odom.getRotation(), base_to_odom.getOrigin());
390 |   
391 |   const Matrix<double, 4, 4> L = convert_transform_25(base_to_odom_ws);
392 |   const Matrix<double, 4, 4> T = translate25(m_offset_x, m_offset_y) * rotate25_z(m_offset_yaw);    // odom to map
393 | 
394 |   const Matrix<double, 3, 1> odom_pose = (L * Matrix<double, 4, 1>{0, 0, 0, 1}).project();
395 |   const double dist_moved = (odom_pose - m_last_odom_pose).get<2>().norm();
396 |   const double rad_rotated = fabs(angles::normalize_angle(odom_pose[2] - m_last_odom_pose[2]));
397 | 
398 |   std::vector<scan_point_t> points;
399 | 
400 |   RCLCPP_INFO_ONCE(this->get_logger(), "map_received");
401 |   // convert all scans to current base frame
402 |   for(const auto& scan : m_scan_buffer)
403 |   {
404 |     
405 |     auto scan_points = convert_scan(scan.second, L.inverse());
406 | 
407 |     points.insert(points.end(), scan_points.begin(), scan_points.end());
408 |   }
409 | 
410 |   // // check for number of points
411 |   if(static_cast<int>(points.size()) < m_min_points)
412 |   {
413 |     RCLCPP_WARN_STREAM(this->get_logger(),"NeoLocalizationNode: Number of points too low: " << points.size());
414 |     return;
415 |   }
416 | 
417 |   geometry_msgs::msg::PoseArray pose_array;
418 |   pose_array.header.stamp = tf2_ros::toMsg(base_to_odom.stamp_);
419 |   pose_array.header.frame_id = m_map_frame;
420 | 
421 |   // calc predicted grid pose based on odometry
422 |   Matrix<double, 3, 1> grid_pose; 
423 |   try {
424 |     grid_pose = (m_grid_to_map.inverse() * T * L * Matrix<double, 4, 1>{0, 0, 0, 1}).project();
425 |   } catch(const std::exception& ex) {
426 |       RCLCPP_WARN_STREAM(this->get_logger(),"NeoLocalizationNode waiting for map updates " << ex.what());
427 |       return;
428 |   }
429 | 
430 |   // setup distributions
431 |   std::normal_distribution<double> dist_x(grid_pose[0], m_sample_std_xy);
432 |   std::normal_distribution<double> dist_y(grid_pose[1], m_sample_std_xy);
433 |   std::normal_distribution<double> dist_yaw(grid_pose[2], m_sample_std_yaw);
434 | 
435 |   // solve odometry prediction first
436 |   m_solver.pose_x = grid_pose[0];
437 |   m_solver.pose_y = grid_pose[1];
438 |   m_solver.pose_yaw = grid_pose[2];
439 | 
440 |   for(int iter = 0; iter < m_solver_iterations; ++iter) {
441 |     m_solver.solve<float>(*m_map, points);
442 |   }
443 | 
444 |   double best_x = m_solver.pose_x;
445 |   double best_y = m_solver.pose_y;
446 |   double best_yaw = m_solver.pose_yaw;
447 |   double best_score = m_solver.r_norm;
448 | 
449 |   std::vector<Matrix<double, 3, 1>> seeds(m_sample_rate);
450 |   std::vector<Matrix<double, 3, 1>> samples(m_sample_rate);
451 |   std::vector<double> sample_errors(m_sample_rate);
452 | 
453 |   for(int i = 0; i < m_sample_rate; ++i)
454 |   {
455 |     // generate new sample
456 |     m_solver.pose_x = dist_x(m_generator);
457 |     m_solver.pose_y = dist_y(m_generator);
458 |     m_solver.pose_yaw = dist_yaw(m_generator);
459 | 
460 |     seeds[i] = Matrix<double, 3, 1>{m_solver.pose_x, m_solver.pose_y, m_solver.pose_yaw};
461 | 
462 |     // solve sample
463 |     for(int iter = 0; iter < m_solver_iterations; ++iter) {
464 |       m_solver.solve<float>(*m_map, points);
465 |     }
466 | 
467 |     // save sample
468 |     const auto sample = Matrix<double, 3, 1>{m_solver.pose_x, m_solver.pose_y, m_solver.pose_yaw};
469 |     samples[i] = sample;
470 |     sample_errors[i] = m_solver.r_norm;
471 | 
472 |     // check if sample is better
473 |     if(m_solver.r_norm > best_score) {
474 |       best_x = m_solver.pose_x;
475 |       best_y = m_solver.pose_y;
476 |       best_yaw = m_solver.pose_yaw;
477 |       best_score = m_solver.r_norm;
478 |     }
479 | 
480 |     // add to visualization
481 |     {
482 |       const Matrix<double, 3, 1> map_pose = (m_grid_to_map * sample.extend()).project();
483 |       tf2::Quaternion tmp;
484 |       geometry_msgs::msg::Pose pose;
485 |       pose.position.x = map_pose[0];
486 |       pose.position.y = map_pose[1];
487 |       tmp.setRPY( 0, 0, map_pose[2]);
488 |       auto tmp_msg = tf2::toMsg(tmp);
489 |       pose.orientation = tmp_msg;
490 |       pose_array.poses.push_back(pose);
491 |     }
492 |   }
493 | 
494 |   // compute covariances
495 |   double mean_score = 0;
496 |   Matrix<double, 3, 1> mean_xyw;
497 |   Matrix<double, 3, 1> seed_mean_xyw;
498 |   const double var_error = compute_variance(sample_errors, mean_score);
499 |   const Matrix<double, 3, 3> var_xyw = compute_covariance(samples, mean_xyw);
500 |   const Matrix<double, 3, 3> grad_var_xyw =
501 |       compute_virtual_scan_covariance_xyw(m_map, points, Matrix<double, 3, 1>{best_x, best_y, best_yaw});
502 | 
503 |   // compute gradient characteristic
504 |   std::array<Matrix<double, 2, 1>, 2> grad_eigen_vectors;
505 |   const Matrix<double, 2, 1> grad_eigen_values = compute_eigenvectors_2(grad_var_xyw.get<2, 2>(), grad_eigen_vectors);
506 |   const Matrix<double, 3, 1> grad_std_uvw{sqrt(grad_eigen_values[0]), sqrt(grad_eigen_values[1]), sqrt(grad_var_xyw(2, 2))};
507 | 
508 |   // decide if we have 3D, 2D, 1D or 0D localization
509 |   int mode = 0;
510 |   if ((best_score > m_min_score) && (only_using_odom == false)){
511 |     if(grad_std_uvw[0] > m_constrain_threshold) {
512 |       if(grad_std_uvw[1] > m_constrain_threshold) {
513 |         mode = 3; // 2D position + rotation
514 |       } else if(grad_std_uvw[2] > m_constrain_threshold_yaw) {
515 |         mode = 2; // 1D position + rotation
516 |       } else {
517 |         mode = 1; // 1D position only
518 |       }
519 |     }
520 |   }
521 | 
522 |   if(mode > 0)
523 |   {
524 |     double new_grid_x = best_x;
525 |     double new_grid_y = best_y;
526 |     double new_grid_yaw = best_yaw;
527 | 
528 |     if(mode < 3)
529 |     {
530 |       // constrain update to the good direction (ie. in direction of the eigen vector with the smaller sigma)
531 |       const auto delta = Matrix<double, 2, 1>{best_x, best_y} - Matrix<double, 2, 1>{grid_pose[0], grid_pose[1]};
532 |       const auto dist = grad_eigen_vectors[0].dot(delta);
533 |       new_grid_x = grid_pose[0] + dist * grad_eigen_vectors[0][0];
534 |       new_grid_y = grid_pose[1] + dist * grad_eigen_vectors[0][1];
535 |     }
536 |     if(mode < 2) {
537 |       new_grid_yaw = grid_pose[2];  // keep old orientation
538 |     }
539 | 
540 |     // use best sample for update
541 |     Matrix<double, 4, 4> grid_pose_new = translate25(new_grid_x, new_grid_y) * rotate25_z(new_grid_yaw);
542 | 
543 |     // compute new odom to map offset from new grid pose
544 |     const Matrix<double, 3, 1> new_offset =
545 |         (m_grid_to_map * grid_pose_new * L.inverse() * Matrix<double, 4, 1>{0, 0, 0, 1}).project();
546 | 
547 |     // apply new offset with an exponential low pass filter
548 |     m_offset_x += (new_offset[0] - m_offset_x) * m_update_gain;
549 |     m_offset_y += (new_offset[1] - m_offset_y) * m_update_gain;
550 |     m_offset_yaw += angles::shortest_angular_distance(m_offset_yaw, new_offset[2]) * m_update_gain;
551 |   }
552 |   m_offset_time = tf2_ros::toMsg(base_to_odom.stamp_);
553 | 
554 |   // update particle spread depending on mode
555 |   if(mode >= 3) {
556 |     m_sample_std_xy *= (1 - m_confidence_gain);
557 |   } else {
558 |     m_sample_std_xy += dist_moved * m_odometry_std_xy;
559 |   }
560 |   if(mode >= 2) {
561 |     m_sample_std_yaw *= (1 - m_confidence_gain);
562 |   } else {
563 |     m_sample_std_yaw += rad_rotated * m_odometry_std_yaw;
564 |   }
565 | 
566 |   // limit particle spread
567 |   m_sample_std_xy = fmin(fmax(m_sample_std_xy, m_min_sample_std_xy), m_max_sample_std_xy);
568 |   m_sample_std_yaw = fmin(fmax(m_sample_std_yaw, m_min_sample_std_yaw), m_max_sample_std_yaw);
569 | 
570 |   // publish new transform
571 |   broadcast();
572 | 
573 |   const Matrix<double, 3, 1> new_map_pose = (translate25(m_offset_x, m_offset_y) * rotate25_z(m_offset_yaw) *
574 |                         L * Matrix<double, 4, 1>{0, 0, 0, 1}).project();
575 |   tf2::Quaternion myQuaternion;
576 |   // publish localization pose
577 |   geometry_msgs::msg::PoseWithCovarianceStamped loc_pose;
578 |   loc_pose.header.stamp = m_offset_time;
579 |   loc_pose.header.frame_id = m_map_frame;
580 |   loc_pose.pose.pose.position.x = new_map_pose[0];
581 |   loc_pose.pose.pose.position.y = new_map_pose[1];
582 |   loc_pose.pose.pose.position.z = 0;
583 |   myQuaternion.setRPY(0, 0, new_map_pose[2]);
584 |   auto temp_quat = tf2::toMsg(myQuaternion);
585 |   loc_pose.pose.pose.orientation = temp_quat;
586 |   for(int j = 0; j < 3; ++j) {
587 |     for(int i = 0; i < 3; ++i) {
588 |       const int i_ = (i == 2 ? 5 : i);
589 |       const int j_ = (j == 2 ? 5 : j);
590 |       loc_pose.pose.covariance[j_ * 6 + i_] = var_xyw(i, j);
591 |     }
592 |   }
593 |   m_pub_loc_pose->publish(loc_pose);
594 |   m_pub_loc_pose_2->publish(loc_pose);
595 | 
596 |   // publish visualization
597 |   m_pub_pose_array->publish(pose_array);
598 | 
599 |   // keep last odom pose
600 |   m_last_odom_pose = odom_pose;
601 | 
602 |   if(m_broadcast_info == true) {
603 |     if(update_counter++ % 10 == 0) {
604 |       RCLCPP_INFO_STREAM(this->get_logger(),  "NeoLocalizationNode: score=" << float(best_score) << ", grad_uvw=[" << float(grad_std_uvw[0]) << ", " << float(grad_std_uvw[1])
605 |         << ", " << float(grad_std_uvw[2]) << "], std_xy=" << float(m_sample_std_xy) << " m, std_yaw=" << float(m_sample_std_yaw)
606 |         << " rad, mode=" << mode << "D, " << m_scan_buffer.size() << " scans, var_error=" << var_error);
607 |     }
608 |   }
609 | 
610 |   // clear scan buffer
611 |   m_scan_buffer.clear();
612 | }
613 | 
614 | /*
615 |   * Resets localization to given position.
616 |   */
617 | void NeoLocalizationNode::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose)
618 | {
619 |   {
620 |     std::lock_guard<std::mutex> lock(m_node_mutex);
621 | 
622 |     if(pose->header.frame_id != m_map_frame) {
623 |       RCLCPP_WARN_STREAM(this->get_logger(), "NeoLocalizationNode: Invalid pose estimate frame");
624 |       return;
625 |     }
626 | 
627 |     tf2::Stamped<tf2::Transform> base_to_odom;
628 |     tf2::Transform map_pose;
629 |     tf2::fromMsg(pose->pose.pose, map_pose);
630 | 
631 |     RCLCPP_INFO_STREAM(this->get_logger(), "NeoLocalizationNode: Got new map pose estimate: x=" << map_pose.getOrigin()[0]
632 |             << " m, y=" <<  map_pose.getOrigin()[1] );
633 | 
634 |     try {
635 |       auto tempTransform = buffer->lookupTransform(m_odom_frame, m_base_frame, tf2::TimePointZero);
636 |       tf2::convert(tempTransform, base_to_odom);
637 |     } catch(const std::exception& ex) {
638 |       RCLCPP_WARN_STREAM(this->get_logger(),"NeoLocalizationNode: lookupTransform(m_base_frame, m_odom_frame) failed: "<< ex.what());
639 |       return;
640 |     }
641 | 
642 |     const Matrix<double, 4, 4> L = convert_transform_25(base_to_odom);
643 | 
644 |     // compute new odom to map offset
645 |     const Matrix<double, 3, 1> new_offset =
646 |         (convert_transform_25(map_pose) * L.inverse() * Matrix<double, 4, 1>{0, 0, 0, 1}).project();
647 | 
648 |     // set new offset based on given position
649 |     m_offset_x = new_offset[0];
650 |     m_offset_y = new_offset[1];
651 |     m_offset_yaw = new_offset[2];
652 | 
653 |     // reset particle spread to maximum
654 |     m_sample_std_xy = m_max_sample_std_xy;
655 |     m_sample_std_yaw = m_max_sample_std_yaw;
656 | 
657 |     broadcast();
658 |   }
659 | 
660 |   // get a new map tile immediately
661 |   update_map();
662 | }
663 | 
664 | /*
665 |  * Stores the given map.
666 |  */
667 | void NeoLocalizationNode::map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr ros_map)
668 | {
669 |   std::lock_guard<std::mutex> lock(m_node_mutex);
670 |   map_received_ = true;
671 | 
672 |   RCLCPP_INFO_STREAM(this->get_logger(), "NeoLocalizationNode: Got new map with dimensions " << ros_map->info.width << " x " << ros_map->info.height
673 |       << " and cell size " << ros_map->info.resolution);
674 | 
675 |   {
676 |     tf2::Transform tmp;
677 |     tf2::fromMsg(ros_map->info.origin, tmp);
678 |     m_world_to_map = convert_transform_25(tmp);
679 |   }
680 |   m_world = ros_map;
681 |   // reset particle spread to maximum
682 |   m_sample_std_xy = m_max_sample_std_xy;
683 |   m_sample_std_yaw = m_max_sample_std_yaw;
684 | }
685 | 
686 | /*
687 |  * Extracts a new map tile around current position.
688 |  */
689 | void NeoLocalizationNode::update_map()
690 | {
691 |   Matrix<double, 4, 4> world_to_map;      // transformation from original grid map (integer coords) to "map frame"
692 |   Matrix<double, 3, 1> world_pose;      // pose in the original (integer coords) grid map (not map tile)
693 |   nav_msgs::msg::OccupancyGrid::SharedPtr world;
694 |   {
695 |     std::lock_guard<std::mutex> lock(m_node_mutex);
696 |     if(!m_world) {
697 |       return;
698 |     }
699 | 
700 |     tf2::Stamped<tf2::Transform> base_to_odom;
701 |     try {
702 |       auto tempTransform = buffer->lookupTransform(m_odom_frame, m_base_frame, tf2::TimePointZero);
703 |       tf2::fromMsg(tempTransform, base_to_odom);
704 |     } catch(const std::exception& ex) {
705 |       RCLCPP_WARN_STREAM(this->get_logger(),"NeoLocalizationNode: lookupTransform(m_base_frame, m_odom_frame) failed: " << ex.what());
706 |       return;
707 |     }
708 | 
709 |     const Matrix<double, 4, 4> L = convert_transform_25(base_to_odom);
710 |     const Matrix<double, 4, 4> T = translate25(m_offset_x, m_offset_y) * rotate25_z(m_offset_yaw);    // odom to map
711 |     world_pose = (m_world_to_map.inverse() * T * L * Matrix<double, 4, 1>{0, 0, 0, 1}).project();
712 | 
713 |     world = m_world;
714 |     world_to_map = m_world_to_map;
715 |   }
716 | 
717 |   // compute tile origin in pixel coords
718 |   const double world_scale = world->info.resolution;
719 |   const int tile_x = int(world_pose[0] / world_scale) - m_map_size / 2;
720 |   const int tile_y = int(world_pose[1] / world_scale) - m_map_size / 2;
721 | 
722 |   auto map = std::make_shared<GridMap<float>>(m_map_size, m_map_size, world_scale);
723 | 
724 |   // extract tile and convert to our format (occupancy between 0 and 1)
725 |   for(int y = 0; y < map->size_y(); ++y) {
726 |     for(int x = 0; x < map->size_x(); ++x) {
727 |       const int x_ = std::min(std::max(tile_x + x, 0), int(world->info.width) - 1);
728 |       const int y_ = std::min(std::max(tile_y + y, 0), int(world->info.height) - 1);
729 |       const auto cell = world->data[y_ * world->info.width + x_];
730 |       if(cell >= 0) {
731 |         (*map)(x, y) = fminf(cell / 100.f, 1.f);
732 |       } else {
733 |         (*map)(x, y) = 0;
734 |       }
735 |     }
736 |   }
737 | 
738 |   // optionally downscale map
739 |   for(int i = 0; i < m_map_downscale; ++i) {
740 |     map = map->downscale();
741 |   }
742 | 
743 |   // smooth map
744 |   for(int i = 0; i < m_num_smooth; ++i) {
745 |     map->smooth_33_1();
746 |   }
747 | 
748 |   // update map
749 |   {
750 |     std::lock_guard<std::mutex> lock(m_node_mutex);
751 |     m_map = map;
752 |     m_grid_to_map = world_to_map * translate25<double>(tile_x * world_scale, tile_y * world_scale);
753 |     m_initialized = true;
754 |   }
755 | 
756 |   const auto tile_origin = (m_grid_to_map * Matrix<double, 4, 1>{0, 0, 0, 1}).project();
757 |   (void)((m_grid_to_map * Matrix<double, 4, 1>{ map->scale() * map->size_x() / 2,
758 |                                   map->scale() * map->size_y() / 2, 0, 1}).project());
759 | 
760 |   // publish new map tile for visualization
761 |   tf2::Quaternion myQuaternion;
762 |   nav_msgs::msg::OccupancyGrid ros_grid;
763 |   ros_grid.header.stamp = m_offset_time;
764 |   ros_grid.header.frame_id = m_map_frame;
765 |   ros_grid.info.resolution = map->scale();
766 |   ros_grid.info.width = map->size_x();
767 |   ros_grid.info.height = map->size_y();
768 |   ros_grid.info.origin.position.x = tile_origin[0];
769 |   ros_grid.info.origin.position.y = tile_origin[1];
770 |   tf2::Quaternion Quaternion1;
771 |   myQuaternion.setRPY( 0, 0, tile_origin[2]);
772 |   ros_grid.info.origin.orientation = tf2::toMsg(myQuaternion);
773 |   ros_grid.data.resize(map->num_cells());
774 |   for(int y = 0; y < map->size_y(); ++y) {
775 |     for(int x = 0; x < map->size_x(); ++x) {
776 |       ros_grid.data[y * map->size_x() + x] = (*map)(x, y) * 100.f;
777 |     }
778 |   }
779 |   m_pub_map_tile->publish(ros_grid);
780 | 
781 | }
782 | 
783 | /*
784 |  * Asynchronous map update loop, running in separate thread.
785 |  */
786 | void NeoLocalizationNode::update_loop()
787 | {
788 |   RCLCPP_INFO_ONCE(this->get_logger(), "NeoLocalizationNode: Activating map update loop");
789 | 
790 |   // period from Hz (map_update_rate is in 1/s == Hz)
791 |   const auto period = std::chrono::duration<double>(1.0 / std::max(1e-6, m_map_update_rate));
792 | 
793 |   while (rclcpp::ok() && !stop_threads_) {
794 |     if (!m_initialized && !m_set_initial_pose) {
795 |       RCLCPP_WARN_THROTTLE(this->get_logger(), *get_clock(), 2000, "Is the robot pose initialized?");
796 |     } else {
797 |       try { update_map(); }
798 |       catch (const std::exception& ex) {
799 |         RCLCPP_WARN(this->get_logger(), "NeoLocalizationNode: update_map() failed: %s", ex.what());
800 |       }
801 |     }
802 | 
803 |     std::unique_lock<std::mutex> lk(stop_mtx_);
804 |     if (stop_cv_.wait_for(lk, period, [this]{ return stop_threads_.load(); })) {
805 |       break; // woke because we're stopping
806 |     }
807 |   }
808 | }
809 | 
810 | 
811 | /*
812 |  * Publishes "map" frame on tf_->
813 |  */
814 | void NeoLocalizationNode::broadcast()
815 | {
816 |   if (!m_broadcast_tf || !m_tf_broadcaster) {
817 |     return;
818 |   }
819 | 
820 |   geometry_msgs::msg::TransformStamped pose;
821 |   // Use a fresh or normalized time; don’t mutate m_offset_time directly
822 |   auto expiry = rclcpp::Time(m_offset_time) + rclcpp::Duration::from_seconds(1.0);
823 | 
824 |   pose.header.stamp = expiry;
825 |   pose.header.frame_id = m_map_frame;
826 |   pose.child_frame_id = m_odom_frame;
827 |   pose.transform.translation.x = m_offset_x;
828 |   pose.transform.translation.y = m_offset_y;
829 |   pose.transform.translation.z = 0.0;
830 |   tf2::Quaternion q; q.setRPY(0, 0, m_offset_yaw);
831 |   pose.transform.rotation = tf2::toMsg(q);
832 | 
833 |   m_tf_broadcaster->sendTransform(pose);
834 | }
835 | 
836 | /*
837 |  * Dynamic parameters callback
838 |  */
839 | rcl_interfaces::msg::SetParametersResult
840 | NeoLocalizationNode::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
841 | {
842 |   using PT = rcl_interfaces::msg::ParameterType;
843 | 
844 |   rcl_interfaces::msg::SetParametersResult res;
845 |   res.successful = true;
846 |   res.reason = "ok";
847 | 
848 |   for (const auto & p : parameters) {
849 |     const std::string & name = p.get_name();
850 |     if (name == "loc_update_rate" || name == "loc_update_rate_ms") {
851 |       res.successful = false;
852 |       res.reason = name + ": cannot be changed at runtime";
853 |       return res;
854 |     }
855 |     if (name == "base_frame" || name == "odom_frame" || name == "map_frame" ||
856 |         name == "scan_topic" || name == "map_topic" || name == "initialpose" ||
857 |         name == "map_tile" || name == "map_pose" || name == "particle_cloud" ||
858 |         name == "amcl_pose")
859 |     {
860 |       res.successful = false;
861 |       res.reason = name + ": read-only at runtime";
862 |       return res;
863 |     }
864 |   }
865 | 
866 |   std::lock_guard<std::mutex> lock(m_node_mutex);
867 | 
868 |   auto clamp_min = [](double v, double lo) { return v < lo ? lo : v; };
869 |   auto clamp_01  = [](double v) { return v < 0.0 ? 0.0 : (v > 1.0 ? 1.0 : v); };
870 | 
871 |   for (const auto & p : parameters) {
872 |     const std::string & name = p.get_name();
873 |     const auto type = p.get_type();
874 | 
875 |     switch (type) {
876 |       case PT::PARAMETER_DOUBLE: {
877 |         const double v = p.as_double();
878 | 
879 |         if (name == "update_gain") {
880 |           m_update_gain = clamp_01(v);
881 |         } else if (name == "confidence_gain") {
882 |           m_confidence_gain = clamp_01(v);
883 |         } else if (name == "min_score") {
884 |           m_min_score = clamp_min(v, 0.0);
885 |         } else if (name == "odometry_std_xy") {
886 |           m_odometry_std_xy = clamp_min(v, 0.0);
887 |         } else if (name == "odometry_std_yaw") {
888 |           m_odometry_std_yaw = clamp_min(v, 0.0);
889 |         } else if (name == "min_sample_std_xy") {
890 |           m_min_sample_std_xy = clamp_min(v, 0.0);
891 |           if (m_min_sample_std_xy > m_max_sample_std_xy) {
892 |             m_min_sample_std_xy = m_max_sample_std_xy;
893 |           }
894 |         } else if (name == "min_sample_std_yaw") {
895 |           m_min_sample_std_yaw = clamp_min(v, 0.0);
896 |           if (m_min_sample_std_yaw > m_max_sample_std_yaw) {
897 |             m_min_sample_std_yaw = m_max_sample_std_yaw;
898 |           }
899 |         } else if (name == "max_sample_std_xy") {
900 |           m_max_sample_std_xy = clamp_min(v, 0.0);
901 |           if (m_max_sample_std_xy < m_min_sample_std_xy) {
902 |             m_max_sample_std_xy = m_min_sample_std_xy;
903 |           }
904 |         } else if (name == "max_sample_std_yaw") {
905 |           m_max_sample_std_yaw = clamp_min(v, 0.0);
906 |           if (m_max_sample_std_yaw < m_min_sample_std_yaw) {
907 |             m_max_sample_std_yaw = m_min_sample_std_yaw;
908 |           }
909 |         } else if (name == "constrain_threshold") {
910 |           m_constrain_threshold = clamp_min(v, 0.0);
911 |         } else if (name == "constrain_threshold_yaw") {
912 |           m_constrain_threshold_yaw = clamp_min(v, 0.0);
913 |         } else if (name == "solver_gain") {
914 |           m_solver.gain = clamp_min(v, 0.0);
915 |         } else if (name == "solver_damping") {
916 |           m_solver.damping = clamp_min(v, 0.0);
917 |         } else if (name == "transform_timeout") {
918 |           m_transform_timeout = clamp_min(v, std::numeric_limits<double>::min()); // > 0
919 |         } else if (name == "map_update_rate") {
920 |           m_map_update_rate = clamp_min(v, std::numeric_limits<double>::min()); // > 0 Hz
921 |         } else {
922 |           // ignore unknown doubles (e.g., initial_pose.x/y/yaw if sent here)
923 |         }
924 |         break;
925 |       }
926 | 
927 |       case PT::PARAMETER_INTEGER: {
928 |         const int64_t vi = p.as_int();
929 |         if (name == "map_size") {
930 |           m_map_size = static_cast<int>(std::max<int64_t>(1, vi));
931 |         } else if (name == "sample_rate") {
932 |           m_sample_rate = static_cast<int>(std::max<int64_t>(1, vi));
933 |         } else if (name == "map_downscale") {
934 |           m_map_downscale = static_cast<int>(std::max<int64_t>(0, vi));
935 |         } else if (name == "num_smooth") {
936 |           m_num_smooth = static_cast<int>(std::max<int64_t>(0, vi));
937 |         } else if (name == "min_points") {
938 |           m_min_points = static_cast<int>(std::max<int64_t>(0, vi));
939 |         } else if (name == "solver_iterations") {
940 |           m_solver_iterations = static_cast<int>(std::max<int64_t>(1, vi));
941 |         } else {
942 |           // ignore unknown ints
943 |         }
944 |         break;
945 |       }
946 | 
947 |       case PT::PARAMETER_BOOL: {
948 |         const bool vb = p.as_bool();
949 |         if (name == "broadcast_tf") {
950 |           m_broadcast_tf = vb;
951 |         } else if (name == "broadcast_info") {
952 |           m_broadcast_info = vb;
953 |         } else {
954 |           // ignore unknown bools
955 |         }
956 |         break;
957 |       }
958 | 
959 |       default:
960 |         // reject unsupported types explicitly (prevents silent surprises)
961 |         res.successful = false;
962 |         res.reason = name + ": unsupported parameter type at runtime";
963 |         return res;
964 |     }
965 |   }
966 | 
967 |   return res;
968 | }
969 | 
970 | 
971 | } // namespace neo_localization2
972 | 
973 | #include "rclcpp_components/register_node_macro.hpp"
974 | RCLCPP_COMPONENTS_REGISTER_NODE(neo_localization2::NeoLocalizationNode)
975 | 
976 | 


--------------------------------------------------------------------------------