Make Tricycle Controller a chainable controller

[catcracker]

Is your feature suggestion related to a problem? Please describe.
While using the PID controller and tricycle controller, there might be room for improvement in the following areas:

There seems to be no straightforward method to enable or disable feedforward control.
The controller state topics (~/controller_state or /pid_state) don't appear to include feedforward gain information.
The feedforward gain in the PID controller may not work as expected in some cases.
The tricycle controller might benefit from additional parameters to better support cascaded control scenarios.

Describe the solution you'd like to see
For both PID and Tricycle Controllers:

Consider adding a boolean parameter in the YAML configuration to enable/disable feedforward control, for example:
enable_feedforward: true
Alternatively, it might be worth considering automatically enabling feedforward control when feedforward_gain is non-zero.
It could be beneficial to publish feedforward_gain in the ~/controller_state or /pid_state topics.

For PID Controller:

It might be necessary to investigate and address the issue with feedforward_gain application. Currently, in the pid_controller.cpp file (lines 444-445), the result appears to always be 0.000.
A potential fix to consider might be:
tmp_command= reference_interfaces_[i] * params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain;

For Tricycle Controller:

Consider adding the following parameters to support cascaded control:
traction_joint_command_name: "traction_command_interface"
steering_joint_command_name: "steering_command_interface"
traction_joint_state_name: "traction_state_interface"
steering_joint_state_name: "steering_state_interface"

The controller might need modifications to utilize these new parameters for cascaded control scenarios.
It could be worthwhile to consider standardizing these parameters for use across different controllers in cascaded control situations.

Here's my system structure and controller config file.

  ros__parameters:
    update_rate: 100 # Hz
    use_sim_time: false

    tricycle_controller:
      type: tricycle_controller/TricycleController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster


    pid_steering_controller:
      type: pid_controller/PidController

    pid_traction_controller:
      type: pid_controller/PidController

joint_state_broadcaster:
  ros__parameters:
    extra_joints: ["right_wheel_joint", "left_wheel_joint"]

pid_traction_controller:
  ros__parameters:
    dof_names: ["traction_joint"]
    command_interface: "velocity"
    reference_and_state_dof_names : ["traction_joint"]
    reference_and_state_interfaces: ["velocity"]
    gains:
      traction_joint:
        p: 0.000
        i: 0.0
        d: 0.00
        i_clamp_max: +0.8
        i_clamp_min: -0.8
        antiwindup: true
        feedforward_gain: 1.0
     
pid_steering_controller:
  ros__parameters:
    dof_names: ["steering_joint"]
    command_interface: "position"
    reference_and_state_dof_names : ["steering_joint"]
    reference_and_state_interfaces: ["position"]
    gains:
      steering_joint:
        p: 0.000
        i: 0.0
        d: 0.0
        i_clamp_max: +0.18
        i_clamp_min: -0.18
        antiwindup: true
        feedforward_gain: 1.0
  

tricycle_controller:
  ros__parameters: 
    # Model
    traction_joint_command_name: pid_traction_controller/traction_joint # Name of traction joint in URDF
    steering_joint_command_name: pid_steering_controller/steering_joint # Name of steering joint in URDF
    traction_joint_state_name: traction_joint
    steering_joint_state_name: steering_joint
    wheel_radius: 0.127 # Radius of front wheel
    wheelbase: 1.050 # Distance between center of back wheels and front wheel

    # Odometry
    odom_frame_id: odom
    base_frame_id: base_link
    publish_rate: 50.0 # publish rate of odom and tf
    open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry
    enable_odom_tf: true # If True, publishes odom<-base_link TF
    odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf
    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] # Need to be set if fusing odom with other localization source
    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] # Need to be set if fusing odom with other localization source
    velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom

    # Rate Limiting
    traction: # All values should be positive
      min_velocity: 0.0
      max_velocity: 17.5  #rad/s =8km/h
      min_acceleration: 0.0
      max_acceleration: 8.75 #rad/s^2=1,11m/s^2
      min_deceleration: 0.0
      max_deceleration: 8.75 #rad/s^2=-1.11m/s^2
      # min_jerk: 0.0
      # max_jerk: 1000.0
    steering:
      min_position: -1.57
      max_position: 1.57
      #min_velocity: 0.0
      #max_velocity: 1.0
      # min_acceleration: 0.0
      # max_acceleration: 1000.0

    # cmd_vel input
    cmd_vel_timeout: 100 # In milliseconds. Timeout to stop if no cmd_vel is received,don't small than publish_rate
    use_stamped_vel: false # Set to True if using TwistStamped.
    # Debug
    publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s.     

[christophfroehlich]

Let's split this into two issues.

[christophfroehlich]

What do you mean with feedforward of the tricycle controller? It has no control loop, it just evaluates the drive kinematics and calculates odometry from sensors.

