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

feat(autoware_utils_pcl): split package #29

Merged
merged 6 commits into from
Feb 27, 2025
Merged

feat(autoware_utils_pcl): split package #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
46266b5
feat(autoware_utils_pcl): split package
isamu-takagi Feb 21, 2025
79ddb19
add maintainer
isamu-takagi Feb 21, 2025
2081036
fix build error
isamu-takagi Feb 21, 2025
8a83320
add compatibility
isamu-takagi Feb 21, 2025
5088606
rename package
isamu-takagi Feb 26, 2025
a561762
update readme
isamu-takagi Feb 26, 2025
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
67 changes: 0 additions & 67 deletions autoware_utils/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,6 @@ The ROS module provides utilities for working with ROS messages and nodes:
- **`debug_publisher.hpp`**: A helper class for publishing debug messages with timestamps.
- **`diagnostics_interface.hpp`**: An interface for publishing diagnostic messages.
- **`logger_level_configure.hpp`**: Utility for configuring logger levels dynamically.
- **`managed_transform_buffer.hpp`**: A managed buffer for handling static and dynamic transforms.
- **`marker_helper.hpp`**: Helper functions for creating and manipulating visualization markers.
- **`msg_covariance.hpp`**: Indices for accessing covariance matrices in ROS messages.
- **`msg_operation.hpp`**: Overloaded operators for quaternion messages.
Expand All @@ -56,12 +55,10 @@ The ROS module provides utilities for working with ROS messages and nodes:
- **`processing_time_publisher.hpp`**: Publishes processing times as diagnostic messages.
- **`published_time_publisher.hpp`**: Tracks and publishes the time when messages are published.
- **`self_pose_listener.hpp`**: Listens to the self-pose of the vehicle.
- **`transform_listener.hpp`**: Manages transformation listeners.
- **`update_param.hpp`**: Updates parameters from remote nodes.
- **`uuid_helper.hpp`**: Utilities for generating and managing UUIDs.
- **`wait_for_param.hpp`**: Waits for parameters from remote nodes.
- **`debug_traits.hpp`**: Traits for identifying debug message types.
- **`pcl_conversion.hpp`**: Efficient conversion and transformation of PointCloud2 messages to PCL point clouds.

#### System Module

Expand All @@ -72,10 +69,6 @@ The system module provides low-level utilities for performance monitoring and er
- **`stop_watch.hpp`**: Measures elapsed time for profiling.
- **`time_keeper.hpp`**: Tracks and reports the processing time of various functions.

#### Transform Module

Efficient methods for transforming and manipulating point clouds.

## Usage

