ScaledJTC - Added support for setting speedscaling factor via topic

[firesurfer]

Hi,
this PR adds support for setting the speed scaling factor via ~/speed_scaling_factor.

The behavior can be activated by specifying use_speed_scaling_topic_instead in the controller configuration. Not specifying or setting the variable to false will keep the default behavior.

This feature allows using the controller in multi arm setups or for usage with different hardware which doesn't have any speed_scaling state.

EDIT: Just saw that I based the PR on the iron branch. If I should rebase it on the main branch let me know

EDIT2: As an alternative we could expose it as a parameter which would make a bit more similar to the cartesian controllers

EDIT3: The topic can now be changed via the config file. This allows to have one speed_scaling_factor topic for multiple controllers. Default is still ~/speed_scaling_factor.
The topic is now transient_local.

[firesurfer]

@fmauch I might have also encountered a bug with the ScaledJTC but I am not sure if it comes from my adaptions. Therefore I add it here.

While testing we quite often ran into the Aborted due goal_time_tolerance exceeding by 31.005506 seconds error.
The motion is still performed as desired.

The corresponding part of the controllers.yaml looks like this:

joint_trajectory_controller_top:
  ros__parameters:
    joints:
      - ur_top/shoulder_pan_joint
      - ur_top/shoulder_lift_joint
      - ur_top/elbow_joint
      - ur_top/wrist_1_joint
      - ur_top/wrist_2_joint
      - ur_top/wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.1
      goal_time: 0.05
      ur_top/shoulder_pan_joint: { trajectory: 0.2, goal: 0.001 }
      ur_top/shoulder_lift_joint: { trajectory: 0.2, goal: 0.001 }
      ur_top/elbow_joint: { trajectory: 0.2, goal: 0.001 }
      ur_top/wrist_1_joint: { trajectory: 0.2, goal: 0.001 }
      ur_top/wrist_2_joint: { trajectory: 0.2, goal: 0.001 }
      ur_top/wrist_3_joint: { trajectory: 0.2, goal: 0.001 }
    use_speed_scaling_topic_instead: true
    speed_scaling_topic_name: "/runtime_speed_scaling_factor"

I can also reproduce this issue when using the mocked interface.
Whats interesting is that once you manage to trigger it will come at every future try to execute a trajectory. (EDIT: If you wait like 20s or so another trajectory execution works)

[firesurfer]

Okey I managed to fix the issue. It is unrelated to the actual change introduced in this PR. (I can separate if necessary into an extra PR).

The Goal Time Tolerance Violation check if only executed if the arm is not within the position tolerances after the trajectory was finished sampling. So this issue will only occur if one runs fast movements with load (I dont know why I had it also in sim...). During such movements we won't be within the goal position tolerances and therefore we start checking the goal time tolerance. But the goal time tolerance did not compensate for the fact that if we scale down the trajectory execution that...well execution takes longer than expected. I hopefully fixed this

EDIT: My fix didn't work unfortunately. I therefore reverted it. I create a new PR for it where we can also discuss how to fix it

[firesurfer]

See #883 for the same PR but based on main. For the issue described in this PR see #882