255 lines
8.7 KiB
Python
255 lines
8.7 KiB
Python
# Copyright 2025 Reazon Holdings, Inc.
|
|
# Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt)
|
|
#
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
|
|
#
|
|
# Source of this file are templates in
|
|
# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
|
|
#
|
|
# Author: Dr. Denis
|
|
#
|
|
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction
|
|
from launch.event_handlers import OnProcessExit, OnProcessStart
|
|
from launch.substitutions import (
|
|
Command,
|
|
FindExecutable,
|
|
LaunchConfiguration,
|
|
PathJoinSubstitution,
|
|
)
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
|
|
|
def generate_launch_description():
|
|
# Declare arguments
|
|
declared_arguments = []
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"runtime_config_package",
|
|
default_value="openarm_bringup",
|
|
description='Package with the controller\'s configuration in "config" folder. \
|
|
Usually the argument is not set, it enables use of a custom setup.',
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"controllers_file",
|
|
default_value="openarm_controllers.yaml",
|
|
description="YAML file with the controllers configuration.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"description_package",
|
|
default_value="openarm_description",
|
|
description="Description package with robot URDF/xacro files. Usually the argument \
|
|
is not set, it enables use of a custom description.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"description_file",
|
|
default_value="openarm.urdf.xacro",
|
|
description="URDF/XACRO description file with the robot.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"prefix",
|
|
default_value='""',
|
|
description="Prefix of the joint names, useful for \
|
|
multi-robot setup. If changed than also joint names in the controllers' configuration \
|
|
have to be updated.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"hardware_type",
|
|
default_value="real",
|
|
description="Hardware interface type: 'real', 'sim' (MuJoCo), or 'mock'",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"mock_sensor_commands",
|
|
default_value="false",
|
|
description="Enable mock command interfaces for sensors used for simple simulations. \
|
|
Used only if 'hardware_type' parameter is 'mock'.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_controller",
|
|
default_value="joint_trajectory_controller",
|
|
choices=["forward_position_controller",
|
|
"joint_trajectory_controller"],
|
|
description="Robot controller to start.",
|
|
)
|
|
)
|
|
|
|
# Initialize Arguments
|
|
runtime_config_package = LaunchConfiguration("runtime_config_package")
|
|
controllers_file = LaunchConfiguration("controllers_file")
|
|
description_package = LaunchConfiguration("description_package")
|
|
description_file = LaunchConfiguration("description_file")
|
|
prefix = LaunchConfiguration("prefix")
|
|
hardware_type = LaunchConfiguration("hardware_type")
|
|
mock_sensor_commands = LaunchConfiguration("mock_sensor_commands")
|
|
robot_controller = LaunchConfiguration("robot_controller")
|
|
|
|
# Get URDF via xacro
|
|
robot_description_content = Command(
|
|
[
|
|
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
|
" ",
|
|
PathJoinSubstitution(
|
|
[FindPackageShare(description_package),
|
|
"urdf", description_file]
|
|
),
|
|
" ",
|
|
"prefix:=",
|
|
prefix,
|
|
" ",
|
|
"hardware_type:=",
|
|
hardware_type,
|
|
" ",
|
|
"mock_sensor_commands:=",
|
|
mock_sensor_commands,
|
|
" ",
|
|
]
|
|
)
|
|
|
|
robot_description = {"robot_description": robot_description_content}
|
|
|
|
robot_controllers = PathJoinSubstitution(
|
|
[FindPackageShare(runtime_config_package), "config", controllers_file]
|
|
)
|
|
rviz_config_file = PathJoinSubstitution(
|
|
[FindPackageShare(description_package), "rviz", "openarm.rviz"]
|
|
)
|
|
|
|
control_node = Node(
|
|
package="controller_manager",
|
|
executable="ros2_control_node",
|
|
output="both",
|
|
parameters=[robot_description, robot_controllers],
|
|
)
|
|
robot_state_pub_node = Node(
|
|
package="robot_state_publisher",
|
|
executable="robot_state_publisher",
|
|
output="both",
|
|
parameters=[robot_description],
|
|
)
|
|
rviz_node = Node(
|
|
package="rviz2",
|
|
executable="rviz2",
|
|
name="rviz2",
|
|
output="log",
|
|
arguments=["-d", rviz_config_file],
|
|
)
|
|
|
|
joint_state_broadcaster_spawner = Node(
|
|
package="controller_manager",
|
|
executable="spawner",
|
|
arguments=[
|
|
"joint_state_broadcaster",
|
|
"--controller-manager",
|
|
"/controller_manager",
|
|
],
|
|
)
|
|
|
|
robot_controller_names = [robot_controller]
|
|
robot_controller_spawners = []
|
|
for controller in robot_controller_names:
|
|
robot_controller_spawners += [
|
|
Node(
|
|
package="controller_manager",
|
|
executable="spawner",
|
|
arguments=[controller, "-c", "/controller_manager"],
|
|
)
|
|
]
|
|
|
|
inactive_robot_controller_names = []
|
|
inactive_robot_controller_spawners = []
|
|
for controller in inactive_robot_controller_names:
|
|
inactive_robot_controller_spawners += [
|
|
Node(
|
|
package="controller_manager",
|
|
executable="spawner",
|
|
arguments=[controller, "-c",
|
|
"/controller_manager", "--inactive"],
|
|
)
|
|
]
|
|
|
|
# Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node
|
|
delay_joint_state_broadcaster_spawner_after_ros2_control_node = (
|
|
RegisterEventHandler(
|
|
event_handler=OnProcessStart(
|
|
target_action=control_node,
|
|
on_start=[
|
|
TimerAction(
|
|
period=3.0,
|
|
actions=[joint_state_broadcaster_spawner],
|
|
),
|
|
],
|
|
)
|
|
)
|
|
)
|
|
|
|
# Delay loading and activation of robot_controller_names after `joint_state_broadcaster`
|
|
delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = []
|
|
for i, controller in enumerate(robot_controller_spawners):
|
|
delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [
|
|
RegisterEventHandler(
|
|
event_handler=OnProcessExit(
|
|
target_action=(
|
|
robot_controller_spawners[i - 1]
|
|
if i > 0
|
|
else joint_state_broadcaster_spawner
|
|
),
|
|
on_exit=[controller],
|
|
)
|
|
)
|
|
]
|
|
|
|
# Delay start of inactive_robot_controller_names after other controllers
|
|
delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner = []
|
|
for i, controller in enumerate(inactive_robot_controller_spawners):
|
|
delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner += [
|
|
RegisterEventHandler(
|
|
event_handler=OnProcessExit(
|
|
target_action=(
|
|
inactive_robot_controller_spawners[i - 1]
|
|
if i > 0
|
|
else robot_controller_spawners[-1]
|
|
),
|
|
on_exit=[controller],
|
|
)
|
|
)
|
|
]
|
|
|
|
return LaunchDescription(
|
|
declared_arguments
|
|
+ [
|
|
control_node,
|
|
robot_state_pub_node,
|
|
rviz_node,
|
|
delay_joint_state_broadcaster_spawner_after_ros2_control_node,
|
|
]
|
|
+ delay_robot_controller_spawners_after_joint_state_broadcaster_spawner
|
|
+ delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner
|
|
)
|