diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py
index 44c1a42..e233d16 100644
--- a/src/iiwa_bringup/launch/iiwa.launch.py
+++ b/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,
- ]
- )
\ No newline at end of file
+ return LaunchDescription([
+ gz_resource_path,
+ declare_simulate,
+ declare_rviz,
+ declare_setting,
+ runtime_setup,
+ ])
+
diff --git a/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py b/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py
deleted file mode 100644
index 9be7d59..0000000
--- a/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py
+++ /dev/null
@@ -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)])
\ No newline at end of file
diff --git a/src/iiwa_bringup/launch/universal.launch.py b/src/iiwa_bringup/launch/universal.launch.py
deleted file mode 100644
index e233d16..0000000
--- a/src/iiwa_bringup/launch/universal.launch.py
+++ /dev/null
@@ -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,
- ])
-
diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml
index 450ae7c..7f12971 100644
--- a/src/iiwa_config/config/setting.yaml
+++ b/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:
diff --git a/src/iiwa_description/urdf/iiwa7_fri.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro
similarity index 100%
rename from src/iiwa_description/urdf/iiwa7_fri.urdf.xacro
rename to src/iiwa_description/urdf/iiwa7.urdf.xacro
diff --git a/src/iiwa_description/urdf/iiwa7_digital.urdf.xacro b/src/iiwa_description/urdf/iiwa7_digital.urdf.xacro
index 9d77780..6b9af09 100644
--- a/src/iiwa_description/urdf/iiwa7_digital.urdf.xacro
+++ b/src/iiwa_description/urdf/iiwa7_digital.urdf.xacro
@@ -4,13 +4,6 @@
-
-
-
-
-
-
-
@@ -22,43 +15,15 @@
-
-
-
- $(find iiwa_config)/config/moveit/iiwa_controller.yaml
-
- ~/robot_description:=/robot_description
-
-
-
-
-
-
+
-
+
-
-
- gz_ros2_control/GazeboSimSystem
-
-
-
-
- iiwa_controller/IIWAHardwareInterface
-
- $(arg robot_ip)
- $(arg fri_port)
- $(arg simulate)
- $(arg command_mode)
-
-
-
-
+
@@ -144,14 +109,6 @@
-
-
-
- 0.0
-
-
-
-
\ No newline at end of file