diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 1b9c4b5e..d9698fc6 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 launch rviz DESTINATION share/${PROJECT_NAME}/) diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py index f6f7dc61..a8458139 100644 --- a/ros/launch/odometry.launch.py +++ b/ros/launch/odometry.launch.py @@ -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, @@ -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, ] ) diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp index dfdc975e..83563896 100644 --- a/ros/src/OdometryServer.cpp +++ b/ros/src/OdometryServer.cpp @@ -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("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_); + 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); @@ -68,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"); @@ -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)); } 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