Skip to content

Commit

Permalink
fix(pointcloud_preprocessor): organize input twist topic (autowarefou…
Browse files Browse the repository at this point in the history 
…ndation#5125)

* fix(pointcloud_preprocessor): organize input twist topic (#25)

* fix(pointcloud_preprocessor): organize input twist topic

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

* fix build bug

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* fix format error

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

* fix

Signed-off-by: kminoda <koji.minoda@tier4.jp>

---------

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* minor fixes

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

* add warning

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@kminoda @pre-commit-ci
kminoda and pre-commit-ci[bot] authored Sep 26, 2023
1 parent 19ab508 commit 68ee0f2
Show file tree
Hide file tree
Showing 5 changed files with 86 additions and 11 deletions.
18 changes: 15 additions & 3 deletions ...ion_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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=[
Expand Down Expand Up @@ -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")}],
Expand All @@ -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")}],
Expand Down
1 change: 1 addition & 0 deletions sensing/pointcloud_preprocessor/docs/concatenate-data.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]<br>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. <br> 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 `<original_msg_name>_synchronized`. |
| `input_twist_topic_type` | std::string | twist | Topic type for twist. Currently support `twist` or `odom`. |

## Actual Usage

Expand Down
6 changes: 6 additions & 0 deletions ...or/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <tier4_debug_msgs/msg/int32_stamped.hpp>
Expand All @@ -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
Expand Down Expand Up @@ -131,10 +133,13 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
std::vector<rclcpp::Subscription<PointCloud2>::SharedPtr> filters_;

rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr sub_twist_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::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_;

Expand Down Expand Up @@ -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);
Expand Down
6 changes: 5 additions & 1 deletion sensing/pointcloud_preprocessor/launch/preprocessor.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
}
],
)
Expand Down
66 changes: 59 additions & 7 deletions sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>("input_twist_topic_type", "twist"))
{
// initialize debug tool
{
Expand Down Expand Up @@ -164,10 +165,24 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
filters_[d] = this->create_subscription<sensor_msgs::msg::PointCloud2>(
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<geometry_msgs::msg::TwistWithCovarianceStamped>(
"~/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<geometry_msgs::msg::TwistWithCovarianceStamped>(
"~/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<nav_msgs::msg::Odometry>(
"~/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
Expand Down Expand Up @@ -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();
}

Expand Down Expand Up @@ -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<geometry_msgs::msg::TwistStamped>();
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)
{
Expand Down

0 comments on commit 68ee0f2

Please sign in to comment.