312 lines
11 KiB
Python
312 lines
11 KiB
Python
import os
|
|
import subprocess
|
|
import yaml
|
|
|
|
from launch import LaunchDescription
|
|
from launch_ros.actions import Node, SetParameter
|
|
from launch.actions import IncludeLaunchDescription, RegisterEventHandler, LogInfo, ExecuteProcess, ExecuteLocal, TimerAction
|
|
from launch_ros.substitutions import FindPackageShare
|
|
from launch.substitutions import PathJoinSubstitution
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch.event_handlers import OnProcessExit
|
|
from launch.launch_description_entity import LaunchDescriptionEntity
|
|
from pathlib import Path
|
|
|
|
|
|
def load_file(package_name, file_path):
|
|
package_path = get_package_share_directory(package_name)
|
|
absolute_file_path = os.path.join(package_path, file_path)
|
|
|
|
try:
|
|
with open(absolute_file_path, 'r') as file:
|
|
print("Opened file")
|
|
return file.read()
|
|
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
|
print("ERROR: load_file " + package_name + " | " + file_path + " -> " + absolute_file_path)
|
|
return None
|
|
|
|
|
|
def load_xacro(package_name, file_path):
|
|
package_path = get_package_share_directory(package_name)
|
|
absolute_file_path = os.path.join(package_path, file_path)
|
|
|
|
process = subprocess.run(["xacro", absolute_file_path], capture_output=True)
|
|
if process.returncode != 0:
|
|
print(process.stderr)
|
|
return process.stdout.decode("UTF-8")
|
|
|
|
|
|
def load_yaml(package_name, file_path):
|
|
package_path = get_package_share_directory(package_name)
|
|
absolute_file_path = os.path.join(package_path, file_path)
|
|
|
|
try:
|
|
with open(absolute_file_path, 'r') as file:
|
|
print("Opened file")
|
|
return yaml.safe_load(file)
|
|
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
|
print("ERROR: load_yaml " + package_name + " | " + file_path + " -> " + absolute_file_path)
|
|
|
|
return None
|
|
|
|
|
|
def launch_chain(nodes: list[ExecuteLocal]):
|
|
generatedNodes: list[LaunchDescriptionEntity] = []
|
|
for index in range(1, len(nodes)):
|
|
generatedNodes.append(RegisterEventHandler(
|
|
OnProcessExit(
|
|
target_action=nodes[index-1],
|
|
on_exit=nodes[index]
|
|
)
|
|
))
|
|
generatedNodes.append(nodes[0])
|
|
return generatedNodes
|
|
|
|
|
|
def generate_launch_description():
|
|
|
|
robot_description_config = load_xacro('iisy_config', 'urdf/iisy.urdf')
|
|
robot_description = {'robot_description': robot_description_config}
|
|
|
|
robot_description_semantic_config = load_file('iisy_config', 'srdf/iisy.srdf')
|
|
robot_description_semantic = {'robot_description_semantic': robot_description_semantic_config}
|
|
|
|
kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
|
|
robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml}
|
|
|
|
controllers = {
|
|
"moveit_simple_controller_manager": load_yaml("iisy_config", "config/iisy_controllers.yml"),
|
|
"moveit_controller_manager": "moveit_ros_control_interface/Ros2ControlMultiManager",
|
|
}
|
|
|
|
ompl_planning_pipeline_config = {
|
|
'move_group': {
|
|
'planning_plugin': 'ompl_interface/OMPLPlanner',
|
|
'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
|
|
'start_state_max_bounds_error': 0.1
|
|
}
|
|
}
|
|
ompl_planning_yaml = load_yaml('iisy_config', 'config/ompl_planning.yml')
|
|
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
|
|
|
|
robot_description_planning = {
|
|
"robot_description_planning": PathJoinSubstitution(
|
|
[
|
|
FindPackageShare("iisy_config"),
|
|
"config/joint_limits.yml"
|
|
]
|
|
)
|
|
}
|
|
|
|
trajectory_execution = {
|
|
'moveit_manage_controllers': False,
|
|
'trajectory_execution.allowed_execution_duration_scaling': 1.2,
|
|
'trajectory_execution.allowed_goal_duration_margin': 0.5,
|
|
'trajectory_execution.allowed_start_tolerance': 0.01
|
|
}
|
|
|
|
planning_scene_monitor_parameters = {
|
|
"publish_planning_scene": True,
|
|
"publish_geometry_updates": True,
|
|
"publish_state_updates": True,
|
|
"publish_transforms_updates": True
|
|
}
|
|
|
|
planning = {
|
|
"move_group": {
|
|
"planning_plugin": "ompl_interface/OMPLPlanner",
|
|
"request_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
|
|
"start_state_max_bounds_error": 0.1
|
|
}
|
|
}
|
|
|
|
spawn_robot = Node(
|
|
package='ros_gz_sim',
|
|
executable='create',
|
|
arguments=['-world', 'default', '-string', robot_description_config],
|
|
output='screen'
|
|
)
|
|
|
|
rviz = Node(
|
|
package='rviz2',
|
|
executable='rviz2',
|
|
arguments=['-d', os.path.join(get_package_share_directory("ign_world"), 'rviz', 'iisy.rviz')],
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_planning,
|
|
ompl_planning_pipeline_config,
|
|
robot_description_kinematics,
|
|
planning,
|
|
trajectory_execution,
|
|
controllers,
|
|
planning_scene_monitor_parameters,
|
|
],
|
|
)
|
|
|
|
controller_manager = Node(
|
|
package="controller_manager",
|
|
executable="ros2_control_node",
|
|
parameters=[
|
|
robot_description,
|
|
str(Path(get_package_share_directory("iisy_config")+"/config/iisy_controllers.yaml")),
|
|
],
|
|
)
|
|
|
|
load_joint_state_broadcaster = ExecuteProcess(
|
|
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
|
|
'joint_state_broadcaster'],
|
|
output='both'
|
|
)
|
|
|
|
load_joint_trajectory_controller = ExecuteProcess(
|
|
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
|
|
'joint_trajectory_controller'],
|
|
output='both'
|
|
)
|
|
|
|
move_group = Node(
|
|
package='moveit_ros_move_group',
|
|
executable='move_group',
|
|
# prefix='xterm -fs 10 -e gdb --ex run --args',
|
|
output='both',
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_planning,
|
|
ompl_planning_pipeline_config,
|
|
robot_description_kinematics,
|
|
planning,
|
|
trajectory_execution,
|
|
controllers,
|
|
planning_scene_monitor_parameters,
|
|
# octomap_config,
|
|
# octomap_updater_config
|
|
]
|
|
)
|
|
|
|
test = Node(
|
|
package='moveit_test',
|
|
executable='move',
|
|
output='both',
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_planning,
|
|
ompl_planning_pipeline_config,
|
|
robot_description_kinematics,
|
|
planning,
|
|
trajectory_execution,
|
|
controllers,
|
|
planning_scene_monitor_parameters,
|
|
# octomap_config,
|
|
# octomap_updater_config
|
|
],
|
|
)
|
|
|
|
# octomap_config = {'octomap_frame': 'camera_rgb_optical_frame',
|
|
# 'octomap_resolution': 0.05,
|
|
# 'max_range': 5.0}
|
|
|
|
# octomap_updater_config = load_yaml('moveit_tutorials', 'config/sensors_3d.yaml')
|
|
|
|
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
|
|
|
|
return LaunchDescription([
|
|
|
|
#IncludeLaunchDescription(
|
|
# PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
|
|
# launch_arguments={'gz_args': '-r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
|
|
#),
|
|
|
|
Node(
|
|
package='ros_gz_bridge',
|
|
executable='parameter_bridge',
|
|
arguments=["/clock[rosgraph_msgs/msg/Clock@ignition.msgs.Clock"],
|
|
output='both',
|
|
),
|
|
|
|
SetParameter(name="use_sim_time", value=True),
|
|
|
|
move_group,
|
|
|
|
#RegisterEventHandler(
|
|
# OnProcessExit(
|
|
# target_action=spawn_robot,
|
|
# on_exit=[load_joint_state_broadcaster]
|
|
# )
|
|
#),
|
|
|
|
controller_manager,
|
|
|
|
RegisterEventHandler(
|
|
OnProcessExit(
|
|
target_action=load_joint_state_broadcaster,
|
|
on_exit=[load_joint_trajectory_controller]
|
|
)
|
|
),
|
|
RegisterEventHandler(
|
|
OnProcessExit(
|
|
target_action=load_joint_trajectory_controller,
|
|
on_exit=TimerAction(period=10.0,actions=[test])
|
|
)
|
|
),
|
|
|
|
#spawn_robot,
|
|
load_joint_state_broadcaster,
|
|
#load_joint_trajectory_controller,
|
|
rviz,
|
|
|
|
Node(
|
|
package='ros_actor_action_server',
|
|
executable='ros_actor_action_server'),
|
|
|
|
Node(
|
|
package='tf2_ros',
|
|
executable='static_transform_publisher',
|
|
name='lidar_1_broadcaster',
|
|
arguments=[
|
|
"--x", "1",
|
|
"--y", "-1.9",
|
|
"--z", "1",
|
|
"--roll", "1.5707963267949",
|
|
"--pitch", "0",
|
|
"--yaw", "0",
|
|
"--frame-id", "world",
|
|
"--child-frame-id", "lidar_1_link"]
|
|
),
|
|
|
|
Node(
|
|
package='tf2_ros',
|
|
executable='static_transform_publisher',
|
|
name='lidar_2_broadcaster',
|
|
arguments=[
|
|
"--x", "-1.9",
|
|
"--y", "1",
|
|
"--z", "1",
|
|
"--roll", "0",
|
|
"--pitch", "0",
|
|
"--yaw", "0",
|
|
"--frame-id", "world",
|
|
"--child-frame-id", "lidar_2_link"]
|
|
),
|
|
|
|
#Node(
|
|
# package='ros_gz_bridge',
|
|
# executable='parameter_bridge',
|
|
# arguments=["/world/default/model/iisy/joint_state[sensor_msgs/msg/JointState@ignition.msgs.Model"],
|
|
# output='both',
|
|
# remappings=[
|
|
# ('/world/default/model/iisy/joint_state', '/joint_states'),
|
|
# ]
|
|
#),
|
|
|
|
Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
output='both',
|
|
parameters=[robot_description]
|
|
),
|
|
])
|