Skip to content

Commit

Permalink
refactor(autoware_pointcloud_preprocessor): rework concatenate_pointc…
Browse files Browse the repository at this point in the history
…loud and time_synchronizer_node parameters (autowarefoundation#8509)

* feat: rewort concatenate pointclouds and time synchronizer parameter

Signed-off-by: vividf <[email protected]>

* chore: fix launch files

Signed-off-by: vividf <[email protected]>

* chore: fix schema

Signed-off-by: vividf <[email protected]>

* chore: fix schema

Signed-off-by: vividf <[email protected]>

* chore: fix integer and number default value in schema

Signed-off-by: vividf <[email protected]>

* chore: add boundary

Signed-off-by: vividf <[email protected]>

---------

Signed-off-by: vividf <[email protected]>
  • Loading branch information
vividf authored and a-maumau committed Sep 2, 2024
1 parent 6cfdd82 commit b895af0
Show file tree
Hide file tree
Showing 11 changed files with 205 additions and 22 deletions.
2 changes: 1 addition & 1 deletion sensing/autoware_pointcloud_preprocessor/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter
ament_auto_add_library(pointcloud_preprocessor_filter SHARED
src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
src/concatenate_data/concatenate_pointclouds.cpp
src/time_synchronizer/time_synchronizer_nodelet.cpp
src/time_synchronizer/time_synchronizer_node.cpp
src/crop_box_filter/crop_box_filter_nodelet.cpp
src/downsample_filter/voxel_grid_downsample_filter_node.cpp
src/downsample_filter/random_downsample_filter_nodelet.cpp
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
/**:
ros__parameters:
output_frame: base_link
input_topics: [
"/sensing/lidar/left/pointcloud_before_sync",
"/sensing/lidar/right/pointcloud_before_sync",
"/sensing/lidar/top/pointcloud_before_sync"
]
max_queue_size: 5
timeout_sec: 0.033
input_offset: [0.0 ,0.0 ,0.0]
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
/**:
ros__parameters:
input_twist_topic_type: twist
output_frame: base_link
keep_input_frame_in_synchronized_pointcloud: false
input_topics: [
"/sensing/lidar/left/pointcloud_before_sync",
"/sensing/lidar/right/pointcloud_before_sync",
"/sensing/lidar/top/pointcloud_before_sync"
]
synchronized_pointcloud_postfix: pointcloud
max_queue_size: 5
timeout_sec: 0.033
input_offset: [0.0, 0.0, 0.0]
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.
// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.
// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -49,8 +49,8 @@
*
*/

#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_
#define AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_
#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODE_HPP_
#define AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODE_HPP_

#include <deque>
#include <map>
Expand Down Expand Up @@ -182,4 +182,4 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node

} // namespace autoware::pointcloud_preprocessor

#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_
#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODE_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
<launch>
<arg name="output" default="/sensing/lidar/concatenated/pointcloud"/>
<!-- Parameter -->
<arg name="param_file" default="$(find-pkg-share autoware_pointcloud_preprocessor)/config/concatenate_pointclouds.param.yaml"/>
<node pkg="autoware_pointcloud_preprocessor" exec="concatenate_pointclouds_node" name="concatenate_pointclouds_node" output="screen">
<remap from="output" to="$(var output)"/>
<param from="$(var param_file)"/>
</node>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
<launch>
<arg name="input/twist" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
<!-- Parameter -->
<arg name="param_file" default="$(find-pkg-share autoware_pointcloud_preprocessor)/config/time_synchronizer_node.param.yaml"/>
<node pkg="autoware_pointcloud_preprocessor" exec="time_synchronizer_node" name="time_synchronizer_node" output="screen">
<remap from="~/input/twist" to="$(var input/twist)"/>
<param from="$(var param_file)"/>
</node>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
{
"$schema": "http://json-schema.org/draft-07/schema#",
"title": "Parameters for Concatenate Pointclouds Node",
"type": "object",
"definitions": {
"concatenate_pointclouds": {
"type": "object",
"properties": {
"timeout_sec": {
"type": "number",
"default": "0.1",
"minimum": 0,
"description": "Tolerance of time to publish the next pointcloud [s]. When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed."
},
"input_offset": {
"type": "array",
"items": {
"type": "number"
},
"default": [],
"description": "This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers."
},
"output_frame": {
"type": "string",
"default": "base_link",
"description": "Output frame id."
},
"input_topics": {
"type": "array",
"items": {
"type": "string"
},
"default": [],
"description": "List of input topics."
},
"max_queue_size": {
"type": "integer",
"default": "5",
"minimum": 1,
"description": "Max queue size of input/output topics."
}
},
"required": ["timeout_sec", "input_offset", "output_frame", "input_topics", "max_queue_size"]
}
},
"properties": {
"/**": {
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/concatenate_pointclouds"
}
},
"required": ["ros__parameters"]
}
},
"required": ["/**"]
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
{
"$schema": "http://json-schema.org/draft-07/schema#",
"title": "Parameters for Time synchronizer Node",
"type": "object",
"definitions": {
"time_synchronizer": {
"type": "object",
"properties": {
"timeout_sec": {
"type": "number",
"default": "0.033",
"minimum": 0,
"description": "Tolerance of time to publish the next pointcloud [s]. When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed."
},
"input_offset": {
"type": "array",
"items": {
"type": "number"
},
"default": [],
"description": "This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers."
},
"input_twist_topic_type": {
"type": "string",
"enum": ["twist", "odom"],
"default": "twist",
"description": "Topic type for twist. Currently supports 'twist' or 'odom'."
},
"output_frame": {
"type": "string",
"default": "base_link",
"description": "Output frame."
},
"keep_input_frame_in_synchronized_pointcloud": {
"type": "boolean",
"default": true,
"description": "Flag to indicate if input frame should be kept in synchronized point cloud."
},
"input_topics": {
"type": "array",
"items": {
"type": "string"
},
"default": [],
"description": "List of input topics."
},
"synchronized_pointcloud_postfix": {
"type": "string",
"default": "pointcloud",
"description": "Postfix for the synchronized point cloud."
},
"max_queue_size": {
"type": "integer",
"default": "5",
"minimum": 1,
"description": "Max queue size of input/output topics."
}
},
"required": [
"timeout_sec",
"input_offset",
"input_twist_topic_type",
"output_frame",
"keep_input_frame_in_synchronized_pointcloud",
"input_topics",
"synchronized_pointcloud_postfix",
"max_queue_size"
]
}
},
"properties": {
"/**": {
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/time_synchronizer"
}
},
"required": ["ros__parameters"]
}
},
"required": ["/**"]
}
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.
// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -49,12 +49,12 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent(

