From 033e14d80e040c9bbe0215157f4d646ff2b46533 Mon Sep 17 00:00:00 2001 From: Ilua Uraev Date: Tue, 3 Jun 2025 01:21:06 +0300 Subject: [PATCH] feat: add webots integration --- launch/robot_nodes_launch.py | 131 +++++++++ launch/robot_world_launch.py | 55 ++++ urdf/aubo_i5.urdf | 495 +++++++++++++++++++---------------- urdf/aubo_i5.urdf.xacro | 22 +- urdf/ros2_control.xacro | 13 +- urdf/xacro_args.yaml | 2 +- world/aubo_robot.wbt | 49 ++++ 7 files changed, 524 insertions(+), 243 deletions(-) create mode 100644 launch/robot_nodes_launch.py create mode 100644 launch/robot_world_launch.py create mode 100644 world/aubo_robot.wbt diff --git a/launch/robot_nodes_launch.py b/launch/robot_nodes_launch.py new file mode 100644 index 0000000..84b8886 --- /dev/null +++ b/launch/robot_nodes_launch.py @@ -0,0 +1,131 @@ +#!/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': '' + }], + ) + + 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())], + ) + ), + ]) diff --git a/launch/robot_world_launch.py b/launch/robot_world_launch.py new file mode 100644 index 0000000..e85d76e --- /dev/null +++ b/launch/robot_world_launch.py @@ -0,0 +1,55 @@ +#!/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 world.""" + +import launch +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.substitutions.path_join_substitution import PathJoinSubstitution +from webots_ros2_driver.webots_launcher import WebotsLauncher + + +PACKAGE_NAME = 'aubo_description' + + +def generate_launch_description(): + package_dir = get_package_share_directory(PACKAGE_NAME) + world = LaunchConfiguration('world') + + # Starts Webots + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'world', world]), + ros2_supervisor=True + ) + + return LaunchDescription([ + DeclareLaunchArgument( + 'world', + default_value='aubo_robot.wbt', + ), + webots, + webots._supervisor, + # This action will kill all nodes once the Webots simulation has exited + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=webots, + on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + ) + ) + ]) diff --git a/urdf/aubo_i5.urdf b/urdf/aubo_i5.urdf index c0e9e9a..1143777 100755 --- a/urdf/aubo_i5.urdf +++ b/urdf/aubo_i5.urdf @@ -1,220 +1,275 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + webots_ros2_control::Ros2ControlSystem + + + + + + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.0 + + + + + + + + diff --git a/urdf/aubo_i5.urdf.xacro b/urdf/aubo_i5.urdf.xacro index d099f34..732b04c 100644 --- a/urdf/aubo_i5.urdf.xacro +++ b/urdf/aubo_i5.urdf.xacro @@ -1,9 +1,4 @@ - - - - - @@ -243,19 +238,12 @@ - - - - - - - - - - - - + + + + + diff --git a/urdf/ros2_control.xacro b/urdf/ros2_control.xacro index 2827d67..94c1062 100644 --- a/urdf/ros2_control.xacro +++ b/urdf/ros2_control.xacro @@ -13,6 +13,9 @@ aubo_driver/AuboHardwareInterface ${robot_ip} + + webots_ros2_control::Ros2ControlSystem + @@ -35,7 +38,7 @@ 0.5 - + @@ -51,7 +54,7 @@ 1.75 - + @@ -66,7 +69,7 @@ 0.0 - + @@ -81,7 +84,7 @@ 1.57 - + @@ -91,7 +94,7 @@ - + 0.0 diff --git a/urdf/xacro_args.yaml b/urdf/xacro_args.yaml index 51557a8..eaa728f 100644 --- a/urdf/xacro_args.yaml +++ b/urdf/xacro_args.yaml @@ -1,4 +1,4 @@ simulation_controllers: !variable controllers_file robot_ip: 192.168.0.106 -hardware: gazebo +hardware: webots with_gripper: "true" diff --git a/world/aubo_robot.wbt b/world/aubo_robot.wbt new file mode 100644 index 0000000..d3e7b99 --- /dev/null +++ b/world/aubo_robot.wbt @@ -0,0 +1,49 @@ +#VRML_SIM R2023b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/appearances/protos/ThreadMetalPlate.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/solids/protos/SolidBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/appearances/protos/GalvanizedMetal.proto" + +WorldInfo { + info [ + "Aubo i5 Robot" + ] + title "Aubo Robot" + basicTimeStep 16 + physicsDisableAngularThreshold 0.1 + contactProperties [ + ContactProperties { + bounce 0.1 + } + ContactProperties { + material1 "slope" + coulombFriction [ + 0.5 + ] + } + ] +} +Viewpoint { + orientation 0.3118460054718159 0.06120895855528569 -0.9481590226664772 2.710827783218323 + position 2.9308358456729615 1.1235070287382412 2.814381048353761 + exposure 1.7 +} +TexturedBackground { + texture "factory" +} +Floor { + size 20 20 + appearance ThreadMetalPlate { + } +} +SolidBox { + translation 0 0 0.3 + size 0.3 0.3 0.6 + appearance GalvanizedMetal { + textureTransform TextureTransform { + scale 2 2 + } + } +}