improve gazebo arguments added debugger

This commit is contained in:
Ilya Uraev 2024-02-16 15:23:20 +03:00
parent 5e14dc00a2
commit 77fa543e78

View file

@ -12,27 +12,38 @@ from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo", default_value="false", description="Gazebo Simulation")
DeclareLaunchArgument("sim_gazebo",
default_value="false",
description="Gazebo Simulation")
)
declared_arguments.append(
DeclareLaunchArgument(
"rbs_robot_type",
description="Type of robot by name",
choices=["rbs_arm" ,"ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
DeclareLaunchArgument("rbs_robot_type",
description="Type of robot by name",
choices=["rbs_arm" ,"ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",)
)
declared_arguments.append(
DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
DeclareLaunchArgument("env_manager",
default_value="false",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui", default_value="false", description="Launch env_manager?")
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("debugger",
default_value="false",
description="launch Gazebo with debugger?")
)
sim_gazebo = LaunchConfiguration("sim_gazebo")
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
env_manager_cond = LaunchConfiguration("env_manager")
gazebo_gui = LaunchConfiguration("gazebo_gui")
debugger = LaunchConfiguration("debugger")
# FIXME: To args when we'll have different files
# TODO: Use global variable to find world file in robossembler_db
world_config_file = PathJoinSubstitution(
@ -44,31 +55,38 @@ def generate_launch_description():
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_gz_sim'),
'launch', 'gz_sim.launch.py')]),
launch_arguments=[('gz_args', [' -r ',world_config_file, " -s"])],
launch_arguments={
'gz_args': [' -r ',world_config_file, " -s"],
"debugger": debugger,
}.items(),
condition=UnlessCondition(gazebo_gui))
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_gz_sim'),
'launch', 'gz_sim.launch.py')]),
launch_arguments=[('gz_args', [' -r ',world_config_file])],
launch_arguments={
'gz_args': [' -r ',world_config_file],
"debugger": debugger,
}.items(),
condition=IfCondition(gazebo_gui))
# Spawn robot
gazebo_spawn_robot = Node(package='ros_gz_sim', executable='create',
arguments=[
'-name', rbs_robot_type,
'-x', '0.0',
'-z', '0.0',
'-y', '0.0',
'-topic', '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
gazebo_spawn_robot = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', rbs_robot_type,
'-x', '0.0',
'-z', '0.0',
'-y', '0.0',
'-topic', '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
env_manager = Node(package="env_manager",
executable="run_env_manager",
condition=IfCondition(env_manager_cond)
)
condition=IfCondition(env_manager_cond))
# Bridge
rgbd_bridge_out = Node(
package='ros_gz_bridge',
@ -80,8 +98,7 @@ def generate_launch_description():
'/outer_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
],
output='screen',
condition=IfCondition(sim_gazebo)
)
condition=IfCondition(sim_gazebo))
rgbd_bridge_in = Node(
package='ros_gz_bridge',
@ -93,9 +110,14 @@ def generate_launch_description():
'/inner_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
],
output='screen',
)
condition=IfCondition(sim_gazebo))
clock_bridge = Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
output='screen',
condition=IfCondition(sim_gazebo))
nodes_to_start = [
gazebo,
@ -103,6 +125,7 @@ def generate_launch_description():
gazebo_spawn_robot,
env_manager,
rgbd_bridge_out,
rgbd_bridge_in
rgbd_bridge_in,
clock_bridge,
]
return LaunchDescription(declared_arguments + nodes_to_start)