-
Notifications
You must be signed in to change notification settings - Fork 3
NDT 3D demo #12
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Draft
Fernando-Sanz
wants to merge
17
commits into
Ekumen-OS:main
Choose a base branch
from
Fernando-Sanz:demo_ndt_3d
base: main
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Draft
NDT 3D demo #12
Changes from all commits
Commits
Show all changes
17 commits
Select commit
Hold shift + click to select a range
0c7a618
First incomplete version
Fernando-Sanz c1fee67
Completed launch file
Fernando-Sanz b26ac74
Clean changes in common
Fernando-Sanz bc9036f
Clean unused changes in NDT demo
Fernando-Sanz 1a545ce
Change name, add config and map
Fernando-Sanz 86057db
Change folder name
Fernando-Sanz 98b05c9
Change NDT params file argument
Fernando-Sanz daabe20
Rviz file
Fernando-Sanz 69a3b3e
Add rviz file
Fernando-Sanz e8304eb
Change quotation marks
Fernando-Sanz 7400147
Add blender script
Fernando-Sanz 39db5c2
Update README
Fernando-Sanz 7c323c6
Update Rviz launch method
Fernando-Sanz 7f7a9cf
Reuse kobuki launch files
Fernando-Sanz 91cc9c5
Add docs in Blender script
Fernando-Sanz a4b802e
Change dates
Fernando-Sanz e9499bb
Change operation_mode name
Fernando-Sanz File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
30 changes: 30 additions & 0 deletions
30
localization/beluga_demo_ndt_3d_localization/CMakeLists.txt
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,30 @@ | ||
| # Copyright 2025 Ekumen, Inc. | ||
| # | ||
| # Licensed under the Apache License, Version 2.0 (the "License"); | ||
| # you may not use this file except in compliance with the License. | ||
| # You may obtain a copy of the License at | ||
| # | ||
| # http://www.apache.org/licenses/LICENSE-2.0 | ||
| # | ||
| # Unless required by applicable law or agreed to in writing, software | ||
| # distributed under the License is distributed on an "AS IS" BASIS, | ||
| # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| # See the License for the specific language governing permissions and | ||
| # limitations under the License. | ||
|
|
||
| cmake_minimum_required(VERSION 3.5) | ||
| project(beluga_demo_ndt_3d_localization) | ||
|
|
||
| find_package(ament_cmake REQUIRED) | ||
|
|
||
| install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) | ||
| install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) | ||
| install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) | ||
| install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) | ||
|
|
||
| if(BUILD_TESTING) | ||
| find_package(ament_lint_auto REQUIRED) | ||
| ament_lint_auto_find_test_dependencies() | ||
| endif() | ||
|
|
||
| ament_package() |
91 changes: 91 additions & 0 deletions
91
localization/beluga_demo_ndt_3d_localization/config/ndt_3d_params.yaml
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,91 @@ | ||
| ndt_amcl: | ||
| ros__parameters: | ||
| # Odometry motion model type. | ||
| robot_model_type: differential_drive | ||
| # Expected process noise in odometry’s rotation estimate from rotation. | ||
| alpha1: 0.05 | ||
| # Expected process noise in odometry’s rotation estimate from translation. | ||
| alpha2: 0.002 | ||
| # Expected process noise in odometry’s translation estimate from translation. | ||
| alpha3: 0.05 | ||
| # Expected process noise in odometry’s translation estimate from rotation. | ||
| alpha4: 0.002 | ||
| # Expected process noise in odometry's strafe estimate from translation. | ||
| alpha5: 0.002 | ||
| # The name of the coordinate frame published by the localization system. | ||
| global_frame_id: map | ||
| # The name of the coordinate frame published by the odometry system. | ||
| odom_frame_id: odom | ||
| # The name of the coordinate frame of the robot base. | ||
| base_frame_id: base_link | ||
| # The name of the topic where the map is published by the map server. | ||
| map_topic: map | ||
| # The name of the topic where scans are being published. | ||
| scan_topic: oak/points_raw | ||
| # The name of the topic where an initial pose can be published. | ||
| # The particle filter will be reset using the provided pose with covariance. | ||
| initial_pose_topic: initialpose | ||
| # Maximum number of particles that will be used. | ||
| max_particles: 1000 | ||
| # Minimum number of particles that will be used. | ||
| min_particles: 1000 | ||
| # Error allowed by KLD criteria. | ||
| pf_err: 0.05 | ||
| # KLD criteria parameter. | ||
| # Upper standard normal quantile for the probability that the error in the | ||
| # estimated distribution is less than pf_err. | ||
| pf_z: 3.0 | ||
| # Fast exponential filter constant, used to filter the average particles weights. | ||
| # Random particles are added if the fast filter result is larger than the slow filter result | ||
| # allowing the particle filter to recover from a bad approximation. | ||
| recovery_alpha_fast: 0.1 | ||
| # Slow exponential filter constant, used to filter the average particles weights. | ||
| # Random particles are added if the fast filter result is larger than the slow filter result | ||
| # allowing the particle filter to recover from a bad approximation. | ||
| recovery_alpha_slow: 0.001 | ||
| # Resample will happen after the amount of updates specified here happen. | ||
| resample_interval: 1 | ||
| # Minimum angle difference from last resample for resampling to happen again. | ||
| update_min_a: 0.1 | ||
| # Maximum angle difference from last resample for resampling to happen again. | ||
| update_min_d: 0.2 | ||
| # Maximum range of the laser. | ||
| laser_max_range: 100.0 | ||
| # Maximum number of beams to use in the sensor model. | ||
| max_beams: 5000000 | ||
| # Whether to broadcast map to odom transform or not. | ||
| tf_broadcast: true | ||
| # Transform tolerance allowed. | ||
| transform_tolerance: 1.0 | ||
| # Execution policy used to apply the motion update and importance weight steps. | ||
| # Valid options: "seq", "par". | ||
| execution_policy: par | ||
| # Whether to set initial pose based on parameters. | ||
| # When enabled, particles will be initialized with the specified pose coordinates and covariance. | ||
| set_initial_pose: true | ||
| # Initial pose x coordinate. | ||
| initial_pose.x: 0.0 | ||
| # Initial pose y coordinate. | ||
| initial_pose.y: 2.0 | ||
| # Initial pose z coordinate. | ||
| initial_pose.z: 0.0 | ||
| # Initial pose yaw coordinate. | ||
| initial_pose.yaw: 0.0 | ||
| # Initial pose xx covariance. | ||
| initial_pose.covariance_x: 0.25 | ||
| # Initial pose yy covariance. | ||
| initial_pose.covariance_y: 0.25 | ||
| # Initial pose zz covariance. | ||
| initial_pose.covariance_z: 0.25 | ||
| # Initial pose rollroll covariance. | ||
| initial_pose.covariance_roll: 0.0685 | ||
| # Initial pose pitchpitch covariance. | ||
| initial_pose.covariance_pitch: 0.0685 | ||
| # Initial pose yawyaw covariance. | ||
| initial_pose.covariance_yaw: 0.0685 | ||
| # Scaling parameter for NDT cells amplitude. | ||
| d1: 1.0 | ||
| # Scaling parameter for NDT cells covariance. | ||
| d2: 0.5 | ||
| # Minimum score NDT measurement cells. | ||
| minimum_likelihood: 0.01 |
116 changes: 116 additions & 0 deletions
116
localization/beluga_demo_ndt_3d_localization/launch/demo_hallway_ndt_localization.launch.py
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,116 @@ | ||
| #!/usr/bin/python3 | ||
|
|
||
| # Copyright 2025 Ekumen, Inc. | ||
| # | ||
| # Licensed under the Apache License, Version 2.0 (the "License"); | ||
| # you may not use this file except in compliance with the License. | ||
| # You may obtain a copy of the License at | ||
| # | ||
| # http://www.apache.org/licenses/LICENSE-2.0 | ||
| # | ||
| # Unless required by applicable law or agreed to in writing, software | ||
| # distributed under the License is distributed on an "AS IS" BASIS, | ||
| # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| # See the License for the specific language governing permissions and | ||
| # limitations under the License. | ||
|
|
||
| import os | ||
|
|
||
| from ament_index_python.packages import get_package_share_directory | ||
| from launch import LaunchDescription | ||
| from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription | ||
| from launch.substitutions import LaunchConfiguration | ||
| from launch.launch_description_sources import PythonLaunchDescriptionSource | ||
| from launch_ros.substitutions import FindPackageShare | ||
| from launch.substitutions import PathJoinSubstitution | ||
| from launch_ros.actions import Node | ||
|
|
||
| from launch_ros.actions import SetParameter | ||
|
|
||
|
|
||
| def generate_launch_description(): | ||
| pkg_dir = get_package_share_directory("beluga_demo_ndt_3d_localization") | ||
|
|
||
| param_files_install_folder = os.path.join(pkg_dir, "config") | ||
| available_param_files = os.listdir(param_files_install_folder) | ||
|
|
||
| worlds_install_folder = os.path.join( | ||
| get_package_share_directory("beluga_demo_gazebo"), | ||
| "worlds", | ||
| ) | ||
| available_worlds = os.listdir(worlds_install_folder) | ||
| world_name_conf = LaunchConfiguration("world_name") | ||
|
|
||
| amcl_ndt_params_file_conf = LaunchConfiguration("amcl_ndt_params_file") | ||
| localization_params_file = PathJoinSubstitution( | ||
| [pkg_dir, "config", amcl_ndt_params_file_conf] | ||
| ) | ||
|
|
||
| rviz_file = PathJoinSubstitution([pkg_dir, "rviz", "ndt_amcl_3d.ros2.rviz"]) | ||
|
|
||
| return LaunchDescription( | ||
| [ | ||
| DeclareLaunchArgument( | ||
| name="amcl_ndt_params_file", | ||
| description="Node parameters file for AMCL NDT node.", | ||
| choices=available_param_files, | ||
| default_value="ndt_3d_params.yaml", | ||
| ), | ||
| DeclareLaunchArgument( | ||
| name="localization_ndt_map", | ||
| description="Map HDF5 file used by the localization node.", | ||
| ), | ||
| DeclareLaunchArgument( | ||
| name="world_name", | ||
| default_value="magazino_hallway.world", | ||
| description="Name of the world file to load in simulation", | ||
| choices=available_worlds, | ||
| ), | ||
| ### | ||
| IncludeLaunchDescription( | ||
| PythonLaunchDescriptionSource( | ||
| PathJoinSubstitution( | ||
| [ | ||
| FindPackageShare("gonbuki_bringup_simulation_stack"), | ||
| "launch", | ||
| "bringup_launch.py", | ||
| ] | ||
| ), | ||
| ), | ||
| launch_arguments={ | ||
| "rviz": "false", | ||
| "input_type": "keyboard", | ||
| "operation_mode": "bare", | ||
| "world_name": world_name_conf, | ||
| }.items(), | ||
| ), | ||
| IncludeLaunchDescription( | ||
| PythonLaunchDescriptionSource( | ||
| PathJoinSubstitution( | ||
| [ | ||
| FindPackageShare("beluga_example"), | ||
| "launch", | ||
| "utils", | ||
| "ndt_3d_localization_launch.py", | ||
| ] | ||
| ), | ||
| ), | ||
| launch_arguments={ | ||
| "localization_params_file": localization_params_file, | ||
| "localization_ndt_map": LaunchConfiguration("localization_ndt_map"), | ||
| }.items(), | ||
| ), | ||
| ### | ||
| SetParameter('use_sim_time', True), | ||
| Node( | ||
| package='rviz2', | ||
| executable='rviz2', | ||
| name='rviz2', | ||
| output='own_log', | ||
| arguments=[ | ||
| '--display-config', | ||
| rviz_file, | ||
| ], | ||
| ), | ||
| ] | ||
| ) | ||
Binary file added
BIN
+342 KB
localization/beluga_demo_ndt_3d_localization/maps/magazino_hallway/map.hdf5
Binary file not shown.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,22 @@ | ||
| <?xml version="1.0"?> | ||
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>beluga_demo_ndt_3d_localization</name> | ||
| <version>2.0.0</version> | ||
| <description>Demos of Beluga NDT AMCL based localization indoors.</description> | ||
| <maintainer email="[email protected]">Gerardo Puga</maintainer> | ||
| <license>Apache License 2.0</license> | ||
|
|
||
| <buildtool_depend>ament_cmake</buildtool_depend> | ||
|
|
||
| <depend>beluga_example</depend> | ||
| <depend>beluga_demo_gazebo</depend> | ||
|
|
||
| <depend>gonbuki_bringup_simulation_stack</depend> | ||
|
|
||
| <depend>teleop_twist_keyboard</depend> | ||
|
|
||
| <export> | ||
| <build_type>ament_cmake</build_type> | ||
| </export> | ||
| </package> |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@Fernando-Sanz meta: hmm, I'm not super fond of (re)using
beluga_examplehere. It's a leaf package we don't release and we are making use of utility launch files that could go away the next refactor.