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