131 lines
4.5 KiB
Python
131 lines
4.5 KiB
Python
#!/usr/bin/env python
|
|
|
|
# Copyright 1996-2023 Cyberbotics Ltd.
|
|
#
|
|
# 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.
|
|
|
|
"""Launch Webots Universal Robot simulation nodes."""
|
|
|
|
import os
|
|
import launch
|
|
from launch_ros.actions import Node
|
|
from launch import LaunchDescription
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from webots_ros2_driver.urdf_spawner import URDFSpawner, get_webots_driver_node
|
|
from webots_ros2_driver.webots_controller import WebotsController
|
|
from rbs_utils.launch import load_xacro_args
|
|
from robot_builder.external.ros2_control import ControllerManager
|
|
from robot_builder.parser.urdf import URDF_parser
|
|
import xacro
|
|
|
|
|
|
PACKAGE_NAME = 'aubo_description'
|
|
|
|
|
|
def generate_launch_description():
|
|
package_dir = get_package_share_directory(PACKAGE_NAME)
|
|
robot_description_path = os.path.join(package_dir, 'urdf', 'aubo_i5.urdf.xacro')
|
|
controllers_file = os.path.join(package_dir, 'config', 'controllers.yaml')
|
|
|
|
mappings_data = load_xacro_args(robot_description_path, locals())
|
|
|
|
robot_description_doc = xacro.process_file(
|
|
robot_description_path, mappings={"robot_ip": "", "simulation_controllers": controllers_file, "hardware": "webots", "with_gripper": "false"}
|
|
)
|
|
|
|
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
|
|
# with open("current.urdf", "w") as file:
|
|
# file.write(robot_description_content)
|
|
# file.close()
|
|
|
|
robot = URDF_parser.load_string(
|
|
robot_description_content,
|
|
base_link_name="base_link",
|
|
ee_link_name="wrist3_Link",
|
|
)
|
|
ControllerManager.save_to_yaml(
|
|
robot, package_dir, "controllers.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_aubo = URDFSpawner(
|
|
name='Aubo-i5',
|
|
robot_description=robot_description_content,
|
|
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.
|
|
aubo_robot_driver = WebotsController(
|
|
robot_name='Aubo-i5',
|
|
parameters=[
|
|
{'robot_description': robot_description_content},
|
|
{'use_sim_time': True},
|
|
{'set_robot_state_publisher': True},
|
|
controllers_file
|
|
],
|
|
)
|
|
|
|
# Other ROS 2 nodes
|
|
controller_manager_timeout = ['--controller-manager-timeout', '100']
|
|
trajectory_controller_spawner = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
output='screen',
|
|
arguments=['cartesian_motion_controller'] + controller_manager_timeout,
|
|
)
|
|
|
|
joint_state_broadcaster_spawner = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
output='screen',
|
|
arguments=['joint_state_broadcaster'] + controller_manager_timeout,
|
|
)
|
|
|
|
robot_state_publisher = Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
output='screen',
|
|
parameters=[{
|
|
'robot_description': '<robot name=""><link name=""/></robot>'
|
|
}],
|
|
)
|
|
|
|
return LaunchDescription([
|
|
# Request to spawn the URDF robot
|
|
spawn_URDF_aubo,
|
|
|
|
# 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_aubo,
|
|
on_stdout=lambda event: get_webots_driver_node(event, aubo_robot_driver),
|
|
)
|
|
),
|
|
|
|
# Kill all the nodes when the driver node is shut down
|
|
launch.actions.RegisterEventHandler(
|
|
event_handler=launch.event_handlers.OnProcessExit(
|
|
target_action=aubo_robot_driver,
|
|
on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
|
|
)
|
|
),
|
|
])
|