97 lines
3.6 KiB
Python
97 lines
3.6 KiB
Python
import os
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
from launch import LaunchDescription
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from launch.actions import IncludeLaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch_ros.actions import Node
|
|
from launch.substitutions import Command
|
|
|
|
|
|
def generate_launch_description():
|
|
|
|
# Check if we're told to use sim time
|
|
lidar_vertical_fov_upper = LaunchConfiguration('lidar_vertical_fov_upper')
|
|
lidar_vertical_fov_lower = LaunchConfiguration('lidar_vertical_fov_lower')
|
|
lidar_horizontal_fov_upper = LaunchConfiguration('lidar_horizontal_fov_upper')
|
|
lidar_horizontal_fov_lower = LaunchConfiguration('lidar_horizontal_fov_lower')
|
|
|
|
|
|
# Process the URDF file
|
|
package_name='sensor_sim' #<--- CHANGE ME
|
|
pkg_path = os.path.join(get_package_share_directory(package_name))
|
|
xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
|
|
robot_description_config = Command(['xacro ', xacro_file, ' lidar_vertical_fov_upper:=', lidar_vertical_fov_upper, ' lidar_vertical_fov_lower:=', lidar_vertical_fov_lower, ' lidar_horizontal_fov_upper:=', lidar_horizontal_fov_upper, ' lidar_horizontal_fov_lower:=', lidar_horizontal_fov_lower])
|
|
world_path = os.path.join(pkg_path,'worlds')
|
|
|
|
# Create a robot_state_publisher node
|
|
params = {'robot_description': robot_description_config, 'use_sim_time': True, 'publish_frequency': 0.0001, 'use_tf_static': False}
|
|
node_robot_state_publisher = Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
parameters=[params]
|
|
)
|
|
|
|
gazebo = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource([os.path.join(
|
|
get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]),
|
|
launch_arguments=[('gz_args', [f" -r -v 0 {world_path}/test_world.sdf"])],
|
|
)
|
|
|
|
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
|
|
spawn_entity = Node(package='ros_gz_sim', executable='create',
|
|
arguments=[
|
|
'-topic', 'robot_description',
|
|
'-name', 'spawn_robot',
|
|
'-z', '1',
|
|
'-x', '0',
|
|
],
|
|
output='screen'
|
|
)
|
|
|
|
# Bridge
|
|
bridge = Node(
|
|
package='ros_gz_bridge',
|
|
executable='parameter_bridge',
|
|
arguments=[
|
|
'/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
|
|
'/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
|
|
],
|
|
output='screen'
|
|
)
|
|
|
|
lidar_visualizer = Node(
|
|
package='sensor_sim',
|
|
executable='lidar_visualizer.py',
|
|
name='lidar_visualizer',
|
|
output='screen'
|
|
)
|
|
|
|
# Launch!
|
|
return LaunchDescription([
|
|
DeclareLaunchArgument(
|
|
'lidar_vertical_fov_upper',
|
|
default_value='45',
|
|
description='Upper limit of FOV of lidar'),
|
|
DeclareLaunchArgument(
|
|
'lidar_vertical_fov_lower',
|
|
default_value='-45',
|
|
description='Lower limit of FOV of lidar'),
|
|
DeclareLaunchArgument(
|
|
'lidar_horizontal_fov_upper',
|
|
default_value='45',
|
|
description='Upper limit of FOV of lidar'),
|
|
DeclareLaunchArgument(
|
|
'lidar_horizontal_fov_lower',
|
|
default_value='-45',
|
|
description='Lower limit of FOV of lidar'),
|
|
node_robot_state_publisher,
|
|
gazebo,
|
|
spawn_entity,
|
|
bridge,
|
|
lidar_visualizer
|
|
])
|