Refactor launch files and update robot description for iiwa7 configuration

src/iiwa_bringup/launch/iiwa.launch.py

@@ -5,6 +5,8 @@ from launch.actions import (
     IncludeLaunchDescription,
     OpaqueFunction,
     RegisterEventHandler,
+    SetEnvironmentVariable,
+    # TimerAction,
 )
 from launch.conditions import IfCondition
 from launch.event_handlers import OnProcessExit
@@ -14,54 +16,92 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from moveit_configs_utils import MoveItConfigsBuilder
+from ament_index_python.packages import get_package_share_directory
 
 from iiwa_utils import converter, setting_loader
 
-import yaml
-import tempfile
-
-def wrap_for_ros2_params(yaml_path: str, namespace: str) -> str:
-    with open(yaml_path, "r") as f:
-        data = yaml.safe_load(f)
-
-    wrapped = {namespace: {"ros__parameters": data}}
-
-    tmp = tempfile.NamedTemporaryFile(
-        mode="w", suffix=".yaml", delete=False
-    )
-    yaml.dump(wrapped, tmp, default_flow_style=False)
-    tmp.close()
-    return tmp.name
-
 
 def _runtime_setup(context, *args, **kwargs):
+    setup = []
+
+    # Настройка параметров
+    simulate = LaunchConfiguration("simulate").perform(context) in ("true", "1", "yes")
+
     settings = setting_loader.build_settings(
-        settings_path=LaunchConfiguration("setting").perform(context), check_files=True
+        settings_path=LaunchConfiguration("setting").perform(context),
+        check_files=True,
     )
 
-    xacro_args = {
-        "initial_positions_file": settings.controller.moveit.initial_positions,
-        "robot_ip": settings.robot.ip,
-        "fri_port": str(settings.robot.port),
-        "simulate": "false",
-        "command_mode": settings.robot.command_mode,
-    }
+    joint_limits_ros2 = converter.wrap_for_ros2_params(
+        settings.controller.moveit.joint_limits,
+        "robot_description_planning",
+    )
+    kinematics_ros2 = converter.wrap_for_ros2_params(
+        settings.controller.moveit.kinematics,
+        "robot_description_kinematics",
+    )
+
+    description_path = settings.robot.description
+
+    if simulate:
+        xacro_args = {
+            "initial_positions_file": settings.controller.moveit.initial_positions,
+            "simulate": "true",
+        }
+        use_sim_time = True
+    else:
+        xacro_args = {
+            "initial_positions_file": settings.controller.moveit.initial_positions,
+            "robot_ip": settings.robot.ip,
+            "fri_port": str(settings.robot.port),
+            "simulate": "false",
+            "command_mode": settings.robot.command_mode,
+        }
+        use_sim_time = False
 
     robot_description = converter.load_robot_description(
-        model_path=settings.robot.description,
+        model_path=description_path,
         robot_name=settings.robot.name,
         xacro_args=xacro_args,
     )
 
+    # Вызов нод
     rsp_node = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
         name="robot_state_publisher",
         output="screen",
-        parameters=[{"robot_description": robot_description, 
-                     "use_sim_time": False}],
+        parameters=[
+            {"robot_description": robot_description},
+            {"use_sim_time": use_sim_time},
+        ],
     )
 
+    setup += [rsp_node]
+
+    # gazebo spawn
+    if simulate:
+        gazebo_launch = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("iiwa_bringup"),
+                        "launch", "supported", "gazebo.launch.py",
+                    ]
+                )
+            ),
+            # TODO: добавить аргументы для transform и rotation и передать их из setting.yaml а также мир
+            launch_arguments={
+                "robot_name": settings.robot.name,
+                "world": "empty.sdf",
+                "gazebo_config": str(get_package_share_directory("iiwa_config") + "/config/gazebo/gz_bridge.yaml"),
+                "simulate": "true",
+            }.items(),
+        )
+
+        setup += [gazebo_launch]
+    
+    # Controller launch
     controllers_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -69,23 +109,24 @@ def _runtime_setup(context, *args, **kwargs):
                     FindPackageShare("iiwa_bringup"),
                     "launch",
                     "supported",
-                    "iiwa_controllers.launch.py"
+                    "controllers.launch.py",
                 ]
             )
         ),
         launch_arguments={
-            "robot_name": str(settings.robot.name),
-            "description": str(settings.robot.description),
-            "initial_positions_file": str(settings.controller.moveit.initial_positions),
-            "controller_path": str(settings.controller.controller_path)
-        }.items()
+            "robot_name": settings.robot.name,
+            "description": description_path,
+            "initial_positions_file": settings.controller.moveit.initial_positions,
+            # "simulate": str(simulate),
+            "controller_path": settings.controller.controller_path,
+        }.items(),
     )
 
