Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
74 changes: 51 additions & 23 deletions launch/display_openarm.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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],
),
]


Expand Down Expand Up @@ -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,
Expand All @@ -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]],
),
])
1 change: 1 addition & 0 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
<depend>joint_state_publisher_gui</depend>
<depend>ros_gz</depend>
<depend>xacro</depend>
<exec_depend>python3-yaml</exec_depend>
<depend>rviz2</depend>
<depend>robot_state_publisher</depend>
<depend>joint_state_publisher</depend>
Expand Down
65 changes: 1 addition & 64 deletions rviz/arm_only.rviz
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -131,7 +68,7 @@ Visualization Manager:
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Show Names: false
Tree:
{}
Update Interval: 0
Expand Down
137 changes: 1 addition & 136 deletions rviz/bimanual.rviz
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -203,7 +68,7 @@ Visualization Manager:
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Show Names: false
Tree:
{}
Update Interval: 0
Expand Down
Loading
Loading