Skip to content

Preserve CAN launch overrides with preset-based descriptions #103

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
stevenaya wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Preserve CAN launch overrides with preset-based descriptions #103

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
95 changes: 63 additions & 32 deletions openarm_bimanual_moveit_config/launch/demo.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
# limitations under the License.

import os
import xml.etree.ElementTree as ET
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription, LaunchContext
Expand Down Expand Up @@ -45,21 +46,48 @@ def resolve_arm_config(arm_type_str: str) -> tuple[str, str]:
return "openarm_v2.0", "openarm_v20.urdf.xacro"


def apply_can_overrides(robot_description, left_can_interface, right_can_interface, can_fd):
"""Apply explicit launch overrides to the standard bimanual hardware systems."""
can_fd = can_fd.strip().lower()
if can_fd not in ("", "true", "false", "0", "1"):
raise ValueError("can_fd must be true, false, 1, 0, or empty.")
can_fd = {"1": "true", "0": "false"}.get(can_fd, can_fd)
if not (left_can_interface or right_can_interface or can_fd):
return robot_description

robot = ET.fromstring(robot_description)
for side, interface in (("left", left_can_interface), ("right", right_can_interface)):
overrides = {name: value for name, value in (
("can_interface", interface.strip()), ("can_fd", can_fd)) if value}
if not overrides:
continue
system_name = f"openarm_{side}_hardware_interface"
hardware = robot.find(f"ros2_control[@name='{system_name}']/hardware")
if hardware is None:
raise ValueError(
f"Cannot apply CAN overrides: missing {system_name}.")
for name, value in overrides.items():
parameter = hardware.find(f"param[@name='{name}']")
if parameter is None:
parameter = ET.SubElement(hardware, "param", name=name)
parameter.text = value
return ET.tostring(robot, encoding="unicode")


def generate_robot_description(
context: LaunchContext,
description_package,
arm_type,
use_fake_hardware,
right_can_interface,
robot_preset,
left_can_interface,
right_can_interface,
can_fd,
):
description_package_str = context.perform_substitution(description_package)
arm_type_str = context.perform_substitution(arm_type)
use_fake_hardware_str = context.perform_substitution(use_fake_hardware)
right_can_interface_str = context.perform_substitution(right_can_interface)
left_can_interface_str = context.perform_substitution(left_can_interface)
can_fd_str = context.perform_substitution(can_fd)
robot_preset_str = context.perform_substitution(robot_preset)

folder_name, file_name = resolve_arm_config(arm_type_str)

Expand All @@ -68,38 +96,45 @@ def generate_robot_description(
"assets", "robot", folder_name, "urdf", file_name
)

return xacro.process_file(
robot_description = xacro.process_file(
xacro_path,
mappings={
"arm_type": arm_type_str,
"bimanual": "true",
"use_fake_hardware": use_fake_hardware_str,
"ros2_control": "true",
"left_can_interface": left_can_interface_str,
"right_can_interface": right_can_interface_str,
"can_fd": can_fd_str,
"robot_preset": robot_preset_str,
},
).toprettyxml(indent=" ")

return apply_can_overrides(
robot_description,
context.perform_substitution(left_can_interface),
context.perform_substitution(right_can_interface),
context.perform_substitution(can_fd),
)


def robot_nodes_spawner(
context: LaunchContext,
description_package,
arm_type,
use_fake_hardware,
controllers_file,
right_can_interface,
robot_preset,
arm_prefix,
left_can_interface,
right_can_interface,
can_fd,
arm_prefix,
):
robot_description = generate_robot_description(
context,
description_package,
arm_type,
use_fake_hardware,
right_can_interface,
robot_preset,
left_can_interface,
right_can_interface,
can_fd,
)

Expand All @@ -121,7 +156,9 @@ def robot_nodes_spawner(
parameters=[robot_description_param, controllers_file_str],
)

return [robot_state_pub_node, control_node]
return [robot_state_pub_node, control_node] + moveit_nodes_spawner(
context, arm_type, use_fake_hardware, robot_preset, robot_description
)


def controller_spawner(context: LaunchContext, robot_controller):
Expand All @@ -146,12 +183,10 @@ def controller_spawner(context: LaunchContext, robot_controller):


def moveit_nodes_spawner(context: LaunchContext, arm_type, use_fake_hardware,
right_can_interface, left_can_interface, can_fd):
robot_preset, robot_description):
arm_type_str = context.perform_substitution(arm_type)
use_fake_hardware_str = context.perform_substitution(use_fake_hardware)
right_can_interface_str = context.perform_substitution(right_can_interface)
left_can_interface_str = context.perform_substitution(left_can_interface)
can_fd_str = context.perform_substitution(can_fd)
robot_preset_str = context.perform_substitution(robot_preset)

