114 lines
4.0 KiB
Python
114 lines
4.0 KiB
Python
import os
|
|
|
|
import yaml
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, LogInfo
|
|
from launch.conditions import IfCondition
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
|
|
# Vi tri lap dat tung frame lidar so voi base_link: frame -> (x, y, z, yaw) met/rad.
|
|
# Frame khong liet ke o day mac dinh dat tai goc base_link (0,0,0,0) -> van co TF
|
|
# nen RViz khong con vut message. Sua toa do thuc te tai day.
|
|
MOUNT_POSES = {
|
|
'front': (0.0, 0.0, 0.0, 0.0),
|
|
'rear': (0.0, 0.0, 0.0, 0.0),
|
|
'sick1': (0.0, 0.00, 0.0, 0.0),
|
|
}
|
|
|
|
|
|
def lidars_from_config(params_file):
|
|
"""Doc config.yaml -> danh sach dict thong tin moi lidar trong 'lidars'."""
|
|
with open(params_file, 'r') as f:
|
|
data = yaml.safe_load(f)
|
|
params = data['lidarlib_node']['ros__parameters']
|
|
lidars = []
|
|
for name in params.get('lidars', []):
|
|
block = params.get(name, {}) or {}
|
|
lidars.append({
|
|
'name': name,
|
|
'brand': block.get('brand', '?'),
|
|
'model': block.get('model', '?'),
|
|
'ip': block.get('ip', '?'),
|
|
'port': block.get('port', '?'),
|
|
'inverted': block.get('inverted', False),
|
|
'topic': block.get('topic', f'scan_{name}'),
|
|
'frame_id': block.get('frame_id', name),
|
|
})
|
|
return lidars
|
|
|
|
|
|
def lidar_frames_from_config(params_file):
|
|
"""Doc config.yaml -> danh sach (frame_id) cua moi lidar trong 'lidars'."""
|
|
return [ld['frame_id'] for ld in lidars_from_config(params_file)]
|
|
|
|
|
|
def generate_launch_description():
|
|
pkg = get_package_share_directory('lidarlib_ros')
|
|
params_file = os.path.join(pkg, 'config', 'lidar.yaml')
|
|
rviz_config = os.path.join(pkg, 'rviz', 'lidar.rviz')
|
|
|
|
use_rviz = LaunchConfiguration('rviz')
|
|
|
|
lidars = lidars_from_config(params_file)
|
|
|
|
# In thong tin cau hinh ra man hinh de tien theo doi/test.
|
|
log_nodes = [
|
|
LogInfo(msg='========== LIDAR LAUNCH INFO =========='),
|
|
LogInfo(msg=f'Params file : {params_file}'),
|
|
LogInfo(msg=f'RViz config : {rviz_config}'),
|
|
LogInfo(msg=f'So luong lidar: {len(lidars)}'),
|
|
LogInfo(msg='----------------------------------------'),
|
|
]
|
|
|
|
# Tu sinh 1 static TF cho MOI frame lidar co trong config -> them lidar la co TF.
|
|
tf_nodes = []
|
|
for ld in lidars:
|
|
frame = ld['frame_id']
|
|
x, y, z, yaw = MOUNT_POSES.get(frame, (0.0, 0.0, 0.0, 0.0))
|
|
default_pose = frame not in MOUNT_POSES
|
|
log_nodes.append(LogInfo(
|
|
msg=(f"[{ld['name']}] brand={ld['brand']} model={ld['model']} "
|
|
f"ip={ld['ip']}:{ld['port']} inverted={ld['inverted']} "
|
|
f"topic={ld['topic']} frame={frame} "
|
|
f"TF(x={x} y={y} z={z} yaw={yaw})"
|
|
f"{' <-- mac dinh (0,0,0), sua trong MOUNT_POSES' if default_pose else ''}")))
|
|
tf_nodes.append(Node(
|
|
package='tf2_ros',
|
|
executable='static_transform_publisher',
|
|
name=f'base_to_{frame}',
|
|
arguments=[str(x), str(y), str(z), str(yaw), '0', '0', 'base_link', frame],
|
|
))
|
|
|
|
log_nodes.append(LogInfo(msg='========================================'))
|
|
|
|
return LaunchDescription([
|
|
DeclareLaunchArgument(
|
|
'rviz', default_value='true',
|
|
description='Mo RViz2 de hien thi LaserScan'),
|
|
|
|
*log_nodes,
|
|
|
|
# Node cau noi lidarlib -> sensor_msgs/LaserScan (nhieu lidar 1 node)
|
|
Node(
|
|
package='lidarlib_ros',
|
|
executable='lidarlib_node',
|
|
name='lidarlib_node',
|
|
output='screen',
|
|
parameters=[params_file],
|
|
),
|
|
|
|
*tf_nodes,
|
|
|
|
# RViz2 voi config san
|
|
Node(
|
|
package='rviz2',
|
|
executable='rviz2',
|
|
name='rviz2',
|
|
arguments=['-d', rviz_config],
|
|
condition=IfCondition(use_rviz),
|
|
output='screen',
|
|
),
|
|
])
|