Skip to content

Commit

Permalink
fix
Browse files Browse the repository at this point in the history
  • Loading branch information
lukicdarkoo committed Dec 18, 2024
1 parent 1c13094 commit 10fc127
Showing 1 changed file with 54 additions and 61 deletions.
115 changes: 54 additions & 61 deletions webots_ros2_universal_robot/launch/robot_launch/robot_nodes_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,95 +25,88 @@
from webots_ros2_driver.webots_controller import WebotsController


PACKAGE_NAME = "webots_ros2_universal_robot"
PACKAGE_NAME = 'webots_ros2_universal_robot'


def generate_launch_description():
package_dir = get_package_share_directory(PACKAGE_NAME)
robot_description_path = os.path.join(
package_dir, "resource", "ur5e_with_gripper.urdf"
)
ros2_control_params = os.path.join(
package_dir, "resource", "ros2_control_config.yaml"
)
robot_description_path = os.path.join(package_dir, 'resource', 'ur5e_with_gripper.urdf')
ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control_config.yaml')

# Define your URDF robots here
# The name of an URDF robot has to match the name of the robot of the driver node
# You can specify the URDF file to use with "urdf_path"
spawn_URDF_ur5e = URDFSpawner(
name="UR5e",
name='UR5e',
urdf_path=robot_description_path,
translation="0 0 0.6",
rotation="0 0 1 -1.5708",
translation='0 0 0.6',
rotation='0 0 1 -1.5708',
)

# Driver nodes
# When having multiple robot it is mandatory to specify the robot name.
universal_robot_driver = WebotsController(
robot_name="UR5e",
namespace="ur5e",
robot_name='UR5e',
namespace='ur5e',
parameters=[
{"robot_description": robot_description_path},
{"use_sim_time": True},
{"set_robot_state_publisher": True},
ros2_control_params,
{'robot_description': robot_description_path},
{'use_sim_time': True},
{'set_robot_state_publisher': True},
ros2_control_params
],
)

# Other ROS 2 nodes
controller_manager_timeout = ["--controller-manager-timeout", "100"]
controller_manager_prefix = "python.exe" if os.name == "nt" else ""
controller_manager_timeout = ['--controller-manager-timeout', '100']
controller_manager_prefix = 'python.exe' if os.name == 'nt' else ''
trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner",
output="screen",
package='controller_manager',
executable='spawner',
output='screen',
prefix=controller_manager_prefix,
arguments=["ur_joint_trajectory_controller", "-c", "ur5e/controller_manager"]
+ controller_manager_timeout
+ ["--param-file", ros2_control_params],
arguments=['ur_joint_trajectory_controller', '-c', 'ur5e/controller_manager'] + controller_manager_timeout,
)

joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
output="screen",
package='controller_manager',
executable='spawner',
output='screen',
prefix=controller_manager_prefix,
arguments=["ur_joint_state_broadcaster", "-c", "ur5e/controller_manager"]
+ controller_manager_timeout
+ ["--param-file", ros2_control_params],
arguments=['ur_joint_state_broadcaster', '-c', 'ur5e/controller_manager'] + controller_manager_timeout,
)

robot_state_publisher = Node(
package="robot_state_publisher",
namespace="ur5e",
executable="robot_state_publisher",
output="screen",
parameters=[{"robot_description": '<robot name=""><link name=""/></robot>'}],
package='robot_state_publisher',
namespace='ur5e',
executable='robot_state_publisher',
output='screen',
parameters=[{
'robot_description': '<robot name=""><link name=""/></robot>'
}],
)

return LaunchDescription(
[
# Request to spawn the URDF robot
spawn_URDF_ur5e,
# Other ROS 2 nodes
robot_state_publisher,
trajectory_controller_spawner,
joint_state_broadcaster_spawner,
# Launch the driver node once the URDF robot is spawned
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessIO(
target_action=spawn_URDF_ur5e,
on_stdout=lambda event: get_webots_driver_node(
event, universal_robot_driver
),
)
),
# Kill all the nodes when the driver node is shut down
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=universal_robot_driver,
on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
)
),
]
)
return LaunchDescription([
# Request to spawn the URDF robot
spawn_URDF_ur5e,

# Other ROS 2 nodes
robot_state_publisher,
trajectory_controller_spawner,
joint_state_broadcaster_spawner,

# Launch the driver node once the URDF robot is spawned
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessIO(
target_action=spawn_URDF_ur5e,
on_stdout=lambda event: get_webots_driver_node(event, universal_robot_driver),
)
),

# Kill all the nodes when the driver node is shut down
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=universal_robot_driver,
on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
)
),
])

0 comments on commit 10fc127

Please sign in to comment.