description_pkg_path = get_package_share_directory("openarm_description")
moveit_pkg_path = get_package_share_directory(
Expand All @@ -178,9 +213,7 @@ def moveit_nodes_spawner(context: LaunchContext, arm_type, use_fake_hardware,
"bimanual": "true",
"use_fake_hardware": use_fake_hardware_str,
"ros2_control": "true",
"left_can_interface": left_can_interface_str,
"right_can_interface": right_can_interface_str,
"can_fd": can_fd_str,
"robot_preset": robot_preset_str,
}
)
.robot_description_semantic(file_path=f"config/{config_dir}/openarm_bimanual.srdf")
Expand All @@ -194,6 +227,7 @@ def moveit_nodes_spawner(context: LaunchContext, arm_type, use_fake_hardware,
.to_moveit_configs()
)

moveit_config.robot_description = {"robot_description": robot_description}
moveit_params = moveit_config.to_dict()

pilz_cartesian_limits_path = os.path.join(
Expand Down Expand Up @@ -249,9 +283,11 @@ def generate_launch_description():
DeclareLaunchArgument("runtime_config_package",
default_value="openarm_bringup"),
DeclareLaunchArgument("arm_prefix", default_value=""),
DeclareLaunchArgument("right_can_interface", default_value="can0"),
DeclareLaunchArgument("left_can_interface", default_value="can1"),
DeclareLaunchArgument("can_fd", default_value="true"),
DeclareLaunchArgument(
"robot_preset", default_value="default_bimanual"),
DeclareLaunchArgument("left_can_interface", default_value=""),
DeclareLaunchArgument("right_can_interface", default_value=""),
DeclareLaunchArgument("can_fd", default_value=""),
DeclareLaunchArgument(
"controllers_file",
default_value="openarm_bimanual_moveit_controllers.yaml"),
Expand All @@ -263,8 +299,9 @@ def generate_launch_description():
robot_controller = LaunchConfiguration("robot_controller")
runtime_config_package = LaunchConfiguration("runtime_config_package")
controllers_file = LaunchConfiguration("controllers_file")
right_can_interface = LaunchConfiguration("right_can_interface")
robot_preset = LaunchConfiguration("robot_preset")
left_can_interface = LaunchConfiguration("left_can_interface")
right_can_interface = LaunchConfiguration("right_can_interface")
can_fd = LaunchConfiguration("can_fd")
arm_prefix = LaunchConfiguration("arm_prefix")

Expand All @@ -280,19 +317,14 @@ def generate_launch_description():
arm_type,
use_fake_hardware,
controllers_file,
right_can_interface,
robot_preset,
arm_prefix,
left_can_interface,
right_can_interface,
can_fd,
arm_prefix,
],
)

moveit_nodes_func = OpaqueFunction(
function=moveit_nodes_spawner,
args=[arm_type, use_fake_hardware,
right_can_interface, left_can_interface, can_fd]
)

