Refactor URDF and launch files for iiwa robot
- Updated gripper macros in gripper_macros.xacro to simplify parameters and add Gazebo support. - Removed unused mesh property in gripper_meshes.xacro. - Enhanced iiwa7 digital and FRI URDF files with simulation arguments and improved controller setup. - Added new launch files for Gazebo simulation and universal launch configuration. - Implemented YAML parameter wrapping for ROS2 in converter.py. - Introduced a new gz_bridge.yaml configuration for Gazebo to ROS2 topic mapping. - Cleaned up setting_loader.py and added necessary imports.
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,117 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.actions import RegisterEventHandler
|
||||
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)
|
||||
simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")
|
||||
|
||||
robot_description = converter.load_robot_description(
|
||||
model_path=description,
|
||||
robot_name=robot_name,
|
||||
xacro_args={"initial_positions_file": initial_positions_file},
|
||||
)
|
||||
|
||||
# СИМУЛЯЦИЯ (Gazebo)
|
||||
if simulate:
|
||||
jsb = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
output="screen",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager", "/controller_manager",
|
||||
"--controller-manager-timeout", "30",
|
||||
],
|
||||
parameters=[{"use_sim_time": True}],
|
||||
)
|
||||
|
||||
jtc = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
output="screen",
|
||||
arguments=[
|
||||
"iiwa_arm_controller",
|
||||
"--controller-manager", "/controller_manager",
|
||||
"--controller-manager-timeout", "30",
|
||||
],
|
||||
parameters=[{"use_sim_time": True}],
|
||||
)
|
||||
|
||||
jtc_after_jsb = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=jsb,
|
||||
on_exit=[jtc],
|
||||
)
|
||||
)
|
||||
|
||||
return [jsb, jtc_after_jsb]
|
||||
|
||||
# РЕАЛЬНЫЙ РОБОТ (FRI)
|
||||
else:
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{"robot_description": robot_description},
|
||||
controller_path,
|
||||
],
|
||||
)
|
||||
|
||||
jsb = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
output="screen",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager", "/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
jtc = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
output="screen",
|
||||
arguments=[
|
||||
"iiwa_arm_controller",
|
||||
"--controller-manager", "/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
torque_controller = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
output="screen",
|
||||
arguments=[
|
||||
"forward_torque_controller",
|
||||
"--controller-manager", "/controller_manager",
|
||||
"--inactive",
|
||||
],
|
||||
)
|
||||
|
||||
jtc_after_jsb = RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=jsb,
|
||||
on_exit=[jtc, torque_controller],
|
||||
)
|
||||
)
|
||||
|
||||
return [
|
||||
ros2_control_node,
|
||||
jsb,
|
||||
jtc_after_jsb,
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([OpaqueFunction(function=_setup_controllers)])
|
||||
@@ -0,0 +1,63 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
IncludeLaunchDescription,
|
||||
OpaqueFunction,
|
||||
)
|
||||
from launch_ros.actions import Node
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def _spawn_setup(context, *args, **kwargs):
|
||||
robot_name = LaunchConfiguration("robot_name").perform(context)
|
||||
world = LaunchConfiguration("world").perform(context)
|
||||
gazebo_config = LaunchConfiguration("gazebo_config").perform(context)
|
||||
simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes")
|
||||
# transform = LaunchConfiguration("transform").perform(context)
|
||||
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("ros_gz_sim"),
|
||||
"launch",
|
||||
"gz_sim.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"gz_args": f"-r {world}",
|
||||
"on_exit_shutdown": "true",
|
||||
}.items(),
|
||||
)
|
||||
|
||||
# TODO: добавить аргументы для transform и rotation
|
||||
spawn_robot = Node(
|
||||
package="ros_gz_sim",
|
||||
executable="create",
|
||||
name="spawn_iiwa7",
|
||||
output="screen",
|
||||
arguments=[
|
||||
"-topic", "/robot_description",
|
||||
"-name", robot_name,
|
||||
"-z", "0.0"
|
||||
],
|
||||
)
|
||||
|
||||
gz_bridge = Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
name="gz_bridge",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"config_file": gazebo_config,
|
||||
"use_sim_time": simulate
|
||||
}],
|
||||
)
|
||||
|
||||
return [gazebo, spawn_robot, gz_bridge]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([OpaqueFunction(function=_spawn_setup)])
|
||||
@@ -33,7 +33,6 @@ def _setup_controllers(context, *args, **kwargs):
|
||||
],
|
||||
)
|
||||
|
||||
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
|
||||
@@ -0,0 +1,223 @@
|
||||
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,
|
||||
])
|
||||
|
||||
Reference in New Issue
Block a user