diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py index ad0900d0..3e9c9257 100644 --- a/python/kiss_icp/tools/visualizer.py +++ b/python/kiss_icp/tools/visualizer.py @@ -29,7 +29,7 @@ import numpy as np -YELLOW = np.array([1, 0.706, 0]) +CYAN = np.array([0.24, 0.898, 1]) RED = np.array([128, 0, 0]) / 255.0 BLACK = np.array([0, 0, 0]) / 255.0 BLUE = np.array([0.4, 0.5, 0.9]) @@ -198,7 +198,7 @@ def _update_geometries(self, source, keypoints, target, pose): # Source hot frame if self.render_source: self.source.points = self.o3d.utility.Vector3dVector(source) - self.source.paint_uniform_color(YELLOW) + self.source.paint_uniform_color(CYAN) if self.global_view: self.source.transform(pose) else: @@ -207,7 +207,7 @@ def _update_geometries(self, source, keypoints, target, pose): # Keypoints if self.render_keypoints: self.keypoints.points = self.o3d.utility.Vector3dVector(keypoints) - self.keypoints.paint_uniform_color(YELLOW) + self.keypoints.paint_uniform_color(CYAN) if self.global_view: self.keypoints.transform(pose) else: 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/rviz/kiss_icp.rviz b/ros/rviz/kiss_icp.rviz index 7320bdd0..0bf30841 100644 --- a/ros/rviz/kiss_icp.rviz +++ b/ros/rviz/kiss_icp.rviz @@ -23,7 +23,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: frame_deskew + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -38,7 +38,7 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 0 + Color: 61; 229; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true @@ -52,7 +52,7 @@ Visualization Manager: Selectable: true Size (Pixels): 3 Size (m): 0.019999999552965164 - Style: Flat Squares + Style: Points Topic: Depth: 5 Durability Policy: Volatile @@ -84,8 +84,8 @@ Visualization Manager: Name: kiss_keypoints Position Transformer: XYZ Selectable: true - Size (Pixels): 10 - Size (m): 1 + Size (Pixels): 3 + Size (m): 0.20000000298023224 Style: Points Topic: Depth: 5 @@ -100,14 +100,14 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 98.31531524658203 - Min Value: 0.6587954163551331 + Max Value: 14.369325637817383 + Min Value: 0.20239250361919403 Value: true Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor + Color: 102; 102; 102 + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false @@ -174,7 +174,7 @@ Visualization Manager: Reliability Policy: Best Effort Value: /kiss/odometry Value: true - Enabled: true + Enabled: false Name: odometry - Class: rviz_default_plugins/Axes Enabled: true @@ -185,7 +185,7 @@ Visualization Manager: Value: true Enabled: true Global Options: - Background Color: 48; 48; 48 + Background Color: 0; 0; 0 Fixed Frame: odom Frame Rate: 30 Name: root @@ -229,16 +229,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 39.488563537597656 + Distance: 178.319580078125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 3.482842445373535 - Y: 4.3972296714782715 - Z: 10.242613792419434 + X: -68.01193237304688 + Y: 258.05126953125 + Z: -27.22064781188965 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp index dfdc975e..47551e4b 100644 --- a/ros/src/OdometryServer.cpp +++ b/ros/src/OdometryServer.cpp @@ -45,6 +45,26 @@ #include #include +namespace { +Sophus::SE3d LookupTransform(const std::string &target_frame, + const std::string &source_frame, + const std::unique_ptr &tf2_buffer) { + std::string err_msg; + if (tf2_buffer->canTransform(target_frame, source_frame, tf2::TimePointZero, &err_msg)) { + try { + auto tf = tf2_buffer->lookupTransform(target_frame, source_frame, tf2::TimePointZero); + return tf2::transformToSophus(tf); + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "%s", ex.what()); + } + } + RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "Failed to find tf. Reason=%s", + err_msg.c_str()); + // default construction is the identity + return Sophus::SE3d(); +} +} // namespace + namespace kiss_icp_ros { using utils::EigenToPointCloud2; @@ -52,11 +72,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 +90,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"); @@ -101,106 +128,70 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized"); } -Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, - const std::string &source_frame) const { - std::string err_msg; - if (tf2_buffer_->_frameExists(source_frame) && // - tf2_buffer_->_frameExists(target_frame) && // - tf2_buffer_->canTransform(target_frame, source_frame, tf2::TimePointZero, &err_msg)) { - try { - auto tf = tf2_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); - return tf2::transformToSophus(tf); - } catch (tf2::TransformException &ex) { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - } - } - RCLCPP_WARN(this->get_logger(), "Failed to find tf. Reason=%s", err_msg.c_str()); - return {}; -} - void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { const auto cloud_frame_id = msg->header.frame_id; const auto points = PointCloud2ToEigen(msg); const auto timestamps = GetTimestamps(msg); - const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); // Register frame, main entry point to KISS-ICP pipeline const auto &[frame, keypoints] = kiss_icp_->RegisterFrame(points, timestamps); - // Compute the pose using KISS, ego-centric to the LiDAR + // Extract the last KISS-ICP pose, ego-centric to the LiDAR const Sophus::SE3d kiss_pose = kiss_icp_->poses().back(); + // Spit the current estimated pose to ROS msgs handling the desired target frame + PublishOdometry(kiss_pose, msg->header); + // Publishing these clouds is a bit costly, so do it only if we are debugging + if (publish_debug_clouds_) { + PublishClouds(frame, keypoints, msg->header); + } +} + +void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose, + const std_msgs::msg::Header &header) { // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame + const auto cloud_frame_id = header.frame_id; + const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); const auto pose = [&]() -> Sophus::SE3d { if (egocentric_estimation) return kiss_pose; - const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_); return cloud2base * kiss_pose * cloud2base.inverse(); }(); - // Spit the current estimated pose to ROS msgs - PublishOdometry(pose, msg->header.stamp, cloud_frame_id); - // Publishing this clouds is a bit costly, so do it only if we are debugging - if (publish_debug_clouds_) { - PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id); - } -} - -void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, - const rclcpp::Time &stamp, - const std::string &cloud_frame_id) { // Broadcast the tf --- if (publish_odom_tf_) { geometry_msgs::msg::TransformStamped transform_msg; - transform_msg.header.stamp = stamp; + transform_msg.header.stamp = header.stamp; transform_msg.header.frame_id = odom_frame_; - transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_; + transform_msg.child_frame_id = egocentric_estimation ? cloud_frame_id : base_frame_; transform_msg.transform = tf2::sophusToTransform(pose); tf_broadcaster_->sendTransform(transform_msg); } // publish odometry msg nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = stamp; + odom_msg.header.stamp = header.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)); } void OdometryServer::PublishClouds(const std::vector frame, const std::vector keypoints, - const rclcpp::Time &stamp, - const std::string &cloud_frame_id) { - std_msgs::msg::Header odom_header; - odom_header.stamp = stamp; - odom_header.frame_id = odom_frame_; - - // Publish map + const std_msgs::msg::Header &header) { const auto kiss_map = kiss_icp_->LocalMap(); + const auto kiss_pose = kiss_icp_->poses().back().inverse(); - if (!publish_odom_tf_) { - // debugging happens in an egocentric world - std_msgs::msg::Header cloud_header; - cloud_header.stamp = stamp; - cloud_header.frame_id = cloud_frame_id; - - frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud_header))); - kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud_header))); - map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header))); - - return; - } - - // If transmitting to tf tree we know where the clouds are exactly - const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id); - frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud2odom, odom_header))); - kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud2odom, odom_header))); - - if (!base_frame_.empty()) { - const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); - map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, cloud2base, odom_header))); - } else { - map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header))); - } + frame_publisher_->publish(std::move(EigenToPointCloud2(frame, header))); + kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, header))); + map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, kiss_pose, header))); } } // namespace kiss_icp_ros diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp index 7ca7ff8c..f894ef9a 100644 --- a/ros/src/OdometryServer.hpp +++ b/ros/src/OdometryServer.hpp @@ -33,6 +33,7 @@ #include #include #include +#include #include namespace kiss_icp_ros { @@ -48,19 +49,12 @@ class OdometryServer : public rclcpp::Node { void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); /// Stream the estimated pose to ROS - void PublishOdometry(const Sophus::SE3d &pose, - const rclcpp::Time &stamp, - const std::string &cloud_frame_id); + void PublishOdometry(const Sophus::SE3d &kiss_pose, const std_msgs::msg::Header &header); /// Stream the debugging point clouds for visualization (if required) void PublishClouds(const std::vector frame, const std::vector keypoints, - const rclcpp::Time &stamp, - const std::string &cloud_frame_id); - - /// Utility function to compute transformation using tf tree - Sophus::SE3d LookupTransform(const std::string &target_frame, - const std::string &source_frame) const; + const std_msgs::msg::Header &header); private: /// Tools for broadcasting TFs. @@ -85,6 +79,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