Skip to content

Commit

Permalink
Refactor QEV3D launch file: streamline argument declarations and impr…
Browse files Browse the repository at this point in the history 
…ove node organization
  • Loading branch information
@bocho0600
bocho0600 committed Feb 6, 2025
1 parent 43634de commit 7437ba5
Showing 1 changed file with 49 additions and 104 deletions.
153 changes: 49 additions & 104 deletions src/control/ros2_control/bringup/launch/qev3d.launch.py
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler
from launch.conditions import IfCondition, UnlessCondition
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
Expand All @@ -9,49 +9,35 @@


def generate_launch_description():

# region [declare arguments]
declared_arguments = []
declared_arguments.append(
# Declare arguments
declared_arguments = [
DeclareLaunchArgument(
"description_package", # Argument name
default_value="vehicle_urdf", # Default argument value
description="Description package with robot URDF/xacro files. Usually the argument \
is not set, it enables use of a custom description.",
)
)
declared_arguments.append(
"description_package",
default_value="vehicle_urdf",
description="Description package with robot URDF/xacro files.",
),
DeclareLaunchArgument(
"description_file", # Argument name
default_value="qev-3d.urdf.xacro", # Default argument value
description="URDF/xacro file to load.",
)
),
declared_arguments.append(
"description_file", default_value="qev-3d.urdf.xacro", description="URDF/xacro file to load."
),
DeclareLaunchArgument(
"gui",
default_value="true",
description="Start Rviz2 and Joint State Publisher gui automatically \
with this launch file.",
)
),
declared_arguments.append(
"gui", default_value="true", description="Start Rviz2 and Joint State Publisher GUI automatically."
),
DeclareLaunchArgument(
"prefix", # Argument name
default_value='""', # value of argument is empty string
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(
"prefix", default_value='""', description="Prefix of the joint names, useful for multi-robot setup."
),
DeclareLaunchArgument(
"remap_odometry_tf", # Argument name
default_value="false", # Default argument value
"remap_odometry_tf",
default_value="false",
description="Remap odometry TF from the steering controller to the TF tree.",
)
)
# endregion
),
]

# Initialize arguments
prefix = LaunchConfiguration("prefix")
gui = LaunchConfiguration("gui")
description_file = LaunchConfiguration("description_file")
description_package = LaunchConfiguration("description_package")
remap_odometry_tf = LaunchConfiguration("remap_odometry_tf")

# Get URDF via xacro
robot_description_content = Command(
Expand All @@ -60,49 +46,20 @@ def generate_launch_description():
" ",
PathJoinSubstitution([FindPackageShare("vehicle_urdf"), "urdf", description_file]),
" ",
"prefix:=", # prefix argument is passed to xacro, WHICH IS EMPTY
"prefix:=",
prefix,
]
)
robot_description = ParameterValue(robot_description_content, value_type=str)

# region [initialize arguments]
robot_description = ParameterValue(
robot_description_content, value_type=str
) # variable storing the value of the argument
prefix = LaunchConfiguration("prefix") # varible storing prefix (an empty string)
gui = LaunchConfiguration("gui") # "True" - enable something
description_file = LaunchConfiguration("description_file") # storing URDF file name (qev-3d.urdf.xacro)
description_package = LaunchConfiguration("description_package") # storing package name (vehicle_urdf)
remap_odometry_tf = LaunchConfiguration("remap_odometry_tf") # "False" - disable something
# endregion

# region [Path Join]
robot_controllers = PathJoinSubstitution( # join the path of controllers configuration file
[
FindPackageShare("qev3d_ros2_control"),
"config",
"qev3d_controller.yaml",
]
)

rviz_config_file = PathJoinSubstitution(
[
FindPackageShare("vehicle_urdf"),
"rviz",
"config.rviz",
]
# Path configurations
robot_controllers = PathJoinSubstitution(
[FindPackageShare("qev3d_ros2_control"), "config", "qev3d_controller.yaml"]
)
rviz_config_file = PathJoinSubstitution([FindPackageShare("vehicle_urdf"), "rviz", "config.rviz"])

# endregion

# TODO: SET UP RVIZ2 CONFIGURATION FILE

joint_state_publisher_node = Node(
package="joint_state_publisher_gui",
executable="joint_state_publisher_gui",
condition=IfCondition(gui), # Only launch the node if gui is set to true
)
control_node = Node( # launch the control node - launching the controller manager
# Nodes
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_controllers],
Expand All @@ -120,14 +77,23 @@ def generate_launch_description():
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
condition=IfCondition(gui), # Only launch the node if gui is set to true
condition=IfCondition(gui),
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster"],
)

drive_pid_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["drive_pid_controller", "--param-file", robot_controllers],
)
steering_pid_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["steering_pid_controller", "--param-file", robot_controllers],
)
robot_bicycle_controller_spawner = Node(
package="controller_manager",
executable="spawner",
Expand All @@ -141,50 +107,29 @@ def generate_launch_description():
condition=IfCondition(remap_odometry_tf),
)

drive_pid_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["drive_pid_controller", "--param-file", robot_controllers],
)

steering_pid_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["drive_pid_controller", "--param-file", robot_controllers],
)

# Delay start of forward_position_controller_spawner after `position_controller_spawner`
# Event Handlers for delayed execution
delay_robot_bicycle_controller_spawner_after_steering_controller_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=steering_pid_controller,
on_exit=[robot_bicycle_controller_spawner],
)
)
delay_robot_bicycle_controller_spawner_after_drive_controller_spawner = RegisterEventHandler(
delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=drive_pid_controller,
on_exit=[robot_bicycle_controller_spawner],
target_action=joint_state_broadcaster_spawner,
on_exit=[rviz_node],
)
)
# Delay rviz start after `joint_state_broadcaster`
delay_rviz_after_joint_state_broadcaster_spawner = (
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[rviz_node],
)
),
)

# Nodes to launch
nodes = [
# joint_state_publisher_node,
control_node,
robot_state_pub_bicycle_node,
joint_state_broadcaster_spawner,
drive_pid_controller,
steering_pid_controller,
delay_robot_bicycle_controller_spawner_after_steering_controller_spawner,
delay_rviz_after_joint_state_broadcaster_spawner,
rviz_node,
]

return LaunchDescription(declared_arguments + nodes)

0 comments on commit 7437ba5

Please sign in to comment.