For Tricycle Controller:

Consider adding the following parameters to support cascaded control:
yamlCopytraction_joint_command_name: "traction_command_interface"
steering_joint_command_name: "steering_command_interface"
traction_joint_state_name: "traction_state_interface"
steering_joint_state_name: "steering_state_interface"

The controller might need modifications to utilize these new parameters for cascaded control scenarios. It could be worthwhile to consider standardizing these parameters for use across different controllers in cascaded control situations.

I do not understand what you mean here, sorry.

[catcracker]

Sorry, the feed forward control has nothing to do with this, it's the cascade of controllers. I didn't draw it on the picture, it's that the command interface and state interface of the tricycle controller come from the pid controller and the hardware respectively, they are not consistent, the original design defaults from the same one for example traction_joint

[christophfroehlich]

I still don't know exactly what you want to achieve, why should the command interfaces of the tricycle controller come from the PID?
Regarding state interfaces: This should be possible since ros-controls/ros2_control#1021

[catcracker]

I still don't know exactly what you want to achieve, why should the command interfaces of the tricycle controller come from the PID? Regarding state interfaces: This should be possible since ros-controls/ros2_control#1021

This does solve my problem, maybe I'm using humble and this feature hasn't been updated yet, but I think if i use this feature the original controller may still need to be modified, because the controller uses the get_prefix_name() function on lines 478 and 504 of the code, which should report an error.

CallbackReturn TricycleController::get_traction(
  const std::string & traction_joint_name, std::vector<TractionHandle> & joint)
{
  RCLCPP_INFO(get_node()->get_logger(), "Get Wheel Joint Instance");

  // Lookup the velocity state interface
  const auto state_handle = std::find_if(
    state_interfaces_.cbegin(), state_interfaces_.cend(),
    [&traction_joint_name](const auto & interface)
    {
      return interface.get_prefix_name() == traction_joint_name &&
             interface.get_interface_name() == HW_IF_VELOCITY;
    });
  if (state_handle == state_interfaces_.cend())
  {
    RCLCPP_ERROR(
      get_node()->get_logger(), "Unable to obtain joint state handle for %s",
      traction_joint_name.c_str());
    return CallbackReturn::ERROR;
  }

  // Lookup the velocity command interface
  const auto command_handle = std::find_if(
    command_interfaces_.begin(), command_interfaces_.end(),
    [&traction_joint_name](const auto & interface)
    {
      return interface.get_prefix_name() == traction_joint_name &&
             interface.get_interface_name() == HW_IF_VELOCITY;
    });
  if (command_handle == command_interfaces_.end())
  {
    RCLCPP_ERROR(
      get_node()->get_logger(), "Unable to obtain joint state handle for %s",
      traction_joint_name.c_str());
    return CallbackReturn::ERROR;
  }

  // Create the traction joint instance
  joint.emplace_back(TractionHandle{std::ref(*state_handle), std::ref(*command_handle)});
  return CallbackReturn::SUCCESS;
}

CallbackReturn TricycleController::get_steering(
  const std::string & steering_joint_name, std::vector<SteeringHandle> & joint)
{
  RCLCPP_INFO(get_node()->get_logger(), "Get Steering Joint Instance");

  // Lookup the velocity state interface
  const auto state_handle = std::find_if(
    state_interfaces_.cbegin(), state_interfaces_.cend(),
    [&steering_joint_name](const auto & interface)
    {
      return interface.get_prefix_name() == steering_joint_name &&
             interface.get_interface_name() == HW_IF_POSITION;
    });
  if (state_handle == state_interfaces_.cend())
  {
    RCLCPP_ERROR(
      get_node()->get_logger(), "Unable to obtain joint state handle for %s",
      steering_joint_name.c_str());
    return CallbackReturn::ERROR;
  }

  // Lookup the velocity command interface
  const auto command_handle = std::find_if(
    command_interfaces_.begin(), command_interfaces_.end(),
    [&steering_joint_name](const auto & interface)
    {
      return interface.get_prefix_name() == steering_joint_name &&
             interface.get_interface_name() == HW_IF_POSITION;
    });
  if (command_handle == command_interfaces_.end())
  {
    RCLCPP_ERROR(
      get_node()->get_logger(), "Unable to obtain joint state handle for %s",
      steering_joint_name.c_str());
    return CallbackReturn::ERROR;
  }

  // Create the steering joint instance
  joint.emplace_back(SteeringHandle{std::ref(*state_handle), std::ref(*command_handle)});
  return CallbackReturn::SUCCESS;
}

[christophfroehlich]

You are right

• this feature won't be backported and is available only from Jazzy on.
• Tricycle controller is not a chainable controller

I plan to include the tricycle controller to the steering controller library, which is already a chainable controller.