69 lines
2.1 KiB
Python
69 lines
2.1 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch.conditions import IfCondition
|
|
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
|
from pathlib import Path
|
|
|
|
def generate_launch_description():
|
|
|
|
basedir = FindPackageShare("").find("toid_lidar")
|
|
|
|
lidar_config = PathJoinSubstitution([basedir, "params/lidar.yaml"])
|
|
rviz_config = PathJoinSubstitution([basedir, "rviz/conf.rviz"])
|
|
|
|
visualize = LaunchConfiguration("visualize")
|
|
draw_markers = LaunchConfiguration("draw_markers")
|
|
use_closest = LaunchConfiguration("use_closest")
|
|
lidar_frame = LaunchConfiguration("lidar_frame")
|
|
|
|
return LaunchDescription([
|
|
DeclareLaunchArgument(
|
|
'visualize',
|
|
default_value='False',
|
|
description="Whether to launch rviz2"
|
|
),
|
|
DeclareLaunchArgument(
|
|
'draw_markers',
|
|
default_value='False',
|
|
description="Draw markers"
|
|
),
|
|
DeclareLaunchArgument(
|
|
'use_closest',
|
|
default_value='True',
|
|
description="Use closest point for calibration"
|
|
),
|
|
DeclareLaunchArgument(
|
|
'lidar_frame',
|
|
default_value='lidar_frame',
|
|
description="TF frame of the lidar"
|
|
),
|
|
Node(
|
|
package='toid_lidar',
|
|
executable='toid_lidar',
|
|
output="screen",
|
|
parameters=[
|
|
lidar_config,
|
|
{
|
|
'closest': use_closest,
|
|
'draw_markers': draw_markers
|
|
}]
|
|
),
|
|
Node(
|
|
package='rplidar_ros',
|
|
executable='rplidar_composition',
|
|
output="screen",
|
|
parameters=[lidar_config, {'frame_id': lidar_frame}]
|
|
),
|
|
Node(
|
|
package='rviz2',
|
|
executable='rviz2',
|
|
name='rviz2',
|
|
output='screen',
|
|
arguments=['-d', rviz_config],
|
|
condition=IfCondition(visualize)
|
|
)
|
|
])
|