// Set parameters
{
output_frame_ = static_cast<std::string>(declare_parameter("output_frame", ""));
output_frame_ = declare_parameter<std::string>("output_frame");
if (output_frame_.empty()) {
RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!");
return;
}
declare_parameter("input_topics", std::vector<std::string>());
declare_parameter<std::vector<std::string>>("input_topics");
input_topics_ = get_parameter("input_topics").as_string_array();
if (input_topics_.empty()) {
RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!");
Expand All @@ -66,11 +66,11 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent(
}

// Optional parameters
maximum_queue_size_ = static_cast<int>(declare_parameter("max_queue_size", 5));
maximum_queue_size_ = declare_parameter<int64_t>("max_queue_size");
/** input pointclouds should be */
timeout_sec_ = static_cast<double>(declare_parameter("timeout_sec", 0.033));
timeout_sec_ = declare_parameter<double>("timeout_sec");

input_offset_ = declare_parameter("input_offset", std::vector<double>{});
input_offset_ = declare_parameter<std::vector<double>>("input_offset");
if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) {
RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets.");
return;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.
// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -21,7 +21,7 @@
* @author Yoshi Ri
*/

#include "autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp"
#include "autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp"

#include <pcl_ros/transforms.hpp>

Expand Down Expand Up @@ -57,14 +57,14 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
// Set parameters
std::string synchronized_pointcloud_postfix;
{
output_frame_ = static_cast<std::string>(declare_parameter("output_frame", ""));
output_frame_ = declare_parameter<std::string>("output_frame");
keep_input_frame_in_synchronized_pointcloud_ =
static_cast<bool>(declare_parameter("keep_input_frame_in_synchronized_pointcloud", false));
declare_parameter<bool>("keep_input_frame_in_synchronized_pointcloud");
if (output_frame_.empty() && !keep_input_frame_in_synchronized_pointcloud_) {
RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!");
return;
}
declare_parameter("input_topics", std::vector<std::string>());
declare_parameter<std::vector<std::string>>("input_topics");
input_topics_ = get_parameter("input_topics").as_string_array();
if (input_topics_.empty()) {
RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!");
Expand All @@ -76,13 +76,12 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent(
}
// output topic name postfix
synchronized_pointcloud_postfix =
declare_parameter("synchronized_pointcloud_postfix", "pointcloud");
declare_parameter<std::string>("synchronized_pointcloud_postfix");

// Optional parameters
maximum_queue_size_ = static_cast<int>(declare_parameter("max_queue_size", 5));
timeout_sec_ = static_cast<double>(declare_parameter("timeout_sec", 0.1));

input_offset_ = declare_parameter("input_offset", std::vector<double>{});
maximum_queue_size_ = declare_parameter<int64_t>("max_queue_size");
timeout_sec_ = declare_parameter<double>("timeout_sec");
input_offset_ = declare_parameter<std::vector<double>>("input_offset");

// If input_offset_ is not defined, set all offsets to 0
if (input_offset_.empty()) {
Expand Down

0 comments on commit b895af0

Please sign in to comment.