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: add periodic tf publisher #15

Open
wants to merge 2 commits into
base: feat/L4T-R35.4.1
Choose a base branch
from
Open

feat: add periodic tf publisher #15

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
39995b4
feat: add periodic_transfrom_publisher
manato Dec 10, 2024
77c8f48
feat: use unified TF launcher to broadcast TF info periodically
manato Dec 11, 2024
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
9 changes: 5 additions & 4 deletions src/drs_launch/launch/component/drs_camera.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,16 +20,17 @@
<arg name="v4l2_camera_param_path" value="$(var camera_param_dir)/v4l2_camera.param.yaml"/>
<arg name="camera_info_url" value="file://$(var camera_param_dir)/camera_info.yaml"/>
</include>
<!-- Load gpuimg_proc into the container -->

Check warning on line 23 in src/drs_launch/launch/component/drs_camera.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (gpuimg)
<include file="$(find-pkg-share drs_launch)/launch/component/accelerated_image_processor.launch.xml">
<arg name="container_name" value="/$(var root_container_name)"/>
<arg name="accelerated_image_processor_param_path" value="$(var camera_param_dir)/accelerated_image_processor.param.yaml"/>
</include>

<!-- <!-\- Launch TF static publisher for camera and LiDAR -\-> -->
<!-- <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py"> -->
<!-- <arg name="tf_file_path" value="$(var camera_param_dir)/lidar_to_camera.json"/> -->
<!-- </include> -->
<!-- Launch TF periodic publisher for camera and LiDAR -->
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var camera_param_dir)/camera$(var camera_id)_calibration_results.yaml"/>
</include>

<!-- Execute readout setter -->
<node pkg="c2_readout_delay_setter" exec="c2_readout_delay_setter" name="readout_setter_$(var camera_id)">
<param from="$(var camera_param_dir)/readout_delay.param.yaml"/>
Expand All @@ -39,7 +40,7 @@

<group unless="$(var live_sensor)"> <!-- Boot decompressor for offline mode -->
<!-- XXX: The following node only accepts RELIABLE QoS as subscribed topic -->
<!-- User may need to overrride QoS when playing rosbag -->

Check warning on line 43 in src/drs_launch/launch/component/drs_camera.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (overrride)
<node pkg="image_transport" exec="republish" args="compressed">
<remap from="in/compressed" to="image_raw/compressed"/>
<remap from="out" to="image_raw"/>
Expand Down
56 changes: 42 additions & 14 deletions ...onent/lidar_camera_tf_publisher.launch.py → ...h/launch/component/tf_publisher.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,19 +15,24 @@

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
from launch.actions import OpaqueFunction, GroupAction, PushLaunchConfigurations, PopLaunchConfigurations
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
import json
import yaml
import re
from pathlib import Path
import transforms3d
from distutils.util import strtobool

def parse_yaml(contents: dict):
def parse_yaml(contents: dict, target_frame: str = None):
get_first_key = lambda d: list(d.keys())[0]
frame_id = get_first_key(contents)
child_frame_id = get_first_key(contents[frame_id])
if target_frame == "":
child_frame_id = get_first_key(contents[frame_id])
else:
child_frame_id = target_frame

x = contents[frame_id][child_frame_id]['x']
y = contents[frame_id][child_frame_id]['y']
Expand All @@ -36,7 +41,7 @@
pitch = contents[frame_id][child_frame_id]['pitch']
yaw = contents[frame_id][child_frame_id]['yaw']

q_wxyz = transforms3d.euler.euler2quat(roll, pitch, yaw) # output format is wxyz

Check warning on line 44 in src/drs_launch/launch/component/tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (wxyz)

Check warning on line 44 in src/drs_launch/launch/component/tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (wxyz)

# pack contents into a similar format as json one
return {
Expand All @@ -44,7 +49,7 @@
'child_frame_id': child_frame_id,
'transform': {
'translation': {'x': x, 'y': y, 'z': z},
'rotation': {'w': q_wxyz[0], 'x': q_wxyz[1], 'y': q_wxyz[2], 'z': q_wxyz[3]}

Check warning on line 52 in src/drs_launch/launch/component/tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (wxyz)

Check warning on line 52 in src/drs_launch/launch/component/tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (wxyz)

Check warning on line 52 in src/drs_launch/launch/component/tf_publisher.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (wxyz)
}
}

Expand All @@ -53,6 +58,15 @@
"""Return Launch Configuration for the TF between the lidar and the camera."""
tf_file = LaunchConfiguration('tf_file_path').perform(context)
file_format = Path(tf_file).suffix.lstrip('.')
target_frame = LaunchConfiguration('target_frame').perform(context)
publish_camera_optical_link = LaunchConfiguration('publish_camera_optical_link')
interval_sec = LaunchConfiguration('broadcast_interval_sec').perform(context)

