This repository provides a ros2_control hardware interface for the mjbots pi3hat.

• Raspberry Pi 4 running Ubuntu 22.04 and ROS2 Humble (tested with this image)
• pi3hat r4.4 or newer (CAN doesn't seem to work on pi3hat r4.2)
• Recommended: configure ros2_control for realtime operation as described here
• Create a workspace /home/pi/ros2_ws

• An example URDF is contained in test_state_publisher.urdf.xacro.

<ros2_control name="pi3hat_hardware_interface" type="system">
    <hardware>
        <plugin>pi3hat_hardware_interface/Pi3HatHardwareInterface</plugin>
        <param name="imu_mounting_deg.yaw">0</param>
        <param name="imu_mounting_deg.pitch">0</param>
        <param name="imu_mounting_deg.roll">0</param>
    </hardware>

    <joint name="joint_1">
        <param name="can_channel">1</param>
        <param name="can_id">1</param>
        <param name="can_protocol">cheetah</param>

        <param name="position_scale">95.5</param>
        <param name="velocity_scale">30.0</param>
        <param name="effort_scale">18.0</param>
        <param name="kp_scale">500.0</param>
        <param name="kd_scale">5.0</param>
        <param name="axis_direction">-1</param>
        <param name="position_offset">0.0</param>

        <param name="position_min">-1.0</param>
        <param name="position_max">1.0</param>
        <param name="velocity_max">1.0</param>
        <param name="effort_max">1.0</param>
        <param name="kp_max">1.0</param>
        <param name="kd_max">1.0</param>

        <command_interface name="position"/>
        <command_interface name="velocity"/>
        <command_interface name="effort"/>
        <command_interface name="kp"/>
        <command_interface name="kd"/>

        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
    </joint>

    <sensor name="imu_sensor">
        <state_interface name="orientation.x"/>
        <state_interface name="orientation.y"/>
        <state_interface name="orientation.z"/>
        <state_interface name="orientation.w"/>
        <state_interface name="angular_velocity.x"/>
        <state_interface name="angular_velocity.y"/>
        <state_interface name="angular_velocity.z"/>
        <state_interface name="linear_acceleration.x"/>
        <state_interface name="linear_acceleration.y"/>
        <state_interface name="linear_acceleration.z"/>
    </sensor>
</ros2_control>

• imu_mounting_deg.*: Mounting angles for the IMU relative the the IMU link, measured in degrees
• can_channel: Which CANbus channel the actuator is connected to (must be between 1 and 5, inclusive)
• can_id: CANbus ID of the actuator (must be 1 or greater)
• can_protocol: Which protocol the actuator uses (only cheetah is implemented currently, works with driver boards running Mini Cheetah firmware such as SteadyWin and CubeMars)
• *_scale, axis_direction: Needed for encoding and decoding numeric values from CAN messages
• position_offset: Home position of the actuator in radians
• position_min, *_max: User-defined safety limits for the command values

• Problem: ImportError: libbcm_host.so.0: cannot open shared object file: No such file or directory
• Solution: sudo ln /usr/lib/aarch64-linux-gnu/libbcm_host.so /usr/lib/libbcm_host.so.0

• Problem: pi3hat: could not open /dev/mem
• Solution: export FASTRTPS_DEFAULT_PROFILES_FILE=/home/pi/ros2_ws/src/pi3hat_hardware_interface/fastrtps_profile_no_shmem.xml and launch the ros2_control node as root with sudo -E bash ~/ros2_ws/src/pi3hat_hardware_interface/run_as_root.sh ros2 launch ~/ros2_ws/src/pi3hat_hardware_interface/test/test_state_publisher.launch.py
• Explanation: The Pi3Hat library needs to be run as root to access the GPIO, but this prevents the FastDDS shared memory transport from working (no messages are published or received). Hence, we create a custom config file to force FastDDS to use UDP instead of shared memory.

• Problem: Sometimes fails to start with Segmentation fault (Address not mapped to object [(nil)])
• Solution: Manually install ros2_control and associated packages from this branch. Compile with colcon build --symlink-install --allow-overriding controller_interface controller_manager hardware_interface ros2_control_test_assets.
• Explanation: The controller manager suffers from a race condition on Humble as described here. The fix for this has not been merged yet.

colcon build --packages-select pi3hat_hardware_interface --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo

sudo -E gdb --args /home/pi/ros2_ws/install/controller_manager/lib/controller_manager/ros2_control_node --ros-args --params-file /tmp/launch_params_sdkq8suy --params-file /home/pi/ros2_ws/src/pi3hat_hardware_interface/test/test_state_publisher.yaml

set environment LD_LIBRARY_PATH /home/pi/ros2_ws/install/transmission_interface/lib:/home/pi/ros2_ws/install/controller_manager/lib:/home/pi/ros2_ws/install/pi3hat_hardware_interface/lib:/home/pi/ros2_ws/install/controller_interface/lib:/home/pi/ros2_ws/install/hardware_interface/lib:/home/pi/ros2_ws/install/controller_manager_msgs/lib:/opt/ros/humble/lib/aarch64-linux-gnu:/opt/ros/humble/lib