100 lines
3.0 KiB
Python
100 lines
3.0 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch.conditions import IfCondition, UnlessCondition
|
|
from launch.substitutions import Command, LaunchConfiguration
|
|
from launch_ros.actions import Node, LifecycleNode
|
|
from launch_ros.substitutions import FindPackageShare
|
|
import os
|
|
|
|
def generate_launch_description():
|
|
pkg_share = FindPackageShare("").find('toid_navigation')
|
|
params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
|
|
map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
|
|
default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'ros_control.rviz')
|
|
|
|
|
|
description_pkg_share = FindPackageShare("").find('toid_bot_description')
|
|
default_model_path = os.path.join(
|
|
description_pkg_share,
|
|
'src',
|
|
'toid_bot_description.urdf'
|
|
)
|
|
|
|
starting_position = LaunchConfiguration("start_position")
|
|
visualize = LaunchConfiguration("visualize")
|
|
|
|
starting_position_arg = DeclareLaunchArgument(
|
|
'start_position',
|
|
default_value='0 0 0 0 0 0',
|
|
description="Set initial robot position"
|
|
)
|
|
|
|
visualize_arg = DeclareLaunchArgument(
|
|
'visualize',
|
|
default_value='True',
|
|
description="Whether to launch rviz2"
|
|
)
|
|
|
|
odom_broadcast = Node(
|
|
package='tf2_ros',
|
|
executable='static_transform_publisher',
|
|
name='map_to_odom_broadcaster',
|
|
arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
|
|
)
|
|
|
|
robot_state_publisher = Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
name='robot_state_publisher',
|
|
output='screen',
|
|
parameters = [{'robot_description': Command(['xacro ', default_model_path])}]
|
|
)
|
|
|
|
controller_manager = Node(
|
|
package='controller_manager',
|
|
executable='ros2_control_node',
|
|
output='screen',
|
|
parameters = [params]
|
|
)
|
|
|
|
joint_state_broadcaster = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
output='screen',
|
|
arguments= ["joint_state_broadcaster"]
|
|
)
|
|
|
|
diffbot_base_controller = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
output='both',
|
|
arguments= [
|
|
"diffbot_base_controller",
|
|
"-p",
|
|
params,
|
|
"--controller-ros-args",
|
|
"-r diffbot_base_controller/cmd_vel:=/cmd_vel",
|
|
"--controller-ros-args",
|
|
"-r diffbot_base_controller/odom:=/odom"
|
|
]
|
|
)
|
|
|
|
rviz_node = Node(
|
|
package='rviz2',
|
|
executable='rviz2',
|
|
name='rviz2',
|
|
output='screen',
|
|
arguments=['-d', default_rviz_config_path],
|
|
condition=IfCondition(visualize)
|
|
)
|
|
|
|
return LaunchDescription([
|
|
starting_position_arg,
|
|
visualize_arg,
|
|
odom_broadcast,
|
|
robot_state_publisher,
|
|
controller_manager,
|
|
joint_state_broadcaster,
|
|
diffbot_base_controller,
|
|
rviz_node
|
|
]) |