Feedback Control in gazebo_ros2_control

edit

Hello,

I have code that takes a position and velocity measurement (state), and a commanded position, and produces a control effort. I want to integrate this in gazebo_ros2_control and ros2_control for use on my robot in Gazebo. I confused on how to tell ros2_control that my control output is an effort. I cant seem to find an example of this in any of the demo code

In my yaml config I have:

controller_manager:
   ros__parameters:
   update_rate: 100  # Hz

my_controller:
   type: my_controller/MyController

joint_state_broadcaster:
   type: joint_state_broadcaster/JointStateBroadcaster

my_controller:
   ros__parameters:
      joints:
         - arm_1_joint
   command_interfaces:
         - position
   state_interfaces:
         - position
         - velocity

In my urdf I have

    <ros2_control name="GazeboSystem" type="system">
    <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
    <!-- Controlled joints -->
    <joint name="arm_1_joint">
        <command_interface name="position">
            <param name="min">-1</param>
            <param name="max">1</param>
        </command_interface>
        <state_interface name="position">
        </state_interface>
        <state_interface name="velocity"/>
    </joint>

and

<gazebo>
   <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <robot_param>robot_description</robot_param>
         <robot_param_node>robot_state_publisher</robot_param_node>
         <parameters>$(find my_robot)/config/my_controller_params.yaml</parameters>
       </plugin>
</gazebo>

Where would I state that my control input is an effort?

Also when building my own controller in ros2_control as in here. I can't seem to figure out how to define my control input in the 'update' method.

The closest example I could find was the JointTrajectoryController in ros2_control but I cannot find anywhere in the 'update' method where the control input is given.

I am completely at a loss on trying to figure this out. I can get the demo controllers to work, but cannot figure out how to integrate my own controller. Documentation is very sparse and trying to read demo code that has little/no comments is driving me a little insane.

If it helps, I am running ros2-foxy with Gazebo 11 on WSL with Windows 11

Thank You