119 lines
5.6 KiB
XML
119 lines
5.6 KiB
XML
<?xml version="1.0"?>
|
||
<!--
|
||
Copyright 2025 Reazon Holdings, 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.
|
||
-->
|
||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||
<xacro:macro name="openarm_ros2_control" params="name initial_positions_file websocket_port='' prefix:='' can_device='can0' hardware_type='real' mock_sensor_commands='{}'">
|
||
<xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
|
||
|
||
<!-- Set default websocket ports based on prefix if not provided -->
|
||
<xacro:if value="${hardware_type == 'sim' and websocket_port == ''}">
|
||
<xacro:property name="computed_websocket_port" value="${'1337' if prefix == 'right_' or prefix == '' else '1338'}"/>
|
||
</xacro:if>
|
||
<xacro:unless value="${hardware_type == 'sim' and websocket_port == ''}">
|
||
<xacro:property name="computed_websocket_port" value="${websocket_port}"/>
|
||
</xacro:unless>
|
||
|
||
<ros2_control name="${prefix}${name}" type="system">
|
||
<hardware>
|
||
<xacro:if value="${hardware_type == 'mock'}">
|
||
<plugin>mock_components/GenericSystem</plugin>
|
||
<param name="mock_sensor_commands">${mock_sensor_commands}</param>
|
||
</xacro:if>
|
||
<xacro:if value="${hardware_type == 'sim'}">
|
||
<plugin>openarm_mujoco_hardware/MujocoHardware</plugin>
|
||
<param name="prefix">${prefix}</param>
|
||
<xacro:if value="${computed_websocket_port != ''}">
|
||
<param name="websocket_port">${computed_websocket_port}</param>
|
||
</xacro:if>
|
||
</xacro:if>
|
||
<xacro:if value="${hardware_type == 'real'}">
|
||
<plugin>openarm_hardware/OpenArmHW</plugin>
|
||
<param name="prefix">${prefix}</param>
|
||
<param name="can_device">${can_device}</param>
|
||
</xacro:if>
|
||
</hardware>
|
||
<joint name="${prefix}rev1">
|
||
<command_interface name="position"/>
|
||
<command_interface name="velocity"/>
|
||
<!-- <command_interface name="effort"/> -->
|
||
<state_interface name="position"/>
|
||
<state_interface name="velocity"/>
|
||
<!-- <state_interface name="effort"/> -->
|
||
</joint>
|
||
<joint name="${prefix}rev2">
|
||
<command_interface name="position"/>
|
||
<command_interface name="velocity"/>
|
||
<!-- <command_interface name="effort"/> -->
|
||
<state_interface name="position"/>
|
||
<state_interface name="velocity"/>
|
||
<!-- <state_interface name="effort"/> -->
|
||
</joint>
|
||
<joint name="${prefix}rev3">
|
||
<command_interface name="position"/>
|
||
<command_interface name="velocity"/>
|
||
<!-- <command_interface name="effort"/> -->
|
||
<state_interface name="position"/>
|
||
<state_interface name="velocity"/>
|
||
<!-- <state_interface name="effort"/> -->
|
||
</joint>
|
||
<joint name="${prefix}rev4">
|
||
<command_interface name="position"/>
|
||
<command_interface name="velocity"/>
|
||
<!-- <command_interface name="effort"/> -->
|
||
<state_interface name="position"/>
|
||
<state_interface name="velocity"/>
|
||
<!-- <state_interface name="effort"/> -->
|
||
</joint>
|
||
<joint name="${prefix}rev5">
|
||
<command_interface name="position"/>
|
||
<command_interface name="velocity"/>
|
||
<!-- <command_interface name="effort"/> -->
|
||
<state_interface name="position"/>
|
||
<state_interface name="velocity"/>
|
||
<!-- <state_interface name="effort"/> -->
|
||
</joint>
|
||
<joint name="${prefix}rev6">
|
||
<command_interface name="position"/>
|
||
<command_interface name="velocity"/>
|
||
<!-- <command_interface name="effort"/> -->
|
||
<state_interface name="position"/>
|
||
<state_interface name="velocity"/>
|
||
<!-- <state_interface name="effort"/> -->
|
||
</joint>
|
||
<joint name="${prefix}rev7">
|
||
<command_interface name="position"/>
|
||
<command_interface name="velocity"/>
|
||
<!-- <command_interface name="effort"/> -->
|
||
<state_interface name="position"/>
|
||
<state_interface name="velocity"/>
|
||
<!-- <state_interface name="effort"/> -->
|
||
</joint>
|
||
<joint name="${prefix}gripper">
|
||
<command_interface name="position"/>
|
||
<state_interface name="position"/>
|
||
<state_interface name="velocity"/>
|
||
</joint>
|
||
|
||
<!-- <joint name="${prefix}gripper_mimic">
|
||
<state_interface name="position">
|
||
<param name="initial_value">${initial_positions['gripper_mimic']}</param>
|
||
</state_interface>
|
||
</joint> -->
|
||
|
||
</ros2_control>
|
||
</xacro:macro>
|
||
</robot>
|