diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
index 7ed5860612601..12190fb659235 100644
--- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
+++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
@@ -122,12 +122,16 @@ def create_ransac_pipeline(self):
plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
name="concatenate_data",
namespace="plane_fitting",
- remappings=[("output", "concatenated/pointcloud")],
+ remappings=[
+ ("~/input/odom", "/localization/kinematic_state"),
+ ("output", "concatenated/pointcloud"),
+ ],
parameters=[
{
"input_topics": self.ground_segmentation_param["ransac_input_topics"],
"output_frame": LaunchConfiguration("base_frame"),
"timeout_sec": 1.0,
+ "input_twist_topic_type": "odom",
}
],
extra_arguments=[
@@ -432,11 +436,15 @@ def get_additional_lidars_concatenated_component(input_topics, output_topic):
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
name="concatenate_data",
- remappings=[("output", output_topic)],
+ remappings=[
+ ("~/input/odom", "/localization/kinematic_state"),
+ ("output", output_topic),
+ ],
parameters=[
{
"input_topics": input_topics,
"output_frame": LaunchConfiguration("base_frame"),
+ "input_twist_topic_type": "odom",
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
@@ -448,11 +456,15 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics,
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
name="concatenate_no_ground_data",
- remappings=[("output", output_topic)],
+ remappings=[
+ ("~/input/odom", "/localization/kinematic_state"),
+ ("output", output_topic),
+ ],
parameters=[
{
"input_topics": input_topics,
"output_frame": LaunchConfiguration("base_frame"),
+ "input_twist_topic_type": "odom",
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md
index a3b5268815f24..74b4f3df17615 100644
--- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md
+++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md
@@ -42,6 +42,7 @@ The figure below represents the reception time of each sensor data and how it is
| `timeout_sec` | double | 0.1 | tolerance of time to publish 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` | vector of double | [] | 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.
For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). |
| `publish_synchronized_pointcloud` | bool | false | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `_synchronized`. |
+| `input_twist_topic_type` | std::string | twist | Topic type for twist. Currently support `twist` or `odom`. |
## Actual Usage
diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
index f5d55a2ebcbf7..be96aa94382dd 100644
--- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
+++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
@@ -71,6 +71,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -88,6 +89,7 @@ namespace pointcloud_preprocessor
{
using autoware_point_types::PointXYZI;
using point_cloud_msg_wrapper::PointCloud2Modifier;
+
/** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data
* synchronizer: it listens for a set of input PointCloud messages on the same topic,
* checks their timestamps, and concatenates their fields together into a single
@@ -131,10 +133,13 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
std::vector::SharedPtr> filters_;
rclcpp::Subscription::SharedPtr sub_twist_;
+ rclcpp::Subscription::SharedPtr sub_odom_;
rclcpp::TimerBase::SharedPtr timer_;
diagnostic_updater::Updater updater_{this};
+ const std::string input_twist_topic_type_;
+
/** \brief Output TF frame the concatenated points should be transformed to. */
std::string output_frame_;
@@ -170,6 +175,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr,
const std::string & topic_name);
void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input);
+ void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input);
void timer_callback();
void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py
index 6b1fc0f5bd976..0d65b45b1b3de 100644
--- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py
+++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py
@@ -38,13 +38,17 @@ def launch_setup(context, *args, **kwargs):
package=pkg,
plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
name="sync_and_concatenate_filter",
- remappings=[("output", "points_raw/concatenated")],
+ remappings=[
+ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
+ ("output", "points_raw/concatenated"),
+ ],
parameters=[
{
"input_topics": LaunchConfiguration("input_points_raw_list"),
"output_frame": LaunchConfiguration("tf_output_frame"),
"approximate_sync": True,
"publish_synchronized_pointcloud": False,
+ "input_twist_topic_type": "twist",
}
],
)
diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
index d88665a4c4c98..f3d69b766aa7a 100644
--- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
+++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
@@ -69,7 +69,8 @@ namespace pointcloud_preprocessor
{
PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent(
const rclcpp::NodeOptions & node_options)
-: Node("point_cloud_concatenator_component", node_options)
+: Node("point_cloud_concatenator_component", node_options),
+ input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist"))
{
// initialize debug tool
{
@@ -164,10 +165,24 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
filters_[d] = this->create_subscription(
input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb);
}
- auto twist_cb = std::bind(
- &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, std::placeholders::_1);
- sub_twist_ = this->create_subscription(
- "~/input/twist", rclcpp::QoS{100}, twist_cb);
+
+ if (input_twist_topic_type_ == "twist") {
+ auto twist_cb = std::bind(
+ &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this,
+ std::placeholders::_1);
+ sub_twist_ = this->create_subscription(
+ "~/input/twist", rclcpp::QoS{100}, twist_cb);
+ } else if (input_twist_topic_type_ == "odom") {
+ auto odom_cb = std::bind(
+ &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this,
+ std::placeholders::_1);
+ sub_odom_ = this->create_subscription(
+ "~/input/odom", rclcpp::QoS{100}, odom_cb);
+ } else {
+ RCLCPP_ERROR_STREAM(
+ get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_);
+ throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_);
+ }
}
// Transformed Raw PointCloud2 Publisher
@@ -227,8 +242,19 @@ Eigen::Matrix4f
PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp(
const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp)
{
- // return identity if no twist is available or old_stamp is newer than new_stamp
- if (twist_ptr_queue_.empty() || old_stamp > new_stamp) {
+ // return identity if no twist is available
+ if (twist_ptr_queue_.empty()) {
+ RCLCPP_WARN_STREAM_THROTTLE(
+ get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(),
+ "No twist is available. Please confirm twist topic and timestamp");
+ return Eigen::Matrix4f::Identity();
+ }
+
+ // return identity if old_stamp is newer than new_stamp
+ if (old_stamp > new_stamp) {
+ RCLCPP_WARN_STREAM_THROTTLE(
+ get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(),
+ "old_stamp is newer than new_stamp,");
return Eigen::Matrix4f::Identity();
}
@@ -558,6 +584,32 @@ void PointCloudConcatenateDataSynchronizerComponent::twist_callback(
twist_ptr_queue_.push_back(twist_ptr);
}
+void PointCloudConcatenateDataSynchronizerComponent::odom_callback(
+ const nav_msgs::msg::Odometry::ConstSharedPtr input)
+{
+ // if rosbag restart, clear buffer
+ if (!twist_ptr_queue_.empty()) {
+ if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) {
+ twist_ptr_queue_.clear();
+ }
+ }
+
+ // pop old data
+ while (!twist_ptr_queue_.empty()) {
+ if (
+ rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) >
+ rclcpp::Time(input->header.stamp)) {
+ break;
+ }
+ twist_ptr_queue_.pop_front();
+ }
+
+ auto twist_ptr = std::make_shared();
+ twist_ptr->header = input->header;
+ twist_ptr->twist = input->twist.twist;
+ twist_ptr_queue_.push_back(twist_ptr);
+}
+
void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus(
diagnostic_updater::DiagnosticStatusWrapper & stat)
{