diff --git a/.github/workflows/ros.yml b/.github/workflows/ros.yml
index dc54d50c..4ae5cf29 100644
--- a/.github/workflows/ros.yml
+++ b/.github/workflows/ros.yml
@@ -7,31 +7,11 @@ on:
branches: ["main"]
jobs:
- ros1_node:
- runs-on: ubuntu-latest
- strategy:
- matrix:
- release: [noetic]
- container: osrf/ros:${{ matrix.release }}-desktop-full
- steps:
- - name: Setup cmake
- uses: jwlawson/actions-setup-cmake@v1.13
- with:
- cmake-version: "3.25.x"
- - name: Prepare catkin_ws
- run: mkdir -p catkin_ws/src
- - uses: actions/checkout@v3
- with:
- path: catkin_ws/src
- - name: Run catkin_make
- run: source /opt/ros/${{ matrix.release }}/setup.bash && cd catkin_ws && catkin_make
- shell: bash
-
ros2_node:
runs-on: ubuntu-latest
strategy:
matrix:
- release: [humble, iron]
+ release: [humble, iron, rolling]
container: osrf/ros:${{ matrix.release }}-desktop
steps:
- name: Setup cmake
diff --git a/README.md b/README.md
index 16af3cd1..86443f80 100644
--- a/README.md
+++ b/README.md
@@ -11,12 +11,7 @@
•
Install
•
- ROS 1
- •
ROS 2
- •
- ROS Demo
•
Paper
•
@@ -49,9 +44,10 @@ kiss_icp_pipeline --help
This should print the following help message:
+
-For advanced instructions on the Python pacakge plase see [this README](python/README.md)
+For advanced instructions on the Python package please see [this README](python/README.md)
## ROS support
@@ -61,17 +57,18 @@ For advanced instructions on the Python pacakge plase see [this README](python/R
```sh
cd ~/ros2_ws/src/ && git clone https://github.com/PRBonn/kiss-icp && cd ~/ros2_ws/ && colcon build --packages-select kiss_icp
```
+For more detailed instructions on the ROS wrapper, please visit this [README](ros/README.md)
+
ROS 1
-```sh
-cd ~/catkin_ws/ && git clone https://github.com/PRBonn/kiss-icp && catkin build
-```
-
+⚠️ ⚠️ **ROS 1 is deprecated in KISS-ICP and is not officially supported anymore. Upgrade now to ROS 2!** ⚠️ ⚠️
-For more detailed instructions on the ROS wrappers, please visit this [README](ros/README.md)
+The last release that supports ROS 1 is [v0.3.0](https://github.com/PRBonn/kiss-icp/tree/v0.3.0), if you still need ROS 1 support please check that version.
+
+
## Citation
diff --git a/cpp/kiss_icp/CMakeLists.txt b/cpp/kiss_icp/CMakeLists.txt
index ba62c9e2..abbb01a7 100644
--- a/cpp/kiss_icp/CMakeLists.txt
+++ b/cpp/kiss_icp/CMakeLists.txt
@@ -21,7 +21,7 @@
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
cmake_minimum_required(VERSION 3.16...3.26)
-project(kiss_icp_cpp VERSION 0.3.0 LANGUAGES CXX)
+project(kiss_icp_cpp VERSION 0.4.0 LANGUAGES CXX)
# Setup build options
option(USE_CCACHE "Build using Ccache if found on the path" ON)
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 5caf44e7..eaa2f9bd 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -21,7 +21,7 @@
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
cmake_minimum_required(VERSION 3.16...3.26)
-project(kiss_icp_pybind VERSION 0.3.0 LANGUAGES CXX)
+project(kiss_icp_pybind VERSION 0.4.0 LANGUAGES CXX)
# Set build type
set(CMAKE_BUILD_TYPE Release)
diff --git a/python/README.md b/python/README.md
index 90de0415..09725043 100644
--- a/python/README.md
+++ b/python/README.md
@@ -11,8 +11,6 @@
•
Install
•
- ROS 1
- •
ROS 2
•
*.bag
```
-## ROS 1
-
-### How to build
-
-You should not need any extra dependency, just clone and build:
-
-```sh
-cd ~/catkin_ws/
-git clone https://github.com/PRBonn/kiss-icp
-catkin build
-source devel/setup.bash
-```
-
-### How to run
-
-The only required argument to provide is the **topic name** so KISS-ICP knows which PointCloud2 to proces:
-
-```sh
-roslaunch kiss_icp odometry.launch bagfile:= topic:=
-```
-
-You can optionally launch the node with any bagfile, and play the bagfiles on a different shell:
-
-```sh
-roslaunch kiss_icp odometry.launch topic:=
-```
-
-and then,
-
-```sh
-rosbag play *.bag
-```
-
## Out of source builds
Good news! If you don't have git or you don't need to change the core KISS-ICP library, you can just
-copy paste this folder into your ROS1/ROS2 workspace and build as usual. The build system will fetch
+copy paste this folder into your ROS2 workspace and build as usual. The build system will fetch
the latest stable release for you.
## Looking how to run KITTI on ROS along with KISS-ICP?
diff --git a/ros/launch/odometry.launch b/ros/launch/odometry.launch
deleted file mode 100644
index 4dc9a26b..00000000
--- a/ros/launch/odometry.launch
+++ /dev/null
@@ -1,45 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py
index 470f2e07..f6f7dc61 100644
--- a/ros/launch/odometry.launch.py
+++ b/ros/launch/odometry.launch.py
@@ -76,7 +76,7 @@ def generate_launch_description():
package="rviz2",
executable="rviz2",
output={"both": "log"},
- arguments=["-d", PathJoinSubstitution([current_pkg, "rviz", "kiss_icp_ros2.rviz"])],
+ arguments=["-d", PathJoinSubstitution([current_pkg, "rviz", "kiss_icp.rviz"])],
condition=IfCondition(LaunchConfiguration("visualize")),
),
ExecuteProcess(
diff --git a/ros/package.xml b/ros/package.xml
index ac7c00c4..eff04c96 100644
--- a/ros/package.xml
+++ b/ros/package.xml
@@ -25,26 +25,16 @@
-->
kiss_icp
- 0.3.0
- KISS-ICP ROS Wrappers
+ 0.4.0
+ KISS-ICP ROS 2 Wrapper
ivizzo
MIT
-
- ros_environment
+ ament_cmake
-
- catkin
- roscpp
-
-
- ament_cmake
- rcutils
- rclcpp
- rclcpp_components
- ros2launch
-
-
+ rcutils
+ rclcpp
+ rclcpp_components
geometry_msgs
nav_msgs
std_msgs
@@ -52,8 +42,9 @@
tf2
tf2_ros
+ ros2launch
+
- catkin
- ament_cmake
+ ament_cmake
diff --git a/ros/ros1/OdometryServer.cpp b/ros/ros1/OdometryServer.cpp
deleted file mode 100644
index 3cf3c480..00000000
--- a/ros/ros1/OdometryServer.cpp
+++ /dev/null
@@ -1,224 +0,0 @@
-// MIT License
-//
-// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
-// Stachniss.
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// 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.
-#include
-#include
-#include
-#include
-
-// KISS-ICP-ROS
-#include "OdometryServer.hpp"
-#include "Utils.hpp"
-
-// KISS-ICP
-#include "kiss_icp/pipeline/KissICP.hpp"
-
-// ROS 1 headers
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-namespace kiss_icp_ros {
-
-using utils::EigenToPointCloud2;
-using utils::GetTimestamps;
-using utils::PointCloud2ToEigen;
-
-OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &pnh)
- : nh_(nh), pnh_(pnh), tf2_listener_(tf2_ros::TransformListener(tf2_buffer_)) {
- pnh_.param("base_frame", base_frame_, base_frame_);
- pnh_.param("odom_frame", odom_frame_, odom_frame_);
- pnh_.param("publish_odom_tf", publish_odom_tf_, false);
- pnh_.param("visualize", publish_debug_clouds_, publish_debug_clouds_);
- pnh_.param("max_range", config_.max_range, config_.max_range);
- pnh_.param("min_range", config_.min_range, config_.min_range);
- pnh_.param("deskew", config_.deskew, config_.deskew);
- pnh_.param("voxel_size", config_.voxel_size, config_.max_range / 100.0);
- pnh_.param("max_points_per_voxel", config_.max_points_per_voxel, config_.max_points_per_voxel);
- pnh_.param("initial_threshold", config_.initial_threshold, config_.initial_threshold);
- pnh_.param("min_motion_th", config_.min_motion_th, config_.min_motion_th);
- if (config_.max_range < config_.min_range) {
- ROS_WARN("[WARNING] max_range is smaller than min_range, setting min_range to 0.0");
- config_.min_range = 0.0;
- }
-
- // Construct the main KISS-ICP odometry node
- odometry_ = kiss_icp::pipeline::KissICP(config_);
-
- // Initialize subscribers
- pointcloud_sub_ = nh_.subscribe("pointcloud_topic", queue_size_,
- &OdometryServer::RegisterFrame, this);
-
- // Initialize publishers
- odom_publisher_ = pnh_.advertise("/kiss/odometry", queue_size_);
- traj_publisher_ = pnh_.advertise("/kiss/trajectory", queue_size_);
- if (publish_debug_clouds_) {
- frame_publisher_ = pnh_.advertise("/kiss/frame", queue_size_);
- kpoints_publisher_ =
- pnh_.advertise("/kiss/keypoints", queue_size_);
- map_publisher_ = pnh_.advertise("/kiss/local_map", queue_size_);
- }
- // Initialize the transform buffer
- tf2_buffer_.setUsingDedicatedThread(true);
- path_msg_.header.frame_id = odom_frame_;
-
- // publish odometry msg
- ROS_INFO("KISS-ICP ROS 1 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, ros::Time(0), &err_msg)) {
- try {
- auto tf = tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0));
- return tf2::transformToSophus(tf);
- } catch (tf2::TransformException &ex) {
- ROS_WARN("%s", ex.what());
- }
- }
- ROS_WARN("Failed to find tf between %s and %s. Reason=%s", target_frame.c_str(),
- source_frame.c_str(), err_msg.c_str());
- return {};
-}
-
-void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg) {
- const auto cloud_frame_id = msg->header.frame_id;
- const auto points = PointCloud2ToEigen(msg);
- const auto timestamps = [&]() -> std::vector {
- if (!config_.deskew) return {};
- return 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] = odometry_.RegisterFrame(points, timestamps);
-
- // Compute the pose using KISS, ego-centric to the LiDAR
- const Sophus::SE3d kiss_pose = odometry_.poses().back();
-
- // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
- const auto pose = [&]() -> Sophus::SE3d {
- if (egocentric_estimation) return kiss_pose;
- const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
- 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 ros::Time &stamp,
- const std::string &cloud_frame_id) {
- // Header for point clouds and stuff seen from desired odom_frame
-
- // Broadcast the tf
- if (publish_odom_tf_) {
- geometry_msgs::TransformStamped transform_msg;
- transform_msg.header.stamp = stamp;
- transform_msg.header.frame_id = odom_frame_;
- transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_;
- transform_msg.transform = tf2::sophusToTransform(pose);
- tf_broadcaster_.sendTransform(transform_msg);
- }
-
- // publish trajectory msg
- geometry_msgs::PoseStamped pose_msg;
- pose_msg.header.stamp = stamp;
- pose_msg.header.frame_id = odom_frame_;
- pose_msg.pose = tf2::sophusToPose(pose);
- path_msg_.poses.push_back(pose_msg);
- traj_publisher_.publish(path_msg_);
-
- // publish odometry msg
- nav_msgs::Odometry odom_msg;
- odom_msg.header.stamp = stamp;
- odom_msg.header.frame_id = odom_frame_;
- odom_msg.pose.pose = tf2::sophusToPose(pose);
- odom_publisher_.publish(odom_msg);
-}
-
-void OdometryServer::PublishClouds(const std::vector frame,
- const std::vector keypoints,
- const ros::Time &stamp,
- const std::string &cloud_frame_id) {
- std_msgs::Header odom_header;
- odom_header.stamp = stamp;
- odom_header.frame_id = odom_frame_;
-
- // Publish map
- const auto kiss_map = odometry_.LocalMap();
-
- if (!publish_odom_tf_) {
- // debugging happens in an egocentric world
- std_msgs::Header cloud_header;
- cloud_header.stamp = stamp;
- cloud_header.frame_id = cloud_frame_id;
-
- frame_publisher_.publish(*EigenToPointCloud2(frame, cloud_header));
- kpoints_publisher_.publish(*EigenToPointCloud2(keypoints, cloud_header));
- map_publisher_.publish(*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(*EigenToPointCloud2(frame, cloud2odom, odom_header));
- kpoints_publisher_.publish(*EigenToPointCloud2(keypoints, cloud2odom, odom_header));
-
- if (!base_frame_.empty()) {
- const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
- map_publisher_.publish(*EigenToPointCloud2(kiss_map, cloud2base, odom_header));
- } else {
- map_publisher_.publish(*EigenToPointCloud2(kiss_map, odom_header));
- }
-}
-
-} // namespace kiss_icp_ros
-
-int main(int argc, char **argv) {
- ros::init(argc, argv, "kiss_icp");
- ros::NodeHandle nh;
- ros::NodeHandle nh_private("~");
-
- kiss_icp_ros::OdometryServer node(nh, nh_private);
-
- ros::spin();
-
- return 0;
-}
diff --git a/ros/ros1/OdometryServer.hpp b/ros/ros1/OdometryServer.hpp
deleted file mode 100644
index 3786844f..00000000
--- a/ros/ros1/OdometryServer.hpp
+++ /dev/null
@@ -1,96 +0,0 @@
-// MIT License
-//
-// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
-// Stachniss.
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// 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.
-#pragma once
-
-// KISS-ICP
-#include "kiss_icp/pipeline/KissICP.hpp"
-
-// ROS
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include
-
-namespace kiss_icp_ros {
-
-class OdometryServer {
-public:
- /// OdometryServer constructor
- OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &pnh);
-
-private:
- /// Register new frame
- void RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg);
-
- /// Stream the estimated pose to ROS
- void PublishOdometry(const Sophus::SE3d &pose,
- const ros::Time &stamp,
- const std::string &cloud_frame_id);
-
- /// Stream the debugging point clouds for visualization (if required)
- void PublishClouds(const std::vector frame,
- const std::vector keypoints,
- const ros::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;
-
- /// Ros node stuff
- ros::NodeHandle nh_;
- ros::NodeHandle pnh_;
- int queue_size_{1};
-
- /// Tools for broadcasting TFs.
- tf2_ros::TransformBroadcaster tf_broadcaster_;
- tf2_ros::Buffer tf2_buffer_;
- tf2_ros::TransformListener tf2_listener_;
- bool publish_odom_tf_;
- bool publish_debug_clouds_;
-
- /// Data subscribers.
- ros::Subscriber pointcloud_sub_;
-
- /// Data publishers.
- ros::Publisher odom_publisher_;
- ros::Publisher frame_publisher_;
- ros::Publisher kpoints_publisher_;
- ros::Publisher map_publisher_;
- ros::Publisher traj_publisher_;
- nav_msgs::Path path_msg_;
-
- /// KISS-ICP
- kiss_icp::pipeline::KissICP odometry_;
- kiss_icp::pipeline::KISSConfig config_;
-
- /// Global/map coordinate frame.
- std::string odom_frame_{"odom"};
- std::string base_frame_{};
-};
-
-} // namespace kiss_icp_ros
diff --git a/ros/ros1/Utils.hpp b/ros/ros1/Utils.hpp
deleted file mode 100644
index ed4d833f..00000000
--- a/ros/ros1/Utils.hpp
+++ /dev/null
@@ -1,236 +0,0 @@
-// MIT License
-//
-// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
-// Stachniss.
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// 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.
-#pragma once
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-// ROS 1 headers
-#include
-#include
-#include
-#include
-#include
-
-namespace tf2 {
-
-inline geometry_msgs::Transform sophusToTransform(const Sophus::SE3d &T) {
- geometry_msgs::Transform t;
- t.translation.x = T.translation().x();
- t.translation.y = T.translation().y();
- t.translation.z = T.translation().z();
-
- Eigen::Quaterniond q(T.so3().unit_quaternion());
- t.rotation.x = q.x();
- t.rotation.y = q.y();
- t.rotation.z = q.z();
- t.rotation.w = q.w();
-
- return t;
-}
-
-inline geometry_msgs::Pose sophusToPose(const Sophus::SE3d &T) {
- geometry_msgs::Pose t;
- t.position.x = T.translation().x();
- t.position.y = T.translation().y();
- t.position.z = T.translation().z();
-
- Eigen::Quaterniond q(T.so3().unit_quaternion());
- t.orientation.x = q.x();
- t.orientation.y = q.y();
- t.orientation.z = q.z();
- t.orientation.w = q.w();
-
- return t;
-}
-
-inline Sophus::SE3d transformToSophus(const geometry_msgs::TransformStamped &transform) {
- const auto &t = transform.transform;
- return Sophus::SE3d(
- Sophus::SE3d::QuaternionType(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z),
- Sophus::SE3d::Point(t.translation.x, t.translation.y, t.translation.z));
-}
-} // namespace tf2
-
-namespace kiss_icp_ros::utils {
-using PointCloud2 = sensor_msgs::PointCloud2;
-using PointField = sensor_msgs::PointField;
-using Header = std_msgs::Header;
-
-inline std::string FixFrameId(const std::string &frame_id) {
- return std::regex_replace(frame_id, std::regex("^/"), "");
-}
-
-inline auto GetTimestampField(const PointCloud2::ConstPtr msg) {
- PointField timestamp_field;
- for (const auto &field : msg->fields) {
- if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) {
- timestamp_field = field;
- }
- }
- if (!timestamp_field.count) {
- throw std::runtime_error("Field 't', 'timestamp', or 'time' does not exist");
- }
- return timestamp_field;
-}
-
-// Normalize timestamps from 0.0 to 1.0
-inline auto NormalizeTimestamps(const std::vector ×tamps) {
- const auto [min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend());
- const double min_timestamp = *min_it;
- const double max_timestamp = *max_it;
-
- std::vector timestamps_normalized(timestamps.size());
- std::transform(timestamps.cbegin(), timestamps.cend(), timestamps_normalized.begin(),
- [&](const auto ×tamp) {
- return (timestamp - min_timestamp) / (max_timestamp - min_timestamp);
- });
- return timestamps_normalized;
-}
-
-inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstPtr msg, const PointField &field) {
- auto extract_timestamps =
- [&msg](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector {
- const size_t n_points = msg->height * msg->width;
- std::vector timestamps;
- timestamps.reserve(n_points);
- for (size_t i = 0; i < n_points; ++i, ++it) {
- timestamps.emplace_back(static_cast(*it));
- }
- return NormalizeTimestamps(timestamps);
- };
-
- // Get timestamp field that must be one of the following : {t, timestamp, time}
- auto timestamp_field = GetTimestampField(msg);
-
- // According to the type of the timestamp == type, return a PointCloud2ConstIterator
- using sensor_msgs::PointCloud2ConstIterator;
- if (timestamp_field.datatype == PointField::UINT32) {
- return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name));
- } else if (timestamp_field.datatype == PointField::FLOAT32) {
- return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name));
- } else if (timestamp_field.datatype == PointField::FLOAT64) {
- return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name));
- }
-
- // timestamp type not supported, please open an issue :)
- throw std::runtime_error("timestamp field type not supported");
-}
-
-inline std::unique_ptr CreatePointCloud2Msg(const size_t n_points,
- const Header &header,
- bool timestamp = false) {
- auto cloud_msg = std::make_unique();
- sensor_msgs::PointCloud2Modifier modifier(*cloud_msg);
- cloud_msg->header = header;
- cloud_msg->header.frame_id = FixFrameId(cloud_msg->header.frame_id);
- cloud_msg->fields.clear();
- int offset = 0;
- offset = addPointField(*cloud_msg, "x", 1, PointField::FLOAT32, offset);
- offset = addPointField(*cloud_msg, "y", 1, PointField::FLOAT32, offset);
- offset = addPointField(*cloud_msg, "z", 1, PointField::FLOAT32, offset);
- offset += sizeOfPointField(PointField::FLOAT32);
- if (timestamp) {
- // assuming timestamp on a velodyne fashion for now (between 0.0 and 1.0)
- offset = addPointField(*cloud_msg, "time", 1, PointField::FLOAT64, offset);
- offset += sizeOfPointField(PointField::FLOAT64);
- }
-
- // Resize the point cloud accordingly
- cloud_msg->point_step = offset;
- cloud_msg->row_step = cloud_msg->width * cloud_msg->point_step;
- cloud_msg->data.resize(cloud_msg->height * cloud_msg->row_step);
- modifier.resize(n_points);
- return cloud_msg;
-}
-
-inline void FillPointCloud2XYZ(const std::vector &points, PointCloud2 &msg) {
- sensor_msgs::PointCloud2Iterator msg_x(msg, "x");
- sensor_msgs::PointCloud2Iterator msg_y(msg, "y");
- sensor_msgs::PointCloud2Iterator msg_z(msg, "z");
- for (size_t i = 0; i < points.size(); i++, ++msg_x, ++msg_y, ++msg_z) {
- const Eigen::Vector3d &point = points[i];
- *msg_x = point.x();
- *msg_y = point.y();
- *msg_z = point.z();
- }
-}
-
-inline void FillPointCloud2Timestamp(const std::vector ×tamps, PointCloud2 &msg) {
- sensor_msgs::PointCloud2Iterator msg_t(msg, "time");
- for (size_t i = 0; i < timestamps.size(); i++, ++msg_t) *msg_t = timestamps[i];
-}
-
-inline std::vector GetTimestamps(const PointCloud2::ConstPtr msg) {
- auto timestamp_field = GetTimestampField(msg);
-
- // Extract timestamps from cloud_msg
- std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field);
-
- return timestamps;
-}
-
-inline std::vector PointCloud2ToEigen(const PointCloud2::ConstPtr msg) {
- std::vector points;
- points.reserve(msg->height * msg->width);
- sensor_msgs::PointCloud2ConstIterator msg_x(*msg, "x");
- sensor_msgs::PointCloud2ConstIterator msg_y(*msg, "y");
- sensor_msgs::PointCloud2ConstIterator msg_z(*msg, "z");
- for (size_t i = 0; i < msg->height * msg->width; ++i, ++msg_x, ++msg_y, ++msg_z) {
- points.emplace_back(*msg_x, *msg_y, *msg_z);
- }
- return points;
-}
-
-inline std::unique_ptr EigenToPointCloud2(const std::vector &points,
- const Header &header) {
- auto msg = CreatePointCloud2Msg(points.size(), header);
- FillPointCloud2XYZ(points, *msg);
- return msg;
-}
-
-inline std::unique_ptr EigenToPointCloud2(const std::vector &points,
- const Sophus::SE3d &T,
- const Header &header) {
- std::vector points_t;
- points_t.resize(points.size());
- std::transform(points.cbegin(), points.cend(), points_t.begin(),
- [&](const auto &point) { return T * point; });
- return EigenToPointCloud2(points_t, header);
-}
-
-inline std::unique_ptr EigenToPointCloud2(const std::vector &points,
- const std::vector ×tamps,
- const Header &header) {
- auto msg = CreatePointCloud2Msg(points.size(), header, true);
- FillPointCloud2XYZ(points, *msg);
- FillPointCloud2Timestamp(timestamps, *msg);
- return msg;
-}
-} // namespace kiss_icp_ros::utils
diff --git a/ros/rviz/kiss_icp_ros2.rviz b/ros/rviz/kiss_icp.rviz
similarity index 100%
rename from ros/rviz/kiss_icp_ros2.rviz
rename to ros/rviz/kiss_icp.rviz
diff --git a/ros/rviz/kiss_icp_ros1.rviz b/ros/rviz/kiss_icp_ros1.rviz
deleted file mode 100644
index d313da5f..00000000
--- a/ros/rviz/kiss_icp_ros1.rviz
+++ /dev/null
@@ -1,226 +0,0 @@
-Panels:
- - Class: rviz/Displays
- Help Height: 78
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /local_map1/Autocompute Value Bounds1
- Splitter Ratio: 0.5
- Tree Height: 752
- - Class: rviz/Selection
- Name: Selection
- - Class: rviz/Tool Properties
- Expanded:
- - /2D Pose Estimate1
- - /2D Nav Goal1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
- - Class: rviz/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
- - Class: rviz/Time
- Name: Time
- SyncMode: 0
- SyncSource: local_map
-Preferences:
- PromptSaveOnExit: true
-Toolbars:
- toolButtonStyle: 2
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.699999988079071
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 10
- Min Value: -10
- Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/PointCloud2
- Color: 243; 243; 0
- Color Transformer: FlatColor
- Decay Time: 0
- Enabled: true
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Min Color: 0; 0; 0
- Name: frame_deskewed
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 1
- Size (m): 0.009999999776482582
- Style: Points
- Topic: /kiss/frame
- Unreliable: false
- Use Fixed Frame: true
- Use rainbow: false
- Value: true
- - Alpha: 0.5
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 29.128881454467773
- Min Value: -13.79653549194336
- Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/PointCloud2
- Color: 255; 255; 255
- Color Transformer: AxisColor
- Decay Time: 0
- Enabled: true
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Min Color: 0; 0; 0
- Name: local_map
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 1
- Size (m): 0.009999999776482582
- Style: Points
- Topic: /kiss/local_map
- Unreliable: true
- Use Fixed Frame: true
- Use rainbow: true
- Value: true
- - Alpha: 1
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 18.019136428833008
- Min Value: -5.5211381912231445
- Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/PointCloud2
- Color: 255; 255; 255
- Color Transformer: FlatColor
- Decay Time: 0
- Enabled: false
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Min Color: 0; 0; 0
- Name: keypoints
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 3
- Size (m): 0.009999999776482582
- Style: Points
- Topic: /odometry_node/keypoints
- Unreliable: false
- Use Fixed Frame: true
- Use rainbow: true
- Value: false
- - Alpha: 1
- Class: rviz/Axes
- Enabled: true
- Length: 3
- Name: odometry_frame
- Radius: 0.10000000149011612
- Reference Frame: odom
- Show Trail: false
- Value: true
- - Alpha: 1
- Class: rviz/Axes
- Enabled: true
- Length: 3
- Name: child_frame
- Radius: 0.10000000149011612
- Reference Frame: base
- Show Trail: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 30; 85; 157
- Enabled: true
- Head Diameter: 0.30000001192092896
- Head Length: 0.20000000298023224
- Length: 1
- Line Style: Billboards
- Line Width: 0.20000000298023224
- Name: trajectory
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 134; 44; 134
- Pose Style: None
- Queue Size: 10
- Radius: 0.20000000298023224
- Shaft Diameter: 0.10000000149011612
- Shaft Length: 0.10000000149011612
- Topic: /kiss/trajectory
- Unreliable: false
- Value: true
- Enabled: true
- Global Options:
- Background Color: 0; 0; 0
- Default Light: true
- Fixed Frame: odom
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz/Interact
- Hide Inactive Objects: true
- - Class: rviz/MoveCamera
- - Class: rviz/Select
- - Class: rviz/FocusCamera
- - Class: rviz/Measure
- - Class: rviz/SetInitialPose
- Theta std deviation: 0.2617993950843811
- Topic: /initialpose
- X std deviation: 0.5
- Y std deviation: 0.5
- - Class: rviz/SetGoal
- Topic: /move_base_simple/goal
- - Class: rviz/PublishPoint
- Single click: true
- Topic: /clicked_point
- Value: true
- Views:
- Current:
- Class: rviz/Orbit
- Distance: 73.19767761230469
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Field of View: 0.7853981852531433
- Focal Point:
- X: -0.9206065535545349
- Y: -1.2210272550582886
- Z: 4.959392547607422
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 0.6503981947898865
- Target Frame:
- Yaw: 4.798571586608887
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: true
- Height: 1043
- Hide Left Dock: true
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000027200000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000359fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000359000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: false
- Width: 1920
- X: 3840
- Y: 0
diff --git a/ros/ros2/OdometryServer.cpp b/ros/src/OdometryServer.cpp
similarity index 100%
rename from ros/ros2/OdometryServer.cpp
rename to ros/src/OdometryServer.cpp
diff --git a/ros/ros2/OdometryServer.hpp b/ros/src/OdometryServer.hpp
similarity index 100%
rename from ros/ros2/OdometryServer.hpp
rename to ros/src/OdometryServer.hpp
diff --git a/ros/ros2/Utils.hpp b/ros/src/Utils.hpp
similarity index 100%
rename from ros/ros2/Utils.hpp
rename to ros/src/Utils.hpp