jsb_spawner = Node(
package="controller_manager",
executable="spawner",
Expand All @@ -315,7 +347,6 @@ def generate_launch_description():
declared_arguments
+ [
robot_nodes_spawner_func,
moveit_nodes_func,
TimerAction(period=2.0, actions=[jsb_spawner]),
TimerAction(period=1.0, actions=[controller_spawner_func]),
TimerAction(period=1.0, actions=[gripper_spawner]),
Expand Down
13 changes: 10 additions & 3 deletions openarm_bringup/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,10 @@ ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v10 use_fake_ha

- `arm_type` - Arm type: `v1.0`/`v10`/`openarm_v1.0` or `v2.0`/`v20`/`openarm_v2.0` (default: openarm_v2.0)
- `use_fake_hardware` - Use fake hardware instead of real hardware (default: true)
- `right_can_interface` - CAN interface to use for the right arm (default: can0)
- `left_can_interface` - CAN interface to use for the left arm (default: can1)
- `can_fd` - Use CAN-FD. Set to `false` for CAN 2.0 (default: true)
- `robot_preset` - Description preset containing each component's `control` configuration (default: `default_bimanual`). Set `can_interface` and `can_fd` directly in that preset.
- `left_can_interface` - Override the left arm's CAN device for this launch (default: empty, using the preset).
- `right_can_interface` - Override the right arm's CAN device for this launch (default: empty, using the preset).
- `can_fd` - Override CAN-FD mode for both arms with `true` or `false` (default: empty, using the preset).
- `robot_controller` - Controller type: `joint_trajectory_controller` (default) or `forward_position_controller`
- `arm_prefix` - Namespace for the robot's nodes and topics, for example `leader` (default: empty, no namespace)

Expand All @@ -38,3 +39,9 @@ ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v10 use_fake_ha
- Robot controller (joint trajectory or forward position)
- Gripper controller
- RViz2 visualization

Explicit CAN launch arguments override the preset's values in the generated
`ros2_control` hardware configuration. The standard bimanual launch targets
`openarm_left_hardware_interface` and `openarm_right_hardware_interface`.
For example, select a different left device and use CAN 2.0 with
`left_can_interface:=can2 can_fd:=false`.
80 changes: 58 additions & 22 deletions openarm_bringup/launch/openarm.bimanual.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
# limitations under the License.

import os
import xml.etree.ElementTree as ET
import xacro

from ament_index_python.packages import get_package_share_directory
Expand Down Expand Up @@ -54,16 +55,42 @@ def namespace_from_context(context, arm_prefix):
return None


def apply_can_overrides(robot_description, left_can_interface, right_can_interface, can_fd):
"""Apply explicit launch overrides to the standard bimanual hardware systems."""
can_fd = can_fd.strip().lower()
if can_fd not in ("", "true", "false", "0", "1"):
raise ValueError("can_fd must be true, false, 1, 0, or empty.")
can_fd = {"1": "true", "0": "false"}.get(can_fd, can_fd)
if not (left_can_interface or right_can_interface or can_fd):
return robot_description

robot = ET.fromstring(robot_description)
for side, interface in (("left", left_can_interface), ("right", right_can_interface)):
overrides = {name: value for name, value in (
("can_interface", interface.strip()), ("can_fd", can_fd)) if value}
if not overrides:
continue
system_name = f"openarm_{side}_hardware_interface"
hardware = robot.find(f"ros2_control[@name='{system_name}']/hardware")
if hardware is None:
raise ValueError(
f"Cannot apply CAN overrides: missing {system_name}.")
for name, value in overrides.items():
parameter = hardware.find(f"param[@name='{name}']")
if parameter is None:
parameter = ET.SubElement(hardware, "param", name=name)
parameter.text = value
return ET.tostring(robot, encoding="unicode")


def generate_robot_description(context: LaunchContext, description_package, description_file,
arm_type, use_fake_hardware, right_can_interface, left_can_interface,
can_fd):
arm_type, use_fake_hardware, robot_preset,
left_can_interface, right_can_interface, can_fd):
"""Generate robot description using xacro processing."""
description_package_str = context.perform_substitution(description_package)
arm_type_str = context.perform_substitution(arm_type)
use_fake_hardware_str = context.perform_substitution(use_fake_hardware)
right_can_interface_str = context.perform_substitution(right_can_interface)
left_can_interface_str = context.perform_substitution(left_can_interface)
can_fd_str = context.perform_substitution(can_fd)
robot_preset_str = context.perform_substitution(robot_preset)

folder_name, file_name = resolve_arm_config(arm_type_str)

Expand All @@ -79,24 +106,29 @@ def generate_robot_description(context: LaunchContext, description_package, desc
"bimanual": "true",
"use_fake_hardware": use_fake_hardware_str,
"ros2_control": "true",
"right_can_interface": right_can_interface_str,
"left_can_interface": left_can_interface_str,
"can_fd": can_fd_str,
"robot_preset": robot_preset_str,
}
).toprettyxml(indent=" ")