-    # Moveit
+    # Moveit launch
     moveit_configs = (
         MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
         .robot_description(
-            file_path=settings.robot.description,
+            file_path=description_path,
             mappings={
                 "initial_positions_file": settings.controller.moveit.initial_positions
             },
@@ -106,19 +147,11 @@ def _runtime_setup(context, *args, **kwargs):
         parameters=[
             moveit_configs.to_dict(),
             {"robot_description": robot_description},
+            {"use_sim_time": use_sim_time},
         ],
     )
 
-
-    joint_limits_ros2  = wrap_for_ros2_params(
-        settings.controller.moveit.joint_limits,
-        "robot_description_planning"
-    )
-    kinematics_ros2 = wrap_for_ros2_params(
-        settings.controller.moveit.kinematics,
-        "robot_description_kinematics"
-    )
-
+    # Rviz launch
     rviz_launch = Node(
         condition=IfCondition(LaunchConfiguration("rviz")),
         package="rviz2",
@@ -129,48 +162,62 @@ def _runtime_setup(context, *args, **kwargs):
         parameters=[
             moveit_configs.robot_description,
             moveit_configs.robot_description_semantic,
-            # moveit_configs.robot_description_kinematics,
             moveit_configs.planning_pipelines,
-            # moveit_configs.joint_limits,
             joint_limits_ros2,
-            kinematics_ros2, 
+            kinematics_ros2,
+            {"use_sim_time": use_sim_time},
         ],
     )
 
     shutdown_on_rviz_exit = RegisterEventHandler(
-        OnProcessExit(target_action=rviz_launch, on_exit=[EmitEvent(event=Shutdown())])
+        OnProcessExit(
+            target_action=rviz_launch,
+            on_exit=[EmitEvent(event=Shutdown())],
+        )
     )
-    
-    return [
-        rsp_node,
-        controllers_launch,
-        move_group,
-        rviz_launch,
+
+    setup += [
+        controllers_launch, 
+        move_group, 
+        rviz_launch, 
         shutdown_on_rviz_exit
     ]
 
+    return setup
 
 def generate_launch_description():
-    declare_rviz = DeclareLaunchArgument(
-        name="rviz",
-        default_value="0",
-        description="If true|1|yes then launch RViz/MoveIt branch (instead of controllers branch)",
+    declare_simulate = DeclareLaunchArgument(
+        name="simulate",
+        default_value="false",
+        description="true = Gazebo симуляция, false = реальный робот через FRI",
     )
 
-    declacre_setting = DeclareLaunchArgument(
+    declare_rviz = DeclareLaunchArgument(
+        name="rviz",
+        default_value="false",
+        description="true = запустить RViz",
+    )
+
+    declare_setting = DeclareLaunchArgument(
         name="setting",
         default_value=PathJoinSubstitution(
             [FindPackageShare("iiwa_config"), "config", "setting.yaml"]
         ),
-        description="Absolute path to settings file",
+        description="Путь к файлу настроек",
+    )
+
+    gz_resource_path = SetEnvironmentVariable(
+        name="GZ_SIM_RESOURCE_PATH",
+        value=get_package_share_directory("iiwa_description") + "/..",
     )
 
     runtime_setup = OpaqueFunction(function=_runtime_setup)
 
-    return LaunchDescription(
-        [
-            declare_rviz,
-            declacre_setting,
-            runtime_setup,
-        ]
-    )
+    return LaunchDescription([
+        gz_resource_path,
+        declare_simulate,
+        declare_rviz,
+        declare_setting,
+        runtime_setup,
+    ])
+

src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py

@@ -1,65 +0,0 @@
-from launch.substitutions import LaunchConfiguration
-from launch.actions import RegisterEventHandler
-from launch.event_handlers import OnProcessExit
-from launch.actions import OpaqueFunction
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-
-from iiwa_utils import converter
-
-
-def _setup_controllers(context, *args, **kwargs):
-    robot_name = LaunchConfiguration("robot_name").perform(context)
-    description = LaunchConfiguration("description").perform(context)
-    initial_positions_file = LaunchConfiguration("initial_positions_file").perform(
-        context
-    )
-    controller_path = LaunchConfiguration("controller_path").perform(context)
-
-    robot_description = converter.load_robot_description(
-        model_path=description,
-        robot_name=robot_name,
-        xacro_args={"initial_positions_file": initial_positions_file},
-    )
-
-    ros2_control_node = Node(
-        package="controller_manager",
-        executable="ros2_control_node",
-        output="screen",
-        parameters=[
-            {"robot_description": robot_description},
-            controller_path,
-        ],
-    )
-
-    joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
-        output="screen",
-    )
-
-    arm_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["iiwa_arm_controller", "--controller-manager", "/controller_manager"],
-        output="screen",
-    )
-
-    arm_controller_after_jsb = RegisterEventHandler(
-        OnProcessExit(
-            target_action=joint_state_broadcaster_spawner,
-            on_exit=[arm_controller_spawner],
-        )
-    )
-
-    return [
-        ros2_control_node,
-        joint_state_broadcaster_spawner,
-        arm_controller_after_jsb,
-    ]
-
-
-def generate_launch_description():
-    return LaunchDescription([OpaqueFunction(function=_setup_controllers)])

src/iiwa_bringup/launch/universal.launch.py

@@ -1,223 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-    EmitEvent,
-    IncludeLaunchDescription,
-    OpaqueFunction,
-    RegisterEventHandler,
-    SetEnvironmentVariable,
-    # TimerAction,
-)
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
-from launch.events import Shutdown
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from moveit_configs_utils import MoveItConfigsBuilder
-from ament_index_python.packages import get_package_share_directory
-
-from iiwa_utils import converter, setting_loader
-
-
-def _runtime_setup(context, *args, **kwargs):
-    setup = []
-
-    # Настройка параметров
-    simulate = LaunchConfiguration("simulate").perform(context) in ("true", "1", "yes")
-
-    settings = setting_loader.build_settings(
-        settings_path=LaunchConfiguration("setting").perform(context),
-        check_files=True,
-    )
-
-    joint_limits_ros2 = converter.wrap_for_ros2_params(
-        settings.controller.moveit.joint_limits,
-        "robot_description_planning",
-    )
-    kinematics_ros2 = converter.wrap_for_ros2_params(
-        settings.controller.moveit.kinematics,
-        "robot_description_kinematics",
-    )
-
-    description_path = settings.robot.description
-
-    if simulate:
-        xacro_args = {
-            "initial_positions_file": settings.controller.moveit.initial_positions,
-            "simulate": "true",
-        }
-        use_sim_time = True
-    else:
-        xacro_args = {
-            "initial_positions_file": settings.controller.moveit.initial_positions,
-            "robot_ip": settings.robot.ip,
-            "fri_port": str(settings.robot.port),
-            "simulate": "false",
-            "command_mode": settings.robot.command_mode,
-        }
-        use_sim_time = False
-
-    robot_description = converter.load_robot_description(
-        model_path=description_path,
-        robot_name=settings.robot.name,
-        xacro_args=xacro_args,
-    )
-
-    # Вызов нод
-    rsp_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        name="robot_state_publisher",
-        output="screen",
-        parameters=[
-            {"robot_description": robot_description},
-            {"use_sim_time": use_sim_time},
-        ],
-    )
-
-    setup += [rsp_node]
-
-    # gazebo spawn
-    if simulate:
-        gazebo_launch = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("iiwa_bringup"),
-                        "launch", "supported", "gazebo.launch.py",
-                    ]
-                )
-            ),
-            # TODO: добавить аргументы для transform и rotation и передать их из setting.yaml а также мир
-            launch_arguments={
-                "robot_name": settings.robot.name,
-                "world": "empty.sdf",
-                "gazebo_config": str(get_package_share_directory("iiwa_config") + "/config/gazebo/gz_bridge.yaml"),
-                "simulate": "true",
-            }.items(),
-        )
-
-        setup += [gazebo_launch]
-    
-    # Controller launch
-    controllers_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("iiwa_bringup"),
-                    "launch",
-                    "supported",
-                    "controllers.launch.py",
-                ]
-            )
-        ),
-        launch_arguments={
-            "robot_name": settings.robot.name,
-            "description": description_path,
-            "initial_positions_file": settings.controller.moveit.initial_positions,
-            # "simulate": str(simulate),
-            "controller_path": settings.controller.controller_path,
-        }.items(),
-    )
-
-    # Moveit launch
-    moveit_configs = (
-        MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
-        .robot_description(
-            file_path=description_path,
-            mappings={
-                "initial_positions_file": settings.controller.moveit.initial_positions
-            },
-        )
-        .robot_description_semantic(file_path=settings.controller.moveit.srdf)
-        .robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
-        .joint_limits(file_path=settings.controller.moveit.joint_limits)
-        .pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
-        .trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
-        .moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
-        .to_moveit_configs()
-    )
-
-    move_group = Node(
-        package="moveit_ros_move_group",
-        executable="move_group",
-        output="screen",
-        parameters=[
-            moveit_configs.to_dict(),
-            {"robot_description": robot_description},
-            {"use_sim_time": use_sim_time},
-        ],
-    )
-
-    # Rviz launch
-    rviz_launch = Node(
-        condition=IfCondition(LaunchConfiguration("rviz")),
-        package="rviz2",
-        executable="rviz2",
-        name="rviz2",
-        arguments=["-d", settings.digital_twin.rviz.config],
-        output="log",
-        parameters=[
-            moveit_configs.robot_description,
-            moveit_configs.robot_description_semantic,
-            moveit_configs.planning_pipelines,
-            joint_limits_ros2,
-            kinematics_ros2,
-            {"use_sim_time": use_sim_time},
-        ],
-    )
-
-    shutdown_on_rviz_exit = RegisterEventHandler(
-        OnProcessExit(
-            target_action=rviz_launch,
-            on_exit=[EmitEvent(event=Shutdown())],
-        )
-    )
-
-    setup += [
-        controllers_launch, 
-        move_group, 
-        rviz_launch, 
-        shutdown_on_rviz_exit
-    ]
-
-    return setup
-
-def generate_launch_description():
-    declare_simulate = DeclareLaunchArgument(
-        name="simulate",
-        default_value="false",
-        description="true = Gazebo симуляция, false = реальный робот через FRI",
-    )
-
-    declare_rviz = DeclareLaunchArgument(
-        name="rviz",
-        default_value="false",
-        description="true = запустить RViz",
-    )
-
-    declare_setting = DeclareLaunchArgument(
-        name="setting",
-        default_value=PathJoinSubstitution(
-            [FindPackageShare("iiwa_config"), "config", "setting.yaml"]
-        ),
-        description="Путь к файлу настроек",
-    )
-
-    gz_resource_path = SetEnvironmentVariable(
-        name="GZ_SIM_RESOURCE_PATH",
-        value=get_package_share_directory("iiwa_description") + "/..",
-    )
-
-    runtime_setup = OpaqueFunction(function=_runtime_setup)
-
-    return LaunchDescription([
-        gz_resource_path,
-        declare_simulate,
-        declare_rviz,
-        declare_setting,
-        runtime_setup,
-    ])
-

