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.

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(aip_x2_gen2_launch): update launch to run new concatenate node #386

Merged
merged 6 commits into from
Feb 5, 2025

Conversation

TomohitoAndo
Copy link
Contributor

Description

I updated launch and added the config file to run the new concatenate node.
These changes were made in this PR originally.

Additionally, I changed the output topic name of pointcloud_preprocessor from pointcloud to pointcloud_before_sync

Tests performed

I confirmed concatenate node works fine in logging_simulator.
image

@@ -0,0 +1,28 @@
/**:
Copy link
Contributor

@vividf vividf Feb 5, 2025

Choose a reason for hiding this comment

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

This parameter config might be wrong because these parameters are for x2 gen1.

Could you run the new algorithm with the parameters

/**:
  ros__parameters:
      debug_mode: false
      has_static_tf_only: false
      rosbag_length: 10.0
      maximum_queue_size: 5
      timeout_sec: 0.2
      is_motion_compensated: true
      publish_synchronized_pointcloud: true
      keep_input_frame_in_synchronized_pointcloud: true
      publish_previous_but_late_pointcloud: false
      synchronized_pointcloud_postfix: pointcloud
      input_twist_topic_type: twist
      input_topics: [
                    "/sensing/lidar/left_lower/pointcloud_before_sync",
                    "/sensing/lidar/left_upper/pointcloud_before_sync",
                    "/sensing/lidar/front_lower/pointcloud_before_sync",
                    "/sensing/lidar/front_upper/pointcloud_before_sync",
                    "/sensing/lidar/right_upper/pointcloud_before_sync",
                    "/sensing/lidar/right_lower/pointcloud_before_sync",
                    "/sensing/lidar/rear_lower/pointcloud_before_sync",
                    "/sensing/lidar/rear_upper/pointcloud_before_sync"
                  ]
      output_frame: base_link
      matching_strategy:
        type: advanced
        lidar_timestamp_offsets: [0.0, 0.0, 0.025, 0.028, 0.026, 0.05, 0.075, 0.076]
        lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]

Copy link
Contributor

@vividf vividf Feb 5, 2025

Choose a reason for hiding this comment

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

Please help me verify that the time offsets between the LiDAR timestamps are accurate. (with ros2 topic echo pointcloud_topic --field header)
I set these parameters at the X2 Gen2 bench months ago, and I'm unsure if any sensor configurations have changed

Copy link
Contributor Author

Choose a reason for hiding this comment

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

@vividf
I played rosbag recorded on the x2 gen vehicle and did ros2 topic echo pointcloud_topic --field header.
Please tell me if you need any other information!

simplescreenrecorder-2025-02-05_14.37.34.mp4

Copy link
Contributor

@vividf vividf Feb 5, 2025

Choose a reason for hiding this comment

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

Thank you!
This helps a lot!

The parameters below should be accurate one

/**:
  ros__parameters:
      debug_mode: false
      has_static_tf_only: false
      rosbag_length: 10.0
      maximum_queue_size: 5
      timeout_sec: 0.2
      is_motion_compensated: true
      publish_synchronized_pointcloud: true
      keep_input_frame_in_synchronized_pointcloud: true
      publish_previous_but_late_pointcloud: false
      synchronized_pointcloud_postfix: pointcloud
      input_twist_topic_type: twist
      input_topics: [
                    "/sensing/lidar/rear_upper/pointcloud_before_sync", # 0.047
                    "/sensing/lidar/rear_lower/pointcloud_before_sync", # 0.048
                    "/sensing/lidar/left_upper/pointcloud_before_sync", # 0.048
                    "/sensing/lidar/left_lower/pointcloud_before_sync", # 0.047
                    "/sensing/lidar/front_upper/pointcloud_before_sync", # 0.072
                    "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.072
                    "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.073
                    "/sensing/lidar/right_lower/pointcloud_before_sync" # 0.097
                  ]
      output_frame: base_link
      matching_strategy:
        type: advanced
        lidar_timestamp_offsets: [0.0, 0.001, 0.001, 0.0, 0.025, 0.025, 0.026, 0.050]
        lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]

Copy link
Contributor

@vividf vividf Feb 5, 2025

Choose a reason for hiding this comment

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

@TomohitoAndo Sorry! I set two values wrong.
0.01 -> 0.001

Copy link
Contributor

Choose a reason for hiding this comment

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

lidar_timestamp_offsets: [0.0, 0.001, 0.001, 0.0, 0.025, 0.025, 0.026, 0.050]

Copy link
Contributor Author

@TomohitoAndo TomohitoAndo Feb 5, 2025

Choose a reason for hiding this comment

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

Thank you!
updated param in fcb43fd

I confirmed diagnostics was OK with logging_simulator 😄
image
image

Copy link
Contributor

Choose a reason for hiding this comment

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

I updated the parameters by myself
#386 (comment)

Signed-off-by: Tomohito Ando <[email protected]>
Copy link
Contributor

@vividf vividf left a comment

Choose a reason for hiding this comment

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

LGTM!

Just a notification: I changed the parameters to #386 (comment).
Result should be the same

@TomohitoAndo TomohitoAndo merged commit 22e633c into tier4/universe Feb 5, 2025
8 of 10 checks passed
@TomohitoAndo TomohitoAndo deleted the feat/add-new-concatenate-x2-gen2 branch February 5, 2025 07:44
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants