<?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="can_if">can0</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>
      <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">
          <param name="id">125</param>
        </command_interface>
        <state_interface name="position">
          <param name="id">125</param>
        </state_interface>
        <!--state_interface name="velocity">
        </state_interface-->
      </joint>
      <joint name="${prefix}right_wheel_joint">
          <param name="nodeid">125</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>
