Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Backport tf multisensor fix #245

Merged
merged 7 commits into from
Oct 20, 2023
Merged

Backport tf multisensor fix #245

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
4b09f95
Build system changes for tf fix
playertr Oct 18, 2023
aac39bd
Modify params for tf fix
playertr Oct 18, 2023
2265981
Add ROS 1 tf fixes similar to ROS 2
playertr Oct 18, 2023
de0b9a7
Update rviz config
playertr Oct 18, 2023
a2f7690
Remove unused debug publishers
playertr Oct 18, 2023
f1bf795
Remove unnecessary smart pointers
playertr Oct 18, 2023
4dd7d27
Update ROS 1 to match ROS 2 changes
playertr Oct 19, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 7 additions & 2 deletions ros/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,11 +47,13 @@ if("$ENV{ROS_VERSION}" STREQUAL "1")
COMPONENTS geometry_msgs
nav_msgs
sensor_msgs
geometry_msgs
roscpp
rosbag
std_msgs
tf2
tf2_ros)
tf2_ros
Sophus)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

From this one, I'm not quite sure what is the "best" practice... KISS-ICP has its own, isolated and self-contained build system... This means that the only way you couldn't "link" against on of its dependencies is when you copy-paste by hand some source files.

On the other hand, if we add sophus here, then, we should also add eigen, TBB, etc...

Do you have a better idea for this?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmmm, that's a good question. I added the Sophus dependency because I got a linker error for Sophus when doing the following build process for ROS 1:

cd ~/catkin_ws/src
git clone https://github.com/PRBonn/kiss-icp
cd ~/catkin_ws
rosdep install --from-paths src --ignore-src -r -y
catkin build
# the compiler initially complained that Sophus could not be found
# but worked once I added the dependency to CMakeLists.txt and package.xml

I think that, because we explicitly include Sophus and Eigen headers in the OdometryServer code, we should have a dependency on these in package.xml and CMakeLists.txt. That way, if kiss-icp-cpp ever somehow removed its dependencies on these packages in the future (unlikely) then the ROS packages would still compile. It feels risky to include a dependency's dependency based on prior knowledge of the build process without explicitly adding it to the package.xml.

(I'm keen to know if there's a better way to do this too.)

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Linking error is unlikely since sophus is a header-only library. On the other hand, lets take for example tf2_ros, just because is the one above:
https://github.com/ros/geometry2/blob/309814fd7d55d874f9bac40b2cc8669a5b86f3c5/tf2_ros/CMakeLists.txt#L9

We don't need to include actionlib as our dependency. That's the whole magic of build systems.

On the other other hand, I also agree that just because we explicitly include the headers in the Odometryserver, that injects a hard dependency.... so it might be cleaner to explicitly put it there. My problem is with ROS-based build systems, that's if you put sophus on the package.xml and it's not in your system, then the build should fail. In contrast, KISS will fetch this at build time...

I'm puzzled about which is the best approach for this case. I will meditate and be back at this PR

catkin_package()

# ROS 1 node
Expand All @@ -71,6 +73,7 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2")
find_package(rclcpp_components REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(sophus REQUIRED)

# ROS 2 node
add_library(odometry_component SHARED ros2/OdometryServer.cpp)
Expand All @@ -84,7 +87,9 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2")
rclcpp_components
nav_msgs
sensor_msgs
tf2_ros)
geometry_msgs
tf2_ros
Sophus)

rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE odometry_node)

Expand Down
9 changes: 4 additions & 5 deletions ros/launch/odometry.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,10 +4,9 @@
<arg name="bagfile" default=""/>
<arg name="visualize" default="true"/>
<arg name="odom_frame" default="odom"/>
<arg name="child_frame" default="base_link"/>
<arg name="base_frame" default=""/>
<arg name="topic" default=""/>
<arg name="publish_odom_tf" default="true"/>
<arg name="publish_alias_tf" default="true"/>
<arg name="publish_odom_tf" default="false"/>

<!-- KISS-ICP paramaters -->
<arg name="deskew" default="false"/>
Expand All @@ -20,9 +19,9 @@
<!-- ROS params -->
<remap from="pointcloud_topic" to="$(arg topic)"/>
<param name="odom_frame" value="$(arg odom_frame)"/>
<param name="child_frame" value="$(arg child_frame)"/>
<param name="base_frame" value="$(arg base_frame)"/>
<param name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
<param name="publish_alias_tf" value="$(arg publish_alias_tf)"/>
<param name="visualize" value="$(arg visualize)"/>
<!-- KISS-ICP params -->
<param name="max_range" value="$(arg max_range)"/>
<param name="min_range" value="$(arg min_range)"/>
Expand Down
1 change: 1 addition & 0 deletions ros/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>sophus</depend>

<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
Expand Down
172 changes: 105 additions & 67 deletions ros/ros1/OdometryServer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,11 +51,11 @@ using utils::GetTimestamps;
using utils::PointCloud2ToEigen;

OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &pnh)
: nh_(nh), pnh_(pnh) {
pnh_.param("child_frame", child_frame_, child_frame_);
: 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_alias_tf", publish_alias_tf_, true);
pnh_.param("publish_odom_tf", publish_odom_tf_, true);
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);
Expand All @@ -76,102 +76,140 @@ OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle
&OdometryServer::RegisterFrame, this);

