Skip to content

Commit

Permalink
Refactor ROS parametrization (#282)
Browse files Browse the repository at this point in the history 
* Move ros params from launch file to YAML

* Reuse arguments for debug clouds

* Rename odometry_node to kiss_icp_node

odometry is way to generic

* Rename node agin

* Voxel size is optional paramter

* Reads better like this

* New parameter proposal

* Add odometry covariance to ros node (#283)

* Add fixed covariance to odometry msg

* Add default value just in case

* pre-commit

* Expose number of icp iterations in ros (#292)

* Expose registration params to ROS node

* More merge conflicts

* Default to multithread
  • Loading branch information
@nachovizzo
nachovizzo authored Mar 25, 2024
1 parent 8450d58 commit 6a941d0
Show file tree
Hide file tree
Showing 4 changed files with 119 additions and 55 deletions.
2 changes: 1 addition & 1 deletion ros/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 launch rviz DESTINATION share/${PROJECT_NAME}/)
Expand Down
150 changes: 98 additions & 52 deletions ros/launch/odometry.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,8 @@
# 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.actions import ExecuteProcess
from launch.conditions import IfCondition
from launch.substitutions import (
LaunchConfiguration,
Expand All @@ -33,58 +32,105 @@
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

# 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


def generate_launch_description():
current_pkg = FindPackageShare("kiss_icp")
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", pointcloud_topic),
],
parameters=[
{
# 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,
# 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
"publish_debug_clouds": visualize,
"use_sim_time": use_sim_time,
},
],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
output="screen",
arguments=[
"-d",
PathJoinSubstitution([FindPackageShare("kiss_icp"), "rviz", "kiss_icp.rviz"]),
],
condition=IfCondition(visualize),
)

bagfile_play = ExecuteProcess(
cmd=["ros2", "bag", "play", bagfile],
output="screen",
condition=IfCondition(PythonExpression(["'", 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"), "' != ''"])
),
),
kiss_icp_node,
rviz_node,
bagfile_play,
]
)
18 changes: 16 additions & 2 deletions ros/src/OdometryServer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,11 +52,13 @@ using utils::GetTimestamps;
using utils::PointCloud2ToEigen;

OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
: rclcpp::Node("odometry_node", options) {
: rclcpp::Node("kiss_icp_node", options) {
base_frame_ = declare_parameter<std::string>("base_frame", base_frame_);
odom_frame_ = declare_parameter<std::string>("odom_frame", odom_frame_);
publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
publish_debug_clouds_ = declare_parameter<bool>("visualize", publish_debug_clouds_);
publish_debug_clouds_ = declare_parameter<bool>("publish_debug_clouds", publish_debug_clouds_);
position_covariance_ = declare_parameter<double>("position_covariance", 0.1);
orientation_covariance_ = declare_parameter<double>("orientation_covariance", 0.1);

kiss_icp::pipeline::KISSConfig config;
config.max_range = declare_parameter<double>("max_range", config.max_range);
Expand All @@ -68,6 +70,11 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
config.initial_threshold =
declare_parameter<double>("initial_threshold", config.initial_threshold);
config.min_motion_th = declare_parameter<double>("min_motion_th", config.min_motion_th);
config.max_num_iterations =
declare_parameter<int>("max_num_iterations", config.max_num_iterations);
config.convergence_criterion =
declare_parameter<double>("convergence_criterion", config.convergence_criterion);
config.max_num_threads = declare_parameter<int>("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");
Expand Down Expand Up @@ -163,6 +170,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));
}

Expand Down
4 changes: 4 additions & 0 deletions ros/src/OdometryServer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

0 comments on commit 6a941d0

Please sign in to comment.