124 lines
3.3 KiB
Python
124 lines
3.3 KiB
Python
import os.path
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
from launch import LaunchDescription
|
|
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch.substitutions import Command
|
|
|
|
def generate_launch_description():
|
|
|
|
ld = LaunchDescription()
|
|
|
|
pkg_name = 'sherpa'
|
|
pkg_share = get_package_share_directory(pkg_name)
|
|
pkg_prefix = os.path.dirname(pkg_share)
|
|
|
|
world_path = os.path.join(pkg_share, 'worlds', 'asd_course.world')
|
|
xacro_file = os.path.join(pkg_share, 'urdf', 'sherpa.xacro')
|
|
|
|
# create nodes
|
|
|
|
gz_env = SetEnvironmentVariable(
|
|
name='GZ_SIM_RESOURCE_PATH',
|
|
value=[pkg_prefix]
|
|
)
|
|
|
|
gz = ExecuteProcess(
|
|
cmd=[
|
|
'ros2', 'launch', 'ros_gz_sim', 'gz_sim.launch.py',
|
|
f'gz_args:=-r {world_path}'
|
|
],
|
|
output='screen'
|
|
)
|
|
|
|
state_publisher = Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
name='robot_state_publisher',
|
|
output='screen',
|
|
parameters=[{
|
|
'robot_description': Command(['xacro ', xacro_file])
|
|
}]
|
|
)
|
|
|
|
controller = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
arguments=['ackermann_controller'],
|
|
#output='screen'
|
|
)
|
|
|
|
joint_state_broadcaster = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
|
|
parameters=[{'use_sim_time': True}],
|
|
#output='screen'
|
|
)
|
|
|
|
static_tf = Node(
|
|
package='tf2_ros',
|
|
executable='static_transform_publisher',
|
|
name='static_tf_base_to_lidar',
|
|
arguments=[
|
|
'0', '0', '0',
|
|
'0', '0', '0',
|
|
'velodyne',
|
|
'lidar_link',
|
|
]
|
|
)
|
|
|
|
robot = ExecuteProcess(
|
|
cmd=[
|
|
'ros2', 'run', 'ros_gz_sim', 'create',
|
|
'-name', 'sherpa',
|
|
'-topic', 'robot_description',
|
|
'-x', '0', '-y', '0', '-z', '2'
|
|
],
|
|
output='screen'
|
|
)
|
|
|
|
twist_stamper = Node(
|
|
package="twist_stamper",
|
|
executable="twist_stamper",
|
|
remappings=[
|
|
('/cmd_vel_out', '/ackermann_controller/reference'),
|
|
],
|
|
output='screen'
|
|
)
|
|
|
|
gz_bridge = Node(
|
|
package="ros_gz_bridge",
|
|
executable="parameter_bridge",
|
|
arguments=[
|
|
"--ros-args",
|
|
"-p",
|
|
f"config_file:={os.path.join(pkg_share, 'config', 'gz_bridge.yaml')}",
|
|
]
|
|
)
|
|
|
|
|
|
rviz2 = Node(
|
|
package='rviz2',
|
|
executable='rviz2',
|
|
arguments=['-d', f"{os.path.join(get_package_share_directory('sherpa'), 'config', 'sherpa.rviz')}"]
|
|
)
|
|
|
|
ld.add_action(gz_env)
|
|
ld.add_action(gz)
|
|
ld.add_action(state_publisher)
|
|
ld.add_action(static_tf)
|
|
ld.add_action(robot)
|
|
ld.add_action(gz_bridge)
|
|
ld.add_action(joint_state_broadcaster)
|
|
ld.add_action(controller)
|
|
ld.add_action(twist_stamper)
|
|
ld.add_action(rviz2)
|
|
|
|
return ld
|