// Initialize publishers
odom_publisher_ = pnh_.advertise<nav_msgs::Odometry>("odometry", queue_size_);
frame_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("frame", queue_size_);
kpoints_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("keypoints", queue_size_);
map_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("local_map", queue_size_);

// Initialize trajectory publisher
path_msg_.header.frame_id = odom_frame_;
traj_publisher_ = pnh_.advertise<nav_msgs::Path>("trajectory", queue_size_);

// Broadcast a static transformation that links with identity the specified base link to the
// pointcloud_frame, basically to always be able to visualize the frame in rviz
if (publish_alias_tf_ && child_frame_ != "base_link") {
static tf2_ros::StaticTransformBroadcaster br;
geometry_msgs::TransformStamped alias_transform_msg;
alias_transform_msg.header.stamp = ros::Time::now();
alias_transform_msg.transform.translation.x = 0.0;
alias_transform_msg.transform.translation.y = 0.0;
alias_transform_msg.transform.translation.z = 0.0;
alias_transform_msg.transform.rotation.x = 0.0;
alias_transform_msg.transform.rotation.y = 0.0;
alias_transform_msg.transform.rotation.z = 0.0;
alias_transform_msg.transform.rotation.w = 1.0;
alias_transform_msg.header.frame_id = child_frame_;
alias_transform_msg.child_frame_id = "base_link";
br.sendTransform(alias_transform_msg);
odom_publisher_ = pnh_.advertise<nav_msgs::Odometry>("/kiss/odometry", queue_size_);
traj_publisher_ = pnh_.advertise<nav_msgs::Path>("/kiss/trajectory", queue_size_);
if (publish_debug_clouds_) {
frame_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("/kiss/frame", queue_size_);
kpoints_publisher_ =
pnh_.advertise<sensor_msgs::PointCloud2>("/kiss/keypoints", queue_size_);
map_publisher_ = pnh_.advertise<sensor_msgs::PointCloud2>("/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<double> {
if (!config_.deskew) return {};
return GetTimestamps(msg);
}();
const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
nachovizzo marked this conversation as resolved.
Show resolved Hide resolved

// Register frame, main entry point to KISS-ICP pipeline
const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps);

// PublishPose
const auto pose = odometry_.poses().back();
// 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);
}
}

// Convert from Eigen to ROS types
const Eigen::Vector3d t_current = pose.translation();
const Eigen::Quaterniond q_current = pose.unit_quaternion();
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 = msg->header.stamp;
transform_msg.header.stamp = stamp;
transform_msg.header.frame_id = odom_frame_;
transform_msg.child_frame_id = child_frame_;
transform_msg.transform.rotation.x = q_current.x();
transform_msg.transform.rotation.y = q_current.y();
transform_msg.transform.rotation.z = q_current.z();
transform_msg.transform.rotation.w = q_current.w();
transform_msg.transform.translation.x = t_current.x();
transform_msg.transform.translation.y = t_current.y();
transform_msg.transform.translation.z = t_current.z();
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.pose.orientation.x = q_current.x();
pose_msg.pose.orientation.y = q_current.y();
pose_msg.pose.orientation.z = q_current.z();
pose_msg.pose.orientation.w = q_current.w();
pose_msg.pose.position.x = t_current.x();
pose_msg.pose.position.y = t_current.y();
pose_msg.pose.position.z = t_current.z();
pose_msg.header.stamp = msg->header.stamp;
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
auto odom_msg = std::make_unique<nav_msgs::Odometry>();
odom_msg->header = pose_msg.header;
odom_msg->child_frame_id = child_frame_;
odom_msg->pose.pose = pose_msg.pose;
odom_publisher_.publish(*std::move(odom_msg));

// Publish KISS-ICP internal data, just for debugging
auto frame_header = msg->header;
frame_header.frame_id = child_frame_;
frame_publisher_.publish(*std::move(EigenToPointCloud2(frame, frame_header)));
kpoints_publisher_.publish(*std::move(EigenToPointCloud2(keypoints, frame_header)));

// Map is referenced to the odometry_frame
auto local_map_header = msg->header;
local_map_header.frame_id = odom_frame_;
map_publisher_.publish(*std::move(EigenToPointCloud2(odometry_.LocalMap(), local_map_header)));
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 Vector3dVector frame,
const Vector3dVector 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) {
Expand Down
29 changes: 25 additions & 4 deletions ros/ros1/OdometryServer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,10 @@
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <string>

namespace kiss_icp_ros {

Expand All @@ -42,34 +45,52 @@ class OdometryServer {
/// 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)
using Vector3dVector = kiss_icp::pipeline::KissICP::Vector3dVector;
void PublishClouds(const Vector3dVector frame,
const Vector3dVector 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_alias_tf_;
bool publish_debug_clouds_;

/// Data subscribers.
ros::Subscriber pointcloud_sub_;

/// Data publishers.
ros::Publisher odom_publisher_;
ros::Publisher traj_publisher_;
nav_msgs::Path path_msg_;
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 child_frame_{"base_link"};
std::string base_frame_{};
};

} // namespace kiss_icp_ros
Loading