### Including Headers
Expand Down Expand Up @@ -140,36 +133,6 @@ int main() {

### Detailed Usage Examples

#### Transform Point Clouds with ManagedTransformBuffer

```cpp
#include "autoware_utils/ros/managed_transform_buffer.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <rclcpp/rclcpp.hpp>

int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("transform_node");

// Initialize ManagedTransformBuffer
autoware_utils::ManagedTransformBuffer transform_buffer(node, false);

// Load point cloud data
sensor_msgs::msg::PointCloud2 cloud_in; // Assume this is populated with data
sensor_msgs::msg::PointCloud2 cloud_out;

// Transform point cloud from "base_link" to "map" frame
if (transform_buffer.transform_pointcloud("map", cloud_in, cloud_out)) {
RCLCPP_INFO(node->get_logger(), "Point cloud transformed successfully.");
} else {
RCLCPP_ERROR(node->get_logger(), "Failed to transform point cloud.");
}

rclcpp::shutdown();
return 0;
}
```

#### Update Parameters Dynamically with update_param.hpp

```cpp
Expand Down Expand Up @@ -250,36 +213,6 @@ int main(int argc, char * argv[]) {
}
```

#### Efficient Point Cloud Conversion with pcl_conversion.hpp

```cpp
#include "autoware_utils/ros/pcl_conversion.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <Eigen/Core>
#include <rclcpp/rclcpp.hpp>

int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("pcl_conversion_node");

// Load point cloud data
sensor_msgs::msg::PointCloud2 cloud_in; // Assume this is populated with data
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;

// Define transformation matrix
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
// Populate transform matrix with actual values

// Convert and transform point cloud
autoware_utils::transform_point_cloud_from_ros_msg(cloud_in, pcl_cloud, transform);

rclcpp::shutdown();
return 0;
}
```

#### Handling Debug Message Types with debug_traits.hpp

```cpp
Expand Down
207 changes: 3 additions & 204 deletions autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2024 TIER IV, Inc.
// Copyright 2025 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -15,213 +15,12 @@
#ifndef AUTOWARE_UTILS__ROS__MANAGED_TRANSFORM_BUFFER_HPP_
#define AUTOWARE_UTILS__ROS__MANAGED_TRANSFORM_BUFFER_HPP_

#include "autoware_utils/ros/transform_listener.hpp"

#include <eigen3/Eigen/Core>
#include <pcl_ros/transforms.hpp>
#include <rclcpp/rclcpp.hpp>

#include <sensor_msgs/point_cloud2_iterator.hpp>

#include <pcl_conversions/pcl_conversions.h>

#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <string_view>
#include <unordered_map>
#include <utility>

namespace std
{
template <>
struct hash<std::pair<std::string, std::string>>
{
size_t operator()(const std::pair<std::string, std::string> & p) const
{
size_t h1 = std::hash<std::string>{}(p.first);
size_t h2 = std::hash<std::string>{}(p.second);
return h1 ^ (h2 << 1u);
}
};
} // namespace std
#include <autoware_utils_pcl/managed_transform_buffer.hpp>

namespace autoware_utils
{
using std::chrono_literals::operator""ms;
using Key = std::pair<std::string, std::string>;
struct PairEqual
{
bool operator()(const Key & p1, const Key & p2) const
{
return p1.first == p2.first && p1.second == p2.second;
}
};
using TFMap = std::unordered_map<Key, Eigen::Matrix4f, std::hash<Key>, PairEqual>;
constexpr std::array<std::string_view, 3> warn_frames = {"map", "odom", "world"};

class ManagedTransformBuffer
{
public:
explicit ManagedTransformBuffer(rclcpp::Node * node, const bool & has_static_tf_only)
: node_(node)
{
if (has_static_tf_only) {
get_transform_ = [this](
const std::string & target_frame, const std::string & source_frame,
Eigen::Matrix4f & eigen_transform) {
return get_static_transform(target_frame, source_frame, eigen_transform);
};
} else {
tf_listener_ = std::make_unique<autoware_utils::TransformListener>(node);
get_transform_ = [this](
const std::string & target_frame, const std::string & source_frame,
Eigen::Matrix4f & eigen_transform) {
return get_dynamic_transform(target_frame, source_frame, eigen_transform);
};
}
}

bool get_transform(
const std::string & target_frame, const std::string & source_frame,
Eigen::Matrix4f & eigen_transform)
{
return get_transform_(target_frame, source_frame, eigen_transform);
}

/**
* Transforms a point cloud from one frame to another.
*
* @param target_frame The target frame to transform the point cloud to.
* @param cloud_in The input point cloud to be transformed.
* @param cloud_out The transformed point cloud.
* @return True if the transformation is successful, false otherwise.
*/
bool transform_pointcloud(
const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & cloud_in,
sensor_msgs::msg::PointCloud2 & cloud_out)
{
if (
pcl::getFieldIndex(cloud_in, "x") == -1 || pcl::getFieldIndex(cloud_in, "y") == -1 ||
pcl::getFieldIndex(cloud_in, "z") == -1) {
RCLCPP_ERROR(node_->get_logger(), "Input pointcloud does not have xyz fields");
return false;
}
if (target_frame == cloud_in.header.frame_id) {
cloud_out = cloud_in;
return true;
}
Eigen::Matrix4f eigen_transform;
if (!get_transform(target_frame, cloud_in.header.frame_id, eigen_transform)) {
return false;
}
pcl_ros::transformPointCloud(eigen_transform, cloud_in, cloud_out);
cloud_out.header.frame_id = target_frame;
return true;
}

private:
/**
* @brief Retrieves a transform between two static frames.
*
* This function attempts to retrieve a transform between the target frame and the source frame.
* If success, the transform matrix is set to the output parameter and the function returns true.
* Otherwise, transform matrix is set to identity and the function returns false. Transform
* Listener is destroyed after function call.
*
* @param target_frame The target frame.
* @param source_frame The source frame.
* @param eigen_transform The output Eigen transform matrix. It is set to the identity if the
* transform is not found.
* @return True if the transform was successfully retrieved, false otherwise.
*/
bool get_static_transform(
const std::string & target_frame, const std::string & source_frame,
Eigen::Matrix4f & eigen_transform)
{
if (
std::find(warn_frames.begin(), warn_frames.end(), target_frame) != warn_frames.end() ||
std::find(warn_frames.begin(), warn_frames.end(), source_frame) != warn_frames.end()) {
RCLCPP_WARN(
node_->get_logger(), "Using %s -> %s transform. This may not be a static transform.",
target_frame.c_str(), source_frame.c_str());
}

auto key = std::make_pair(target_frame, source_frame);
auto key_inv = std::make_pair(source_frame, target_frame);

// Check if the transform is already in the buffer
auto it = buffer_.find(key);
if (it != buffer_.end()) {
eigen_transform = it->second;
return true;
}

// Check if the inverse transform is already in the buffer
auto it_inv = buffer_.find(key_inv);
if (it_inv != buffer_.end()) {
eigen_transform = it_inv->second.inverse();
buffer_[key] = eigen_transform;
return true;
}

// Check if transform is needed
if (target_frame == source_frame) {
eigen_transform = Eigen::Matrix4f::Identity();
buffer_[key] = eigen_transform;
return true;
}

// Get the transform from the TF tree
tf_listener_ = std::make_unique<autoware_utils::TransformListener>(node_);
auto tf = tf_listener_->get_transform(
target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration(1000ms));
tf_listener_.reset();
RCLCPP_DEBUG(
node_->get_logger(), "Trying to enqueue %s -> %s transform to static TFs buffer...",
target_frame.c_str(), source_frame.c_str());
if (tf == nullptr) {
eigen_transform = Eigen::Matrix4f::Identity();
return false;
}
pcl_ros::transformAsMatrix(*tf, eigen_transform);
buffer_[key] = eigen_transform;
return true;
}

/** @brief Retrieves a transform between two dynamic frames.
*
* This function attempts to retrieve a transform between the target frame and the source frame.
* If successful, the transformation matrix is assigned to the output parameter, and the function
* returns true. Otherwise, the transformation matrix is set to the identity and the function
* returns false.
*
* @param target_frame The target frame.
* @param source_frame The source frame.
* @param eigen_transform The output Eigen transformation matrix. It is set to the identity if the
* transform is not found.
* @return True if the transform was successfully retrieved, false otherwise.
*/
bool get_dynamic_transform(
const std::string & target_frame, const std::string & source_frame,
Eigen::Matrix4f & eigen_transform)
{
auto tf = tf_listener_->get_transform(
target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration(1000ms));
if (tf == nullptr) {
eigen_transform = Eigen::Matrix4f::Identity();
return false;
}
pcl_ros::transformAsMatrix(*tf, eigen_transform);
return true;
}

TFMap buffer_;
rclcpp::Node * const node_;
std::unique_ptr<autoware_utils::TransformListener> tf_listener_;
std::function<bool(const std::string &, const std::string &, Eigen::Matrix4f &)> get_transform_;
};
using namespace autoware_utils_pcl; // NOLINT(build/namespaces)

} // namespace autoware_utils

Expand Down
51 changes: 3 additions & 48 deletions autoware_utils/include/autoware_utils/ros/pcl_conversion.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 Tier IV, Inc.
// Copyright 2025 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -15,57 +15,12 @@
#ifndef AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_
#define AUTOWARE_UTILS__ROS__PCL_CONVERSION_HPP_

#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

#include <vector>
#include <autoware_utils_pcl/pcl_conversion.hpp>

namespace autoware_utils
{
/**
* @brief a faster implementation of converting sensor_msgs::msg::PointCloud2 to
* pcl::PointCloud<pcl::PointXYZ> and transform the cloud
*
* @tparam Scalar
* @param cloud input PointCloud2 message
* @param pcl_cloud output transformed pcl cloud
* @param transform eigen transformation matrix
*/
template <typename Scalar>
void transform_point_cloud_from_ros_msg(
const sensor_msgs::msg::PointCloud2 & cloud, pcl::PointCloud<pcl::PointXYZ> & pcl_cloud,
const Eigen::Matrix<Scalar, 4, 4> & transform)
{
// Copy info fields
pcl_conversions::toPCL(cloud.header, pcl_cloud.header);
pcl_cloud.width = cloud.width;
pcl_cloud.height = cloud.height;
pcl_cloud.is_dense = cloud.is_dense == 1;

pcl::MsgFieldMap field_map;
std::vector<pcl::PCLPointField> msg_fields;
pcl_conversions::toPCL(cloud.fields, msg_fields);
pcl::createMapping<pcl::PointXYZ>(msg_fields, field_map);

// transform point data
std::uint32_t num_points = cloud.width * cloud.height;
pcl_cloud.points.resize(num_points);
std::uint8_t * cloud_data = reinterpret_cast<std::uint8_t *>(&pcl_cloud.points[0]);
pcl::detail::Transformer<Scalar> tf(transform);

for (std::uint32_t row = 0; row < cloud.height; ++row) {
const std::uint8_t * row_data = &cloud.data[row * cloud.row_step];
for (std::uint32_t col = 0; col < cloud.width; ++col) {
const std::uint8_t * msg_data = row_data + col * cloud.point_step;
for (const pcl::detail::FieldMapping & mapping : field_map) {
const float * msg_ptr = reinterpret_cast<const float *>(msg_data);
float * pcl_ptr = reinterpret_cast<float *>(cloud_data + mapping.struct_offset);
tf.se3(msg_ptr, pcl_ptr);
}
cloud_data += sizeof(pcl::PointXYZ);
}
}
}
using namespace autoware_utils_pcl; // NOLINT(build/namespaces)

} // namespace autoware_utils

Expand Down
Loading