From d21fdb068f036b0fcb7ba2ee9a28bdb168324cf5 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 29 Feb 2024 11:35:09 +0100 Subject: [PATCH 01/10] Move ros params from launch file to YAML --- ros/CMakeLists.txt | 2 +- ros/config/kiss_icp.yaml | 20 ++++++++ ros/launch/odometry.launch.py | 89 +++++++++++++++-------------------- 3 files changed, 59 insertions(+), 52 deletions(-) create mode 100644 ros/config/kiss_icp.yaml diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 1b9c4b5e..4f15f0c4 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -66,6 +66,6 @@ ament_target_dependencies( rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE odometry_node) install(TARGETS odometry_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}/) +install(DIRECTORY config launch rviz DESTINATION share/${PROJECT_NAME}/) ament_package() diff --git a/ros/config/kiss_icp.yaml b/ros/config/kiss_icp.yaml new file mode 100644 index 00000000..ad75feee --- /dev/null +++ b/ros/config/kiss_icp.yaml @@ -0,0 +1,20 @@ +odometry_node: + ros__parameters: + # tf tree configuration + base_frame: "" + odom_frame: odom + publish_odom_tf: true + + #--- KISS-ICP configuration + # config.data + deskew: false + max_range: 100.0 + min_range: 5.0 + + # config.mapping + voxel_size: 1.0 + max_points_per_voxel: 20 + + # config.adaptive_threshold + initial_threshold: 2.0 + min_motion_th: 0.1 diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index f6f7dc61..6ba327f1 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -20,7 +20,6 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.conditions import IfCondition @@ -29,62 +28,50 @@ PathJoinSubstitution, PythonExpression, ) -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter from launch_ros.substitutions import FindPackageShare def generate_launch_description(): current_pkg = FindPackageShare("kiss_icp") + use_sim_time = SetParameter(name="use_sim_time", value=True) + topic_arg = DeclareLaunchArgument( + "topic", description="sensor_msg/PointCloud2 topic to process" + ) + bagfile_arg = DeclareLaunchArgument("bagfile", default_value="") + visualize_arg = DeclareLaunchArgument("visualize", default_value="true") + + kiss_icp_node = Node( + package="kiss_icp", + executable="odometry_node", + name="odometry_node", + output="screen", + remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], + parameters=[PathJoinSubstitution([current_pkg, "config", "kiss_icp.yaml"])], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + output="screen", + arguments=[ + "-d", + PathJoinSubstitution([current_pkg, "rviz", "kiss_icp.rviz"]), + ], + condition=IfCondition(LaunchConfiguration("visualize")), + ) + bagfile_play = ExecuteProcess( + cmd=["ros2", "bag", "play", LaunchConfiguration("bagfile")], + output="screen", + condition=IfCondition(PythonExpression(["'", LaunchConfiguration("bagfile"), "' != ''"])), + ) return LaunchDescription( [ - # ROS 2 parameters - DeclareLaunchArgument("topic", description="sensor_msg/PointCloud2 topic to process"), - DeclareLaunchArgument("bagfile", default_value=""), - DeclareLaunchArgument("visualize", default_value="true"), - DeclareLaunchArgument("odom_frame", default_value="odom"), - DeclareLaunchArgument("base_frame", default_value=""), - DeclareLaunchArgument("publish_odom_tf", default_value="true"), - # KISS-ICP parameters - DeclareLaunchArgument("deskew", default_value="false"), - DeclareLaunchArgument("max_range", default_value="100.0"), - DeclareLaunchArgument("min_range", default_value="5.0"), - # This thing is still not suported: https://github.com/ros2/launch/issues/290#issuecomment-1438476902 - # DeclareLaunchArgument("voxel_size", default_value=None), - Node( - package="kiss_icp", - executable="odometry_node", - name="odometry_node", - output="screen", - remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], - parameters=[ - { - "odom_frame": LaunchConfiguration("odom_frame"), - "base_frame": LaunchConfiguration("base_frame"), - "max_range": LaunchConfiguration("max_range"), - "min_range": LaunchConfiguration("min_range"), - "deskew": LaunchConfiguration("deskew"), - # "voxel_size": LaunchConfiguration("voxel_size"), - "max_points_per_voxel": 20, - "initial_threshold": 2.0, - "min_motion_th": 0.1, - "publish_odom_tf": LaunchConfiguration("publish_odom_tf"), - "visualize": LaunchConfiguration("visualize"), - } - ], - ), - Node( - package="rviz2", - executable="rviz2", - output={"both": "log"}, - arguments=["-d", PathJoinSubstitution([current_pkg, "rviz", "kiss_icp.rviz"])], - condition=IfCondition(LaunchConfiguration("visualize")), - ), - ExecuteProcess( - cmd=["ros2", "bag", "play", LaunchConfiguration("bagfile")], - output="screen", - condition=IfCondition( - PythonExpression(["'", LaunchConfiguration("bagfile"), "' != ''"]) - ), - ), + use_sim_time, + topic_arg, + bagfile_arg, + visualize_arg, + kiss_icp_node, + rviz_node, + bagfile_play, ] ) From 6a6466441056c9bdb5116365585fb4150ee4beb5 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 29 Feb 2024 12:36:26 +0100 Subject: [PATCH 02/10] Reuse arguments for debug clouds --- ros/launch/odometry.launch.py | 14 ++++++++++---- ros/src/OdometryServer.cpp | 2 +- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 6ba327f1..04fc3a77 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -28,18 +28,18 @@ PathJoinSubstitution, PythonExpression, ) -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): current_pkg = FindPackageShare("kiss_icp") - use_sim_time = SetParameter(name="use_sim_time", value=True) topic_arg = DeclareLaunchArgument( "topic", description="sensor_msg/PointCloud2 topic to process" ) bagfile_arg = DeclareLaunchArgument("bagfile", default_value="") visualize_arg = DeclareLaunchArgument("visualize", default_value="true") + use_sim_time_arg = DeclareLaunchArgument("use_sim_time", default_value="true") kiss_icp_node = Node( package="kiss_icp", @@ -47,7 +47,13 @@ def generate_launch_description(): name="odometry_node", output="screen", remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], - parameters=[PathJoinSubstitution([current_pkg, "config", "kiss_icp.yaml"])], + parameters=[ + { + "use_sim_time": LaunchConfiguration("use_sim_time"), + "publish_debug_clouds": LaunchConfiguration("visualize"), + }, + PathJoinSubstitution([current_pkg, "config", "kiss_icp.yaml"]), + ], ) rviz_node = Node( package="rviz2", @@ -66,10 +72,10 @@ def generate_launch_description(): ) return LaunchDescription( [ - use_sim_time, topic_arg, bagfile_arg, visualize_arg, + use_sim_time_arg, kiss_icp_node, rviz_node, bagfile_play, diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp index 0ddedb95..47c29724 100644 --- a/ros/src/OdometryServer.cpp +++ b/ros/src/OdometryServer.cpp @@ -58,7 +58,7 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) base_frame_ = declare_parameter("base_frame", base_frame_); odom_frame_ = declare_parameter("odom_frame", odom_frame_); publish_odom_tf_ = declare_parameter("publish_odom_tf", publish_odom_tf_); - publish_debug_clouds_ = declare_parameter("visualize", publish_debug_clouds_); + publish_debug_clouds_ = declare_parameter("publish_debug_clouds", publish_debug_clouds_); config_.max_range = declare_parameter("max_range", config_.max_range); config_.min_range = declare_parameter("min_range", config_.min_range); config_.deskew = declare_parameter("deskew", config_.deskew); From 902b8c670062d8d626ec224f5579f715dc77186f Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 29 Feb 2024 12:39:46 +0100 Subject: [PATCH 03/10] Rename odometry_node to kiss_icp_node odometry is way to generic --- ros/config/kiss_icp.yaml | 2 +- ros/launch/odometry.launch.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ros/config/kiss_icp.yaml b/ros/config/kiss_icp.yaml index ad75feee..d5a44054 100644 --- a/ros/config/kiss_icp.yaml +++ b/ros/config/kiss_icp.yaml @@ -1,4 +1,4 @@ -odometry_node: +kiss_icp_node: ros__parameters: # tf tree configuration base_frame: "" diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 04fc3a77..4c0ce08e 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -43,8 +43,8 @@ def generate_launch_description(): kiss_icp_node = Node( package="kiss_icp", - executable="odometry_node", - name="odometry_node", + executable="kiss_icp_node", + name="kiss_icp_node", output="screen", remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], parameters=[ From e61ebca1706b6c73e820d5e351f8f0c22807ae6f Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 29 Feb 2024 12:43:40 +0100 Subject: [PATCH 04/10] Rename node agin --- ros/CMakeLists.txt | 2 +- ros/src/OdometryServer.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 4f15f0c4..16ef1473 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -63,7 +63,7 @@ ament_target_dependencies( geometry_msgs tf2_ros) -rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE odometry_node) +rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE kiss_icp_node) install(TARGETS odometry_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config launch rviz DESTINATION share/${PROJECT_NAME}/) diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp index 47c29724..03e0a16c 100644 --- a/ros/src/OdometryServer.cpp +++ b/ros/src/OdometryServer.cpp @@ -53,7 +53,7 @@ using utils::GetTimestamps; using utils::PointCloud2ToEigen; OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) - : rclcpp::Node("odometry_node", options) { + : rclcpp::Node("kiss_icp_node", options) { // clang-format off base_frame_ = declare_parameter("base_frame", base_frame_); odom_frame_ = declare_parameter("odom_frame", odom_frame_); From 09874c618233d6261541bcadde5812f126a38eea Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 29 Feb 2024 12:45:15 +0100 Subject: [PATCH 05/10] Voxel size is optional paramter --- ros/config/kiss_icp.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/config/kiss_icp.yaml b/ros/config/kiss_icp.yaml index d5a44054..12441183 100644 --- a/ros/config/kiss_icp.yaml +++ b/ros/config/kiss_icp.yaml @@ -12,7 +12,7 @@ kiss_icp_node: min_range: 5.0 # config.mapping - voxel_size: 1.0 + # voxel_size: 1.0 # <--optional, default is max_range / 100.0 max_points_per_voxel: 20 # config.adaptive_threshold From 13413b7c801d86b0b1dc636acb8393f9b7a57dfe Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 29 Feb 2024 13:06:21 +0100 Subject: [PATCH 06/10] Reads better like this --- ros/launch/odometry.launch.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 4c0ce08e..3c8ebf6e 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -34,6 +34,7 @@ def generate_launch_description(): current_pkg = FindPackageShare("kiss_icp") + kiss_icp_yaml = PathJoinSubstitution([current_pkg, "config", "kiss_icp.yaml"]) topic_arg = DeclareLaunchArgument( "topic", description="sensor_msg/PointCloud2 topic to process" ) @@ -48,11 +49,9 @@ def generate_launch_description(): output="screen", remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], parameters=[ - { - "use_sim_time": LaunchConfiguration("use_sim_time"), - "publish_debug_clouds": LaunchConfiguration("visualize"), - }, - PathJoinSubstitution([current_pkg, "config", "kiss_icp.yaml"]), + {"use_sim_time": LaunchConfiguration("use_sim_time")}, + {"publish_debug_clouds": LaunchConfiguration("visualize")}, + kiss_icp_yaml, ], ) rviz_node = Node( From 5ad04c37c9e545a234ae84aaae10860d66cb2989 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Thu, 29 Feb 2024 15:38:52 +0100 Subject: [PATCH 07/10] New parameter proposal --- ros/CMakeLists.txt | 2 +- ros/config/kiss_icp.yaml | 20 --------- ros/launch/odometry.launch.py | 79 +++++++++++++++++++++++++---------- 3 files changed, 59 insertions(+), 42 deletions(-) delete mode 100644 ros/config/kiss_icp.yaml diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 16ef1473..d9698fc6 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -66,6 +66,6 @@ ament_target_dependencies( rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE kiss_icp_node) install(TARGETS odometry_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY config launch rviz DESTINATION share/${PROJECT_NAME}/) +install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}/) ament_package() diff --git a/ros/config/kiss_icp.yaml b/ros/config/kiss_icp.yaml deleted file mode 100644 index 12441183..00000000 --- a/ros/config/kiss_icp.yaml +++ /dev/null @@ -1,20 +0,0 @@ -kiss_icp_node: - ros__parameters: - # tf tree configuration - base_frame: "" - odom_frame: odom - publish_odom_tf: true - - #--- KISS-ICP configuration - # config.data - deskew: false - max_range: 100.0 - min_range: 5.0 - - # config.mapping - # voxel_size: 1.0 # <--optional, default is max_range / 100.0 - max_points_per_voxel: 20 - - # config.adaptive_threshold - initial_threshold: 2.0 - min_motion_th: 0.1 diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index 3c8ebf6e..be6304e6 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -21,7 +21,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.actions import ExecuteProcess from launch.conditions import IfCondition from launch.substitutions import ( LaunchConfiguration, @@ -32,26 +32,66 @@ from launch_ros.substitutions import FindPackageShare +# This configuration parameters are not exposed thorught the launch system, meaning you can't modify +# those throw the ros launch CLI. If you need to change these values, you could write your own +# launch file and modify the 'parameters=' block from the Node class. +class config: + # Preprocessing + max_range: float = 100.0 + min_range: float = 5.0 + deskew: bool = False + + # Mapping parameters + voxel_size: float = max_range / 100.0 + max_points_per_voxel: int = 20 + + # Adaptive threshold + initial_threshold: float = 2.0 + min_motion_th: float = 0.1 + + def generate_launch_description(): - current_pkg = FindPackageShare("kiss_icp") - kiss_icp_yaml = PathJoinSubstitution([current_pkg, "config", "kiss_icp.yaml"]) - topic_arg = DeclareLaunchArgument( - "topic", description="sensor_msg/PointCloud2 topic to process" - ) - bagfile_arg = DeclareLaunchArgument("bagfile", default_value="") - visualize_arg = DeclareLaunchArgument("visualize", default_value="true") - use_sim_time_arg = DeclareLaunchArgument("use_sim_time", default_value="true") + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + # tf tree configuration, these are the likely 3 parameters to change and nothing else + base_frame = LaunchConfiguration("base_frame", default="") + odom_frame = LaunchConfiguration("odom_frame", default="odom") + publish_odom_tf = LaunchConfiguration("publish_odom_tf", default=True) + + # ROS configuration + pointcloud_topic = LaunchConfiguration("topic") + visualize = LaunchConfiguration("visualize", default="true") + + # Optional ros bag play + bagfile = LaunchConfiguration("bagfile", default="") + + # KISS-ICP node kiss_icp_node = Node( package="kiss_icp", executable="kiss_icp_node", name="kiss_icp_node", output="screen", - remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], + remappings=[ + ("pointcloud_topic", pointcloud_topic), + ], parameters=[ - {"use_sim_time": LaunchConfiguration("use_sim_time")}, - {"publish_debug_clouds": LaunchConfiguration("visualize")}, - kiss_icp_yaml, + { + # ROS node configuration + "base_frame": base_frame, + "odom_frame": odom_frame, + "publish_odom_tf": publish_odom_tf, + # KISS-ICP configuration + "max_range": config.max_range, + "min_range": config.min_range, + "deskew": config.deskew, + "max_points_per_voxel": config.max_points_per_voxel, + "voxel_size": config.voxel_size, + "initial_threshold": config.initial_threshold, + "min_motion_th": config.min_motion_th, + # ROS CLI arguments + "publish_debug_clouds": visualize, + "use_sim_time": use_sim_time, + }, ], ) rviz_node = Node( @@ -60,21 +100,18 @@ def generate_launch_description(): output="screen", arguments=[ "-d", - PathJoinSubstitution([current_pkg, "rviz", "kiss_icp.rviz"]), + PathJoinSubstitution([FindPackageShare("kiss_icp"), "rviz", "kiss_icp.rviz"]), ], - condition=IfCondition(LaunchConfiguration("visualize")), + condition=IfCondition(visualize), ) + bagfile_play = ExecuteProcess( - cmd=["ros2", "bag", "play", LaunchConfiguration("bagfile")], + cmd=["ros2", "bag", "play", bagfile], output="screen", - condition=IfCondition(PythonExpression(["'", LaunchConfiguration("bagfile"), "' != ''"])), + condition=IfCondition(PythonExpression(["'", bagfile, "' != ''"])), ) return LaunchDescription( [ - topic_arg, - bagfile_arg, - visualize_arg, - use_sim_time_arg, kiss_icp_node, rviz_node, bagfile_play, From f1dc4c91bf5ad017a98d97c398d23a86e42d0b86 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 18 Mar 2024 11:38:28 +0100 Subject: [PATCH 08/10] Add odometry covariance to ros node (#283) * Add fixed covariance to odometry msg * Add default value just in case --- ros/launch/odometry.launch.py | 6 ++++++ ros/src/OdometryServer.cpp | 9 +++++++++ ros/src/OdometryServer.hpp | 4 ++++ 3 files changed, 19 insertions(+) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index be6304e6..df116bba 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -49,6 +49,10 @@ class config: initial_threshold: float = 2.0 min_motion_th: float = 0.1 + # Covariance diagonal values + position_covariance: float = 0.1 + orientation_covariance: float = 0.1 + def generate_launch_description(): use_sim_time = LaunchConfiguration("use_sim_time", default="true") @@ -88,6 +92,8 @@ def generate_launch_description(): "voxel_size": config.voxel_size, "initial_threshold": config.initial_threshold, "min_motion_th": config.min_motion_th, + "position_covariance": config.position_covariance, + "orientation_covariance": config.orientation_covariance, # ROS CLI arguments "publish_debug_clouds": visualize, "use_sim_time": use_sim_time, diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp index af3c79ec..96aca9e2 100644 --- a/ros/src/OdometryServer.cpp +++ b/ros/src/OdometryServer.cpp @@ -57,6 +57,8 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) odom_frame_ = declare_parameter("odom_frame", odom_frame_); publish_odom_tf_ = declare_parameter("publish_odom_tf", publish_odom_tf_); publish_debug_clouds_ = declare_parameter("publish_debug_clouds", publish_debug_clouds_); + position_covariance_ = declare_parameter("position_covariance", 0.1); + orientation_covariance_ = declare_parameter("orientation_covariance",0.1); kiss_icp::pipeline::KISSConfig config; config.max_range = declare_parameter("max_range", config.max_range); @@ -163,6 +165,13 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, odom_msg.header.stamp = stamp; odom_msg.header.frame_id = odom_frame_; odom_msg.pose.pose = tf2::sophusToPose(pose); + odom_msg.pose.covariance.fill(0.0); + odom_msg.pose.covariance[0] = position_covariance_; + odom_msg.pose.covariance[7] = position_covariance_; + odom_msg.pose.covariance[14] = position_covariance_; + odom_msg.pose.covariance[21] = orientation_covariance_; + odom_msg.pose.covariance[28] = orientation_covariance_; + odom_msg.pose.covariance[35] = orientation_covariance_; odom_publisher_->publish(std::move(odom_msg)); } diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp index 7ca7ff8c..87f44aea 100644 --- a/ros/src/OdometryServer.hpp +++ b/ros/src/OdometryServer.hpp @@ -85,6 +85,10 @@ class OdometryServer : public rclcpp::Node { /// Global/map coordinate frame. std::string odom_frame_{"odom"}; std::string base_frame_{}; + + /// Covariance diagonal + double position_covariance_; + double orientation_covariance_; }; } // namespace kiss_icp_ros From b413934dab099300bc0299777adb1252bc84dbf3 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 18 Mar 2024 11:40:39 +0100 Subject: [PATCH 09/10] pre-commit --- ros/src/OdometryServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp index 96aca9e2..1f914a0d 100644 --- a/ros/src/OdometryServer.cpp +++ b/ros/src/OdometryServer.cpp @@ -58,7 +58,7 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) publish_odom_tf_ = declare_parameter("publish_odom_tf", publish_odom_tf_); publish_debug_clouds_ = declare_parameter("publish_debug_clouds", publish_debug_clouds_); position_covariance_ = declare_parameter("position_covariance", 0.1); - orientation_covariance_ = declare_parameter("orientation_covariance",0.1); + orientation_covariance_ = declare_parameter("orientation_covariance", 0.1); kiss_icp::pipeline::KISSConfig config; config.max_range = declare_parameter("max_range", config.max_range); From 916d84d02f4b00e8456dfbf1149cbd02cd3d8f3a Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 18 Mar 2024 11:44:43 +0100 Subject: [PATCH 10/10] Expose number of icp iterations in ros (#292) * Expose registration params to ROS node * More merge conflicts * Default to multithread --- ros/launch/odometry.launch.py | 11 +++++++++++ ros/src/OdometryServer.cpp | 5 +++++ 2 files changed, 16 insertions(+) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index df116bba..a8458139 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -49,6 +49,11 @@ class config: initial_threshold: float = 2.0 min_motion_th: float = 0.1 + # Registration + max_num_iterations: int = 500 # + convergence_criterion: float = 0.0001 + max_num_threads: int = 0 + # Covariance diagonal values position_covariance: float = 0.1 orientation_covariance: float = 0.1 @@ -90,8 +95,14 @@ def generate_launch_description(): "deskew": config.deskew, "max_points_per_voxel": config.max_points_per_voxel, "voxel_size": config.voxel_size, + # Adaptive threshold "initial_threshold": config.initial_threshold, "min_motion_th": config.min_motion_th, + # Registration + "max_num_iterations": config.max_num_iterations, + "convergence_criterion": config.convergence_criterion, + "max_num_threads": config.max_num_threads, + # Fixed covariances "position_covariance": config.position_covariance, "orientation_covariance": config.orientation_covariance, # ROS CLI arguments diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp index 1f914a0d..83563896 100644 --- a/ros/src/OdometryServer.cpp +++ b/ros/src/OdometryServer.cpp @@ -70,6 +70,11 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) config.initial_threshold = declare_parameter("initial_threshold", config.initial_threshold); config.min_motion_th = declare_parameter("min_motion_th", config.min_motion_th); + config.max_num_iterations = + declare_parameter("max_num_iterations", config.max_num_iterations); + config.convergence_criterion = + declare_parameter("convergence_criterion", config.convergence_criterion); + config.max_num_threads = declare_parameter("max_num_threads", config.max_num_threads); if (config.max_range < config.min_range) { RCLCPP_WARN(get_logger(), "[WARNING] max_range is smaller than min_range, settng min_range to 0.0");