Skip to content
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.

Sign up for GitHub

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

Deskewing #16

Open
wants to merge 15 commits into
base: ros2
Choose a base branch
from
Open

Deskewing #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
d1d381c
Add a call to the newly separated mapper->applyInputFilters function
boxanm Dec 19, 2024
f7ad5e2
Add a first attempt for mapper-intergrated deskewing
boxanm Dec 20, 2024
dd94734
Update package version and minimum libpointmatcher_ros version
boxanm Dec 20, 2024
6900434
Add basic deskewing paralelization using OpenMP
boxanm Dec 21, 2024
82538c5
Add optional scan publishing to the mapper node
boxanm Dec 21, 2024
fef0321
Add a deskew parameter, improve logging
boxanm Dec 25, 2024
f75eed3
Improve logging
boxanm Dec 25, 2024
d3c46a0
Fix `can't subtract times with different time sources [2 != 1]` error
boxanm Dec 26, 2024
2be726b
Update a dependency version
boxanm Dec 27, 2024
3248582
Update parameters and README
boxanm Dec 27, 2024
d84e41b
Remove unnecessary hash map
boxanm Jan 5, 2025
a847824
Rename new parameters
boxanm Jan 5, 2025
6dfc0c9
Set deskewingScanPublisher queue size to 1
boxanm Jan 5, 2025
9a89347
Fix a typo and improve chrono duration variable names
boxanm Jan 5, 2025
28b8300
Rename deskewing_round_to_N_secs to deskewing_round_to_nanosecs
boxanm Jan 6, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 20 additions & 4 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,12 @@ find_package(geometry_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2 REQUIRED)
find_package(libpointmatcher_ros REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(libpointmatcher CONFIG 1.4.2 REQUIRED)
find_package(norlab_icp_mapper CONFIG 2.0.0 REQUIRED)
find_package(norlab_icp_mapper CONFIG 2.1.0 REQUIRED)
find_package(libpointmatcher_ros 2.0.1 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(OpenMP REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
"srv/SaveTrajectory.srv"
Expand All @@ -27,7 +29,11 @@ include_directories(
${norlab_icp_mapper_INCLUDE_DIRS}
)

add_executable(mapper_node src/mapper_node.cpp src/NodeParameters.cpp)
add_executable(mapper_node
src/mapper_node.cpp
src/NodeParameters.cpp
src/Deskewer.cpp
)

ament_target_dependencies(mapper_node
rclcpp
Expand All @@ -38,9 +44,14 @@ ament_target_dependencies(mapper_node
tf2_ros
tf2
libpointmatcher_ros
tf2_geometry_msgs
)

# Add OpenMP flags and link OpenMP library
target_compile_options(mapper_node PRIVATE ${OpenMP_CXX_FLAGS})
target_link_libraries(mapper_node
${norlab_icp_mapper_LIBRARIES}
OpenMP::OpenMP_CXX
)

rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
Expand All @@ -51,5 +62,10 @@ install(TARGETS mapper_node
DESTINATION lib/${PROJECT_NAME}
)

ament_package()
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME})

install(DIRECTORY config
DESTINATION share/${PROJECT_NAME})

ament_package()
57 changes: 33 additions & 24 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,37 +1,45 @@
# norlab_icp_mapper_ros

A bridge between [norlab_icp_mapper](https://github.com/norlab-ulaval/norlab_icp_mapper/) and ROS.
Check the [mapper's documentation](https://norlab-icp-mapper.readthedocs.io/en/latest/UsingInRos/) for a detailed guide.

## Node Parameters
| Name | Description | Possible values | Default Value |
|:---------------------------------:|:-----------------------------------------------------------------------------------------------------------:|:------------------------------:|:----------------------------------------------------------:|
| odom_frame | Frame used for odometry. | Any string | "odom" |
| robot_frame | Frame centered on the robot. | Any string | "base_link" |
| mapping_config | Path to the file containing the mapping config. | Any file path | "" |
| initial_map_file_name | Path of the file from which the initial map is loaded. | Any file path | "" |
| initial_robot_pose | Transformation matrix in homogeneous coordinates describing the initial pose of the robot in the map frame. | Any matrix of dimension 3 or 4 | "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]" |
| final_map_file_name | Path of the file in which the final map is saved when is_online is false. | Any file path | "map.vtk" |
| final_trajectory_file_name | Path of the file in which the final trajectory is saved when is_online is false. | Any file path | "trajectory.vtk" |
| map_publish_rate | Rate at which the map is published (in Hertz). It can be slower depending on the map update rate. | (0, ∞) | 10.0 |
| map_tf_publish_rate | Rate at which the map tf is published (in Hertz). | (0, ∞) | 10.0 |
| max_idle_time | Delay to wait being idle before shutting down ROS when is_online is false (in seconds). | [0, ∞) | 10.0 |
| is_mapping | true when map updates are wanted, false when only localization is wanted. | {true, false} | true |
| is_online | true when online mapping is wanted, false otherwise. | {true, false} | true |
| is_3D | true when a 3D sensor is used, false when a 2D sensor is used. | {true, false} | true |
| save_map_cells_on_hard_drive | true when map cell storage on hard drive is wanted, false when map cell storage in RAM is wanted. | {true, false} | true |
| publish_tfs_between_registrations | When false, the map tf is published only after registration. Otherwise with map_tf_publish_rate. | {true, false} | true |

| Name | Description | Possible values | Default Value |
| :---------------------------------: | :---------------------------------------------------------------------------------------------------------: | :----------------------------: | :--------------------------------------------------------: |
| odom_frame | Frame used for odometry. | Any string | "odom" |
| robot_frame | Frame centered on the robot. | Any string | "base_link" |
| mapping_config | Path to the file containing the mapping config. | Any file path | "" |
| initial_map_file_name | Path of the file from which the initial map is loaded. | Any file path | "" |
| initial_robot_pose | Transformation matrix in homogeneous coordinates describing the initial pose of the robot in the map frame. | Any matrix of dimension 3 or 4 | "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]" |
| final_map_file_name | Path of the file in which the final map is saved when is_online is false. | Any file path | "map.vtk" |
| final_trajectory_file_name | Path of the file in which the final trajectory is saved when is_online is false. | Any file path | "trajectory.vtk" |
| map_publish_rate | Rate at which the map is published (in Hertz). It can be slower depending on the map update rate. | (0, ∞) | 10.0 |
| map_tf_publish_rate | Rate at which the map tf is published (in Hertz). | (0, ∞) | 10.0 |
| max_idle_time | Delay to wait being idle before shutting down ROS when is_online is false (in seconds). | [0, ∞) | 10.0 |
| is_mapping | true when map updates are wanted, false when only localization is wanted. | {true, false} | true |
| is_online | true when online mapping is wanted, false otherwise. | {true, false} | true |
| is_3D | true when a 3D sensor is used, false when a 2D sensor is used. | {true, false} | true |
| save_map_cells_on_hard_drive | true when map cell storage on hard drive is wanted, false when map cell storage in RAM is wanted. | {true, false} | true |
| publish_tfs_between_registrations | When false, the map tf is published only after registration. Otherwise with map_tf_publish_rate. | {true, false} | true |
| deskew | Set to true if you want to apply motion compensation on the input point cloud. | {true, false} | true |
| expected_unique_deskewing_TF_number | How much memory should we reserve for the per-point transformations. | A positive integer | 4000 |
| deskewing_round_to_nanosecs | How much should each point's timestamp be rounded for deskewing transformation search. | A positive integer | 50000 |

## Node Topics
| Name | Description |
|:---------:|:---------------------------------------------------:|
| points_in | Topic from which the input points are retrieved. |
| map | Topic in which the map is published. |
| icp_odom | Topic in which the corrected odometry is published. |

| Name | Description |
| :----------------------: | :----------------------------------------------------------------: |
| points_in | Topic from which the input points are retrieved. |
| map | Topic in which the map is published. |
| icp_odom | Topic in which the corrected odometry is published. |
| scan_after_input_filters | The input scan, after all input filters are applied. |
| scan_after_deskew | The input scan, after all input filters and deskewing are applied. |

## Node Services

| Name | Description | Parameter Name | Parameter Description |
|:------------------:|:-----------------------------:|:--------------:|:--------------------------------------------------:|
| :----------------: | :---------------------------: | :------------: | :------------------------------------------------: |
| save_map | Saves the current map. | filename | Path of the file in which the map is saved. |
| save_trajectory | Saves the current trajectory. | filename | Path of the file in which the trajectory is saved. |
| reload_yaml_config | Reload the YAML config file. | | |
Expand All @@ -46,6 +54,8 @@ flowchart LR
/points_in([ /points_in<br>sensor_msgs/msg/PointCloud2 ]):::bugged
/icp_odom([ /icp_odom<br>nav_msgs/msg/Odometry ]):::bugged
/map([ /map<br>sensor_msgs/msg/PointCloud2 ]):::bugged
/scan_after_input_filters([ /map<br>sensor_msgs/msg/PointCloud2 ]):::bugged
/scan_after_deskew([ /map<br>sensor_msgs/msg/PointCloud2 ]):::bugged
/disable_mapping[/ /disable_mapping<br>std_srvs/srv/Empty \]:::bugged
/enable_mapping[/ /enable_mapping<br>std_srvs/srv/Empty \]:::bugged
/load_map[/ /load_map<br>norlab_icp_mapper_ros/srv/LoadMap \]:::bugged
Expand Down Expand Up @@ -92,4 +102,3 @@ style nodes opacity:0.15,fill:#FFF
style connection opacity:0.15,fill:#FFF

```

66 changes: 66 additions & 0 deletions config/mapper.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
mapper:
updateCondition:
type: delay
value: 0.5

sensorMaxRange: 100

mapperModule:
- PointDistanceMapperModule:
minDistNewPoint: 0.1

input:
- RandomSamplingDataPointsFilter:
prob: 0.8

- AddDescriptorDataPointsFilter:
descriptorName: probabilityDynamic
descriptorDimension: 1
descriptorValues: [0.6]

- SamplingSurfaceNormalDataPointsFilter:
ratio: 0.5
knn: 7
samplingMethod: 0
maxBoxDim: inf
averageExistingDescriptors: 1
keepNormals: 1
keepDensities: 0
keepEigenValues: 0
keepEigenVectors: 0

icp:
readingDataPointsFilters:
- RandomSamplingDataPointsFilter:
prob: 0.8

referenceDataPointsFilters:

outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.75

matcher:
KDTreeMatcher:
knn: 3
epsilon: 1
searchType: 1
maxDist: 200.0

errorMinimizer:
PointToPlaneErrorMinimizer:
force4DOF: 1

transformationCheckers:
- CounterTransformationChecker:
maxIterationCount: 40
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.001
smoothLength: 3

inspector: NullInspector

logger: NullLogger

post:
48 changes: 48 additions & 0 deletions launch/mapper.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
return LaunchDescription(
[
DeclareLaunchArgument(
"use_sim_time", default_value="true", description="Use simulation time"
),
Node(
package="norlab_icp_mapper_ros",
executable="mapper_node",
name="mapper_node",
output="screen",
parameters=[
{
"use_sim_time": True,
"odom_frame": "odom",
"robot_frame": "base_link",
"mapping_config": os.path.join(
get_package_share_directory("norlab_icp_mapper_ros"),
"config",
"mapper.yaml",
),
"initial_map_file_name": "",
"initial_robot_pose": "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]",
"final_map_file_name": "map.vtk",
"final_trajectory_file_name": "trajectory.vtk",
"map_publish_rate": 10.0,
"map_tf_publish_rate": 10.0,
"max_idle_time": 10.0,
"is_mapping": True,
"is_online": True,
"is_3D": True,
"save_map_cells_on_hard_drive": True,
"publish_tfs_between_registrations": True,
}
],
remappings=[
("points_in", "lslidar_point_cloud"),
],
),
]
)
10 changes: 6 additions & 4 deletions package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,28 +2,30 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>norlab_icp_mapper_ros</name>
<version>2.0.0</version>
<version>2.1.0</version>
<description>A bridge between norlab_icp_mapper and ROS.</description>
<maintainer email="[email protected]">Simon-Pierre Deschenes</maintainer>
<maintainer email="[email protected]">Matej Boxan</maintainer>
<license>new BSD</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2_ros</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>libpointmatcher_ros</depend>

<build_depend>rosidl_default_generators</build_depend>

<exec_depend>rosidl_default_runtime</exec_depend>

<member_of_group>rosidl_interface_packages</member_of_group>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
Loading