64 lines
2.3 KiB
XML
64 lines
2.3 KiB
XML
<?xml version="1.0"?>
|
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
|
|
<xacro:macro name="staubi_ros2_control" params="name prefix use_mock_hardware">
|
|
|
|
<ros2_control name="${name}" type="system">
|
|
<xacro:unless value="${use_mock_hardware}">
|
|
<hardware>
|
|
<plugin>cyphal_hardware_interface/cyphalSystemHardware</plugin>
|
|
<param name="port">/dev/ttyAMA10</param>
|
|
<!--param name="port">/dev/ttyACM0</param-->
|
|
<param name="baud">460800</param>
|
|
<!--param name="try_next_port_number">false</param-->
|
|
</hardware>
|
|
</xacro:unless>
|
|
<xacro:if value="${use_mock_hardware}">
|
|
<hardware>
|
|
<plugin>mock_components/GenericSystem</plugin>
|
|
<param name="calculate_dynamics">true</param>
|
|
</hardware>
|
|
</xacro:if>
|
|
<sensor name="${prefix}driver">
|
|
<param name="state:battery_voltage">vcc</param>
|
|
<param name="factor">0.001</param>
|
|
<state_interface name="battery_voltage"/>
|
|
</sensor>
|
|
<joint name="${prefix}vacuum">
|
|
<param name="command:power">s</param>
|
|
<param name="factor">0.1427</param>
|
|
<command_interface name="power">
|
|
<param name="min">0</param>
|
|
<param name="max">1</param>
|
|
</command_interface>
|
|
</joint>
|
|
<joint name="${prefix}left_wheel_joint">
|
|
<param name="command:velocity">vt0</param>
|
|
<param name="state:position">a0</param>
|
|
<param name="state:velocity">v0</param>
|
|
<param name="factor">0.0001</param>
|
|
<command_interface name="velocity">
|
|
</command_interface>
|
|
<state_interface name="position">
|
|
</state_interface>
|
|
<!--state_interface name="velocity">
|
|
</state_interface-->
|
|
</joint>
|
|
<joint name="${prefix}right_wheel_joint">
|
|
<param name="command:velocity">vt1</param>
|
|
<param name="state:position">a1</param>
|
|
<param name="state:velocity">v1</param>
|
|
<param name="factor">0.0001</param>
|
|
<command_interface name="velocity">
|
|
</command_interface>
|
|
<state_interface name="position">
|
|
</state_interface>
|
|
<!--state_interface name="velocity">
|
|
</state_interface-->
|
|
</joint>
|
|
</ros2_control>
|
|
|
|
</xacro:macro>
|
|
|
|
</robot>
|