[JTC] Fix state offset tests

[christophfroehlich]

I tried to write tests for #794 and copied the initialization of joint_pos_ from test_hw_states_has_offset_first_controller_start/test_hw_states_has_offset_later_controller_start: This did not work because ActivateTrajectoryController always has set it to the constant values from the utils-header -> I fixed that.

Tests failed now, and thinking about the test criteria itself, considering #189, I rewrote the tests -> succeed now.

add approach to change controller without "jumps" from the command to state values:

• when started HW interface has to set command_interfaces to N aN
• controller checks if command interfaces are NaN --> then is a controller started for the first time, so read states
• if command interface have a value --> so a controller was running already (supports switching between controllers)

@destogl You have added this part, before it got changed a bit by #340 by @AndyZe and @mechwiz

• Why should we set state_desired_ to the value of read_state_from_command_interfaces? If there is no active trajectory (start_with_holding is false), this value will only be published on the state-topic. If there is a new trajectory, it will be overwritten by the sampled trajectory.
• Setting state_current to the command read from the hardware interface is fine: It is only used for setting the set_hold_position if start_with_holding is set. It will be overwritten by the first call of update.

[codecov]

Codecov Report

Merging #797 (387afc7) into master (232154d) will decrease coverage by 0.49%.
Report is 1 commits behind head on master.
The diff coverage is 28.57%.

❗ Current head 387afc7 differs from pull request most recent head efcf0d1. Consider uploading reports for the commit efcf0d1 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #797      +/-   ##
==========================================
- Coverage   45.40%   44.92%   -0.49%     
==========================================
  Files          40       40              
  Lines        3649     3642       -7     
  Branches     1723     1716       -7     
==========================================
- Hits         1657     1636      -21     
- Misses        810      834      +24     
+ Partials     1182     1172      -10     

Flag	Coverage Δ
unittests	44.92% <28.57%> (-0.49%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files	Coverage Δ
...jectory_controller/joint_trajectory_controller.hpp	0.00% <ø> (ø)
...ory_controller/src/joint_trajectory_controller.cpp	47.72% <28.57%> (+0.45%)	⬆️

... and 3 files with indirect coverage changes

[bmagyar]

let's wait for @destogl to reply too

[destogl]

Just a small proposal for optimization since we are discussing those functions.

Answers to comments

• Why should we set state_desired_ to the value of read_state_from_command_interfaces? If there is no active trajectory (start_with_holding is false), this value will only be published on the state-topic. If there is a new trajectory, it will be overwritten by the sampled trajectory.

Can it be that we change this when fixing the “hold on start” feature? I think the reason was that we have immediate output to the command interface even if there is no reference. If we can achieve that by using start_with_holding this is also fine with me. Nevertheless, I would not add additional if, even if we set the value “unnecessary”. Or do you think that this could be confusing?

• Setting state_current to the command read from the hardware interface is fine: It is only used for setting the set_hold_position if start_with_holding is set. It will be overwritten by the first call of update.

OK, you are right. Then we probably should not set state_desired in that case.

[christophfroehlich]

• Why should we set state_desired_ to the value of read_state_from_command_interfaces? If there is no active trajectory (start_with_holding is false), this value will only be published on the state-topic. If there is a new trajectory, it will be overwritten by the sampled trajectory.

Can it be that we change this when fixing the “hold on start” feature? I think the reason was that we have immediate output to the command interface even if there is no reference. If we can achieve that by using start_with_holding this is also fine with me. Nevertheless, I would not add additional if, even if we set the value “unnecessary”. Or do you think that this could be confusing?

I'll think that trough again if this is achieved with the hold-on-start in a fresh moment.

Optional override is already inside an if, for me it is clearer to have it in the else branch instead (but that's only my personal preference).

[christophfroehlich]

• Why should we set state_desired_ to the value of read_state_from_command_interfaces? If there is no active trajectory (start_with_holding is false), this value will only be published on the state-topic. If there is a new trajectory, it will be overwritten by the sampled trajectory.

Can it be that we change this when fixing the “hold on start” feature? I think the reason was that we have immediate output to the command interface even if there is no reference. If we can achieve that by using start_with_holding this is also fine with me. Nevertheless, I would not add additional if, even if we set the value “unnecessary”. Or do you think that this could be confusing?

The current way with this PR is:

• If we have start_with_holding=false, no update of the command interface until the first trajectory is received. See has_active_trajectory() in update method.
• start_with_holding=true: There will be an immediate update of the command interfaces, with position set from state_current_, which is
  • set to the values of the command interfaces, if they are not NaN or
  • set to the values read from the state interfaces otherwise.

Notes:

• I asked in [JTC] Need for start_with_holding option? #816 if we need start_with_holding parameter at all.
• with [JTC] Add option to hold position on last command #794 there would be no change, because last_commanded_state_ is initialized identically.

@destogl What do you think?

[mergify]

This pull request is in conflict. Could you fix it @christophfroehlich?

[destogl]

This looks great! Thanks!