return robot_description
return apply_can_overrides(
robot_description,
context.perform_substitution(left_can_interface),
context.perform_substitution(right_can_interface),
context.perform_substitution(can_fd),
)


def robot_nodes_spawner(context: LaunchContext, description_package, description_file,
arm_type, use_fake_hardware, controllers_file,
right_can_interface, left_can_interface, can_fd, arm_prefix):
robot_preset, arm_prefix,
left_can_interface, right_can_interface, can_fd):
"""Spawn both robot state publisher and control nodes with shared robot description."""
namespace = namespace_from_context(context, arm_prefix)

robot_description = generate_robot_description(
context, description_package, description_file, arm_type,
use_fake_hardware, right_can_interface, left_can_interface, can_fd,
use_fake_hardware, robot_preset,
left_can_interface, right_can_interface, can_fd,
)

controllers_file_str = context.perform_substitution(controllers_file)
Expand Down Expand Up @@ -193,19 +225,21 @@ def generate_launch_description():
description="Prefix for the arm for topic namespacing.",
),
DeclareLaunchArgument(
"right_can_interface",
default_value="can0",
description="CAN interface to use for the right arm.",
"robot_preset",
default_value="default_bimanual",
description="Description preset containing component hardware settings.",
),
DeclareLaunchArgument(
"left_can_interface",
default_value="can1",
description="CAN interface to use for the left arm.",
"left_can_interface", default_value="",
description="Left CAN device override; empty uses the preset.",
),
DeclareLaunchArgument(
"can_fd",
default_value="true",
description="Use CAN-FD. Set to false for CAN 2.0.",
"right_can_interface", default_value="",
description="Right CAN device override; empty uses the preset.",
),
DeclareLaunchArgument(
"can_fd", default_value="",
description="CAN-FD override for both arms; empty uses the preset.",
),
DeclareLaunchArgument(
"controllers_file",
Expand All @@ -221,8 +255,9 @@ def generate_launch_description():
robot_controller = LaunchConfiguration("robot_controller")
runtime_config_package = LaunchConfiguration("runtime_config_package")
controllers_file = LaunchConfiguration("controllers_file")
right_can_interface = LaunchConfiguration("right_can_interface")
robot_preset = LaunchConfiguration("robot_preset")
left_can_interface = LaunchConfiguration("left_can_interface")
right_can_interface = LaunchConfiguration("right_can_interface")
can_fd = LaunchConfiguration("can_fd")
arm_prefix = LaunchConfiguration("arm_prefix")

Expand All @@ -235,7 +270,8 @@ def generate_launch_description():
function=robot_nodes_spawner,
args=[description_package, description_file, arm_type,
use_fake_hardware, controllers_file,
right_can_interface, left_can_interface, can_fd, arm_prefix]
robot_preset, arm_prefix,
left_can_interface, right_can_interface, can_fd]
)

rviz_config_file = PathJoinSubstitution(
Expand Down
Loading