diff --git a/launch/display_openarm.launch.py b/launch/display_openarm.launch.py index c5c107d2..a7466b3a 100644 --- a/launch/display_openarm.launch.py +++ b/launch/display_openarm.launch.py @@ -12,11 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. import os +import importlib.util +import tempfile -import xacro +import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription, LaunchContext -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler +from launch.event_handlers import OnShutdown from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -52,6 +55,7 @@ def robot_state_publisher_spawner( ros2_control, use_fake_hardware, fake_sensor_commands, + rviz_config, ): arm_type_str = context.perform_substitution(arm_type) folder_name, file_name = resolve_arm_config(arm_type_str) @@ -89,19 +93,56 @@ def robot_state_publisher_spawner( "fake_sensor_commands": context.perform_substitution(fake_sensor_commands), } - robot_description = xacro.process_file( - xacro_path, - mappings=mappings, - ).toprettyxml(indent=" ") - - return [ + preset_name = mappings['robot_preset'] or ( + 'default_bimanual' if mappings['bimanual'].lower() in ('true', '1') else 'default_single') + preset_path = os.path.join(pkg_share, 'assets', 'robot', folder_name, + 'config', 'robot_presets', preset_name + '.yaml') + with open(preset_path) as stream: + preset = yaml.safe_load(stream) + spec = importlib.util.spec_from_file_location( + 'openarm_preview_config', os.path.join(pkg_share, 'scripts', 'preview_config.py')) + preview = importlib.util.module_from_spec(spec) + spec.loader.exec_module(preview) + robot_description, joint_names = preview.expand_preview_model( + xacro_path, mappings, preset) + positions = preview.resolve_initial_positions( + preset, joint_names, robot_description) + rviz_path = os.path.join( + pkg_share, 'rviz', context.perform_substitution(rviz_config)) + cleanup = [] + view = preset.get('preview', {}).get('view') + if view: + with open(rviz_path) as stream: + config = preview.apply_preview_view( + yaml.safe_load(stream), view, preset['robot']['root_frame']) + temporary = tempfile.TemporaryDirectory(prefix='openarm-preview-') + rviz_path = os.path.join(temporary.name, 'preview.rviz') + with open(rviz_path, 'w') as stream: + yaml.safe_dump(config, stream) + cleanup.append(RegisterEventHandler(OnShutdown( + on_shutdown=lambda event, context: temporary.cleanup()))) + + return cleanup + [ Node( package="robot_state_publisher", executable="robot_state_publisher", name="robot_state_publisher", output="screen", parameters=[{"robot_description": robot_description}], - ) + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + name="joint_state_publisher_gui", + parameters=[{'zeros.' + name: value for name, + value in positions.items()}], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["--display-config", rviz_path], + ), ] @@ -151,8 +192,6 @@ def generate_launch_description(): use_fake_hardware = LaunchConfiguration("use_fake_hardware") fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") - pkg_share = get_package_share_directory("openarm_description") - return LaunchDescription([ arm_type_arg, robot_preset_arg, @@ -172,18 +211,7 @@ def generate_launch_description(): ros2_control, use_fake_hardware, fake_sensor_commands, + rviz_config, ], ), - Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - name="joint_state_publisher_gui", - ), - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - arguments=["--display-config", - [os.path.join(pkg_share, "rviz/"), rviz_config]], - ), ]) diff --git a/package.xml b/package.xml index 5078ed78..0e1171f9 100644 --- a/package.xml +++ b/package.xml @@ -31,6 +31,7 @@ joint_state_publisher_gui ros_gz xacro + python3-yaml rviz2 robot_state_publisher joint_state_publisher diff --git a/rviz/arm_only.rviz b/rviz/arm_only.rviz index 4f5a637a..1263ed22 100644 --- a/rviz/arm_only.rviz +++ b/rviz/arm_only.rviz @@ -51,69 +51,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - openarm_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_hand_tcp: - Alpha: 1 - Show Axes: false - Show Trail: false - openarm_left_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - openarm_right_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Mass Properties: Inertia: false Mass: false @@ -131,7 +68,7 @@ Visualization Manager: Name: TF Show Arrows: true Show Axes: true - Show Names: true + Show Names: false Tree: {} Update Interval: 0 diff --git a/rviz/bimanual.rviz b/rviz/bimanual.rviz index e58c0365..cc612cc5 100644 --- a/rviz/bimanual.rviz +++ b/rviz/bimanual.rviz @@ -51,141 +51,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - openarm_left_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_hand_tcp: - Alpha: 1 - Show Axes: false - Show Trail: false - openarm_left_left_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_left_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - openarm_left_right_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_hand_tcp: - Alpha: 1 - Show Axes: false - Show Trail: false - openarm_right_left_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - openarm_right_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - openarm_right_right_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - v10_body_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false Mass Properties: Inertia: false Mass: false @@ -203,7 +68,7 @@ Visualization Manager: Name: TF Show Arrows: true Show Axes: true - Show Names: true + Show Names: false Tree: {} Update Interval: 0 diff --git a/scripts/preview_config.py b/scripts/preview_config.py new file mode 100644 index 00000000..ce8fee94 --- /dev/null +++ b/scripts/preview_config.py @@ -0,0 +1,111 @@ +# Copyright 2025 Enactic, Inc. +# +# 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. +"""Preset positions and camera defaults shared by preview callers.""" +import math +import xml.etree.ElementTree as ET + +import xacro + + +def expand_preview_model(path, mappings, preset): + """Read final joint names from the component descriptions in the same expansion.""" + xacro.init_stacks(path) + doc = xacro.parse(None, path) + bindings = [] + enabled = {name for name in preset['robot'] + ['components'] if preset[name]['enabled']} + for system in preset.get('systems', {}).values(): + for component, joints in system['joints'].items(): + if component not in enabled: + continue + for key in joints: + node = doc.createElement('preview_binding') + node.setAttribute( + 'name', '${component_descriptions[%r]["joint_name_map"][%r]}' % (component, key)) + doc.documentElement.appendChild(node) + bindings.append((component, key, node)) + xacro.process_doc(doc, mappings=mappings) + names = {} + for component, key, node in bindings: + names.setdefault(component, {})[key] = node.getAttribute('name') + doc.documentElement.removeChild(node) + return doc.toxml(), names + + +def resolve_initial_positions(preset, joint_name_maps, robot_description, overrides=None): + """Resolve init_pos, optionally overridden by {system_name: {idx: position}}. + + Omitted positions stay omitted. Returned names are the generated URDF names. + """ + overrides = overrides or {} + systems = preset.get('systems', {}) + if set(overrides) - set(systems): + raise ValueError('Unknown system in initial-position overrides') + joints = {j.get('name'): j for j in ET.fromstring( + robot_description).findall('joint')} + positions = {} + for system_name, system in systems.items(): + entries = overrides.get(system_name, {}) + seen = set() + for component, selection in system['joints'].items(): + if component not in joint_name_maps: + continue + for key, config in selection.items(): + idx = config['idx'] + if idx in seen: + raise ValueError(f'Duplicate idx {idx} in {system_name}') + seen.add(idx) + if idx not in entries and 'init_pos' not in config: + continue + value = float( + entries[idx] if idx in entries else config['init_pos']) + name = joint_name_maps[component][key] + joint = joints[name] + if joint.get('type') not in ('revolute', 'continuous', 'prismatic') or joint.find('mimic') is not None: + raise ValueError( + f'Cannot initialize dependent or fixed joint {name}') + if not math.isfinite(value): + raise ValueError(f'Non-finite init_pos for {name}') + limit = joint.find('limit') + if joint.get('type') != 'continuous' and limit is not None: + if not float(limit.get('lower')) <= value <= float(limit.get('upper')): + raise ValueError( + f'init_pos outside limits for {name}: {value}') + if name in positions: + raise ValueError(f'Duplicate init_pos for {name}') + positions[name] = value + if set(entries) - seen: + raise ValueError(f'Unknown or disabled joint idx in {system_name}') + return positions + + +def apply_preview_view(config, view, root_frame): + """Apply a Z-up, zero-roll eye/lookat view in the preset root frame.""" + eye = [float(view['eye'][axis]) for axis in 'xyz'] + target = [float(view['lookat'][axis]) for axis in 'xyz'] + if not all(math.isfinite(v) for v in eye + target): + raise ValueError('Preview camera coordinates must be finite') + dx, dy, dz = [a-b for a, b in zip(eye, target)] + distance = math.sqrt(dx*dx + dy*dy + dz*dz) + if distance == 0 or math.hypot(dx, dy) == 0: + raise ValueError( + 'Preview Orbit camera needs distinct eye/lookat and a horizontal direction') + manager = config['Visualization Manager'] + manager['Global Options']['Fixed Frame'] = root_frame + camera = manager['Views']['Current'] + camera.update({'Class': 'rviz_default_plugins/Orbit', 'Target Frame': '', + 'Distance': distance, 'Yaw': math.atan2(dy, dx), + 'Pitch': math.atan2(dz, math.hypot(dx, dy)), + 'Focal Point': dict(zip('XYZ', target))}) + return config