launch_config_to_bool = lambda conf: bool(strtobool(conf.perform(context)))

print('')
print(f'tf_file: {tf_file}\n'
f'target_frame: {target_frame}')

'''
Here assumes json/yaml format generated by CalibrationTools
Expand All @@ -63,18 +77,22 @@
elif file_format == 'yaml' or file_format == 'yml':
with open(tf_file, 'r') as f:
yaml_contents = yaml.safe_load(f)
tf_data = parse_yaml(yaml_contents)
tf_data = parse_yaml(yaml_contents, target_frame)
print(f'Parsed yaml contents: {tf_data}')
else:
print(f'Invalid TF file ({tf_file}) was specified.')

camera_id = re.findall(r'\d+', str(tf_data['child_frame_id']))[0]
if launch_config_to_bool(publish_camera_optical_link):
camera_id = re.findall(r'\d+', str(tf_data['child_frame_id']))[0]
else:
camera_id = ''

return [
Node(
name=f'lidar_camera_tf_publisher{camera_id}',
package='tf2_ros',
executable='static_transform_publisher',
# name=f'lidar_camera_tf_publisher{camera_id}',
# periodic_transform_publisher assigns unique node name with random postfix
package='periodic_transform_publisher',
executable='periodic_transform_publisher',
arguments=[
'--x', str(tf_data['transform']['translation']['x']),
'--y', str(tf_data['transform']['translation']['y']),
Expand All @@ -85,11 +103,14 @@
'--qw', str(tf_data['transform']['rotation']['w']),
'--frame-id', str(tf_data['header']['frame_id']),
'--child-frame-id', str(tf_data['child_frame_id'])
]),
],
parameters=[{
'interval_sec': float(interval_sec),
}]),
Node(
name=f'camera_optical_link_publisher{camera_id}',
package='tf2_ros',
executable='static_transform_publisher',
name=f'periodic_camera_optical_link_publisher{camera_id}',
package='periodic_transform_publisher',
executable='periodic_transform_publisher',
arguments=[
'--x', '0.0',
'--y', '0.0',
Expand All @@ -101,13 +122,20 @@
'--frame-id', str(tf_data['child_frame_id']),
'--child-frame-id', str(tf_data['child_frame_id'].replace('camera_link',
'camera_optical_link'))
])
],
parameters=[{
'interval_sec': float(interval_sec),
}],
condition=IfCondition(publish_camera_optical_link))
]

def generate_launch_description():
return LaunchDescription(
[
DeclareLaunchArgument("tf_file_path"),
OpaqueFunction(function=launch_setup),
DeclareLaunchArgument("target_frame", default_value=''),
DeclareLaunchArgument("publish_camera_optical_link", default_value='True'),
DeclareLaunchArgument("broadcast_interval_sec", default_value='1.0'),
OpaqueFunction(function=launch_setup)
]
)
24 changes: 24 additions & 0 deletions src/drs_launch/launch/drs_ecu0.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,30 @@
<!-- <arg name="launch_driver" value="$(var live_sensor)"/> -->
<!-- </include> -->

<!-- TF for each LiDARs and base_link to drs_base_link -->
<group if="$(var live_sensor)">
<group> <!-- guard by group not to polute argument dict by multiple calling-->

Check warning on line 73 in src/drs_launch/launch/drs_ecu0.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Misspelled word (polute) Suggestions: (pollute*)
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
<arg name="target_frame" value="lidar_front"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
<arg name="target_frame" value="lidar_right"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/base_link_to_drs_base_link.yaml"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
</group>

<!-- topic monitor -->
<group scoped="True">
<!-- To prevent topic monitor communicate over network, set LOCALHOST_ONLY=1 -->
Expand Down
18 changes: 18 additions & 0 deletions src/drs_launch/launch/drs_ecu1.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,24 @@
<arg name="launch_driver" value="$(var live_sensor)"/>
</include>

<!-- TF for each LiDARs -->
<group if="$(var live_sensor)">
<group> <!-- guard by group not to polute argument dict by multiple calling-->

Check warning on line 68 in src/drs_launch/launch/drs_ecu1.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Misspelled word (polute) Suggestions: (pollute*)
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
<arg name="target_frame" value="lidar_rear"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
<arg name="target_frame" value="lidar_left"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
</group>

<!-- topic monitor -->
<group scoped="True">
<!-- To prevent topic monitor communicate over network, set LOCALHOST_ONLY=1 -->
Expand Down
112 changes: 77 additions & 35 deletions src/drs_launch/launch/drs_offline.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
<arg name="vehicle_id" default="default"/>
<arg name="live_sensor" default="False"
description="If true, sensor drivers will be executed (for actual recording operation)"/>
<arg name="publish_tf" default="True"/>

<let name="param_root_dir" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)"/>

Expand Down Expand Up @@ -105,40 +106,81 @@
<arg name="use_pointcloud_container" value="false"/>
</include>

<node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
args="--frame-id base_link --child-frame-id drs_base_link --x 0.6895 --y 0.0 --z 1.971 --yaw 0.000 --pitch -0.020, --roll 0.000"/>
<node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
args="--frame-id drs_base_link --child-frame-id lidar_front --x 0.4734 --y 0.05 --z -0.0702 --yaw -1.5708 --pitch 0.000, --roll 0.005"/>
<node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
args="--frame-id drs_base_link --child-frame-id lidar_left --x 0.100 --y 0.560 --z -0.0832 --yaw 1.5708 --pitch 0.000, --roll 0.000"/>
<node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
args="--frame-id drs_base_link --child-frame-id lidar_right --x 0.100 --y -0.555 --z -0.0502 --yaw -1.583 --pitch 0.014, --roll 0.005"/>
<node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
args="--frame-id drs_base_link --child-frame-id lidar_rear --x -0.2494 --y 0.0 --z 0.0873 --yaw 3.142 --pitch 0.000, --roll 0.000"/>
<include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera0/camera0_calibration_results.yaml"/>
</include>
<include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera1/camera1_calibration_results.yaml"/>
</include>
<include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera2/camera2_calibration_results.yaml"/>
</include>
<include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera3/camera3_calibration_results.yaml"/>
</include>
<include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera4/camera4_calibration_results.yaml"/>
</include>
<include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera5/camera5_calibration_results.yaml"/>
</include>
<include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera6/camera6_calibration_results.yaml"/>
</include>
<include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera7/camera7_calibration_results.yaml"/>
</include>

<group if="$(var publish_tf)">
<group> <!-- guard by group not to polute argument dict by multiple calling-->

Check warning on line 110 in src/drs_launch/launch/drs_offline.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Misspelled word (polute) Suggestions: (pollute*)
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/base_link_to_drs_base_link.yaml"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
<arg name="target_frame" value="lidar_front"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
<arg name="target_frame" value="lidar_left"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
<arg name="target_frame" value="lidar_right"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
<arg name="target_frame" value="lidar_rear"/>
<arg name="publish_camera_optical_link" value="False"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera0/camera0_calibration_results.yaml"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera1/camera1_calibration_results.yaml"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera2/camera2_calibration_results.yaml"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera3/camera3_calibration_results.yaml"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera4/camera4_calibration_results.yaml"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera5/camera5_calibration_results.yaml"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera6/camera6_calibration_results.yaml"/>
</include>
</group>
<group>
<include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
<arg name="tf_file_path" value="$(var param_root_dir)/camera7/camera7_calibration_results.yaml"/>
</include>
</group>
</group>

</launch>
1 change: 1 addition & 0 deletions src/drs_launch/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
<exec_depend>accelerated_image_processor</exec_depend>
<exec_depend>c2_readout_delay_setter</exec_depend>
<exec_depend>ros_topic_monitor</exec_depend>
<exec_depend>periodic_transform_publisher</exec_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
9 changes: 9 additions & 0 deletions src/individual_params/config/default/base_link_to_drs_base_link.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
base_link:
drs_base_link:
x: 0.6895
y: 0.0
z: 1.971
roll: 0.000
pitch: -0.020
yaw: 0.000

30 changes: 30 additions & 0 deletions src/individual_params/config/default/drs_base_link_to_lidars.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
drs_base_link:
lidar_left:
x: 0.100
y: 0.560
z: -0.0832
roll: 0.000
pitch: 0.000
yaw: 1.5708
lidar_right:
x: 0.100
y: -0.555
z: -0.0502
roll: 0.005
pitch: 0.014
yaw: -1.583
lidar_rear:
x: -0.2494
y: 0.0
z: 0.0873
roll: 0.000
pitch: 0.000
yaw: 3.142
lidar_front:
x: 0.4734
y: 0.05
z: -0.0702
roll: 0.005
pitch: 0.000
yaw: -1.5708

15 changes: 15 additions & 0 deletions src/periodic_transform_publisher/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.8)
project(periodic_transform_publisher)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

ament_auto_add_executable(${PROJECT_NAME}
src/periodic_transform_publisher.cpp
)

ament_auto_package()
Loading
Loading