src/iiwa_config/config/setting.yaml

@@ -3,7 +3,7 @@ robot:
   ip: "192.170.10.2"
   port: 30200
   command_mode: "position" # torque, position
-  description: pkg://iiwa_description/urdf/iiwa7_fri.urdf.xacro
+  description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 
 digital_twin:

src/iiwa_description/urdf/iiwa7_digital.urdf.xacro

@@ -4,13 +4,6 @@
     <!-- Аргукменты -->
     <xacro:arg name="initial_positions_file"
                default="$(find iiwa_config)/config/moveit/initial_positions.yaml"/>
-    <xacro:arg name="simulate" default="false"/>
-
-    <!-- Аргументы только для реального робота -->
-    <xacro:arg name="robot_ip" default="192.170.10.2"/>
-    <xacro:arg name="fri_port" default="30200"/>
-    <xacro:arg name="command_mode" default="position"/>
-
 
     <xacro:property name="initial_positions"
                     value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -22,43 +15,15 @@
 
     <xacro:include filename="$(find iiwa_description)/urdf/gripper/gripper.xacro"/>
 
-    <xacro:if value="$(arg simulate)">
-        <gazebo>
-            <plugin filename="gz_ros2_control-system"
-                    name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-                <parameters>$(find iiwa_config)/config/moveit/iiwa_controller.yaml</parameters>
-                <ros>
-                    <remapping>~/robot_description:=/robot_description</remapping>
-                </ros>
-            </plugin>
-        </gazebo>
-    </xacro:if>
-
-    <!-- <webots>
+    <webots>
         <plugin type="webots_ros2_control::Ros2Control"/>
-    </webots> -->
+    </webots>
 
-    <ros2_control name="iiwaControl" type="system">
+    <ros2_control name="iiwaGazeboControl" type="system">
 
-        <xacro:if value="$(arg simulate)">
-            <hardware>
-                <plugin>gz_ros2_control/GazeboSimSystem</plugin>
-            </hardware>
-        </xacro:if>
-
-        <hardware>
-            <plugin>iiwa_controller/IIWAHardwareInterface</plugin>
-
-            <param name="robot_ip">$(arg robot_ip)</param>
-            <param name="fri_port">$(arg fri_port)</param>
-            <param name="simulate">$(arg simulate)</param>
-            <param name="command_mode">$(arg command_mode)</param>
-
-        </hardware>
-
-         <!-- <hardware>
+         <hardware>
             <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
-        </hardware> -->
+        </hardware>
 
         <joint name="joint1">
             <command_interface name="position">
@@ -144,14 +109,6 @@
             <state_interface name="effort"/>
         </joint>
 
-        <joint name="base_screw">
-            <command_interface name="position"/>
-            <state_interface name="position">
-                <param name="initial_value">0.0</param>
-            </state_interface>
-            <state_interface name="velocity"/>
-        </joint>
-
     </ros2_control>
 
 </robot>