[JTC] Segmentation fault with action tests

[christophfroehlich]

Describe the bug
Included with #607, the action tests get activated again. But with the latest changes to the master branch, the tests fail with a segmentation fault.
Before the rebase from @bmagyar today everything was fine. Might this be a result of #682?

Locally I get the following stack trace:

4: [ RUN      ] OnlyEffortTrajectoryControllers/TestTrajectoryActionsTestParameterized.test_success_single_point_sendgoal/0
4: [INFO] [1687809125.124157051] [test_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
4: [INFO] [1687809125.124258126] [test_joint_trajectory_controller]: Command interfaces are [effort] and state interfaces are [position velocity].
4: [INFO] [1687809125.124306425] [test_joint_trajectory_controller]: Using 'splines' interpolation method.
4: [INFO] [1687809125.125600011] [test_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
4: [INFO] [1687809125.431236591] [test_joint_trajectory_controller]: Received new action goal
4: [INFO] [1687809125.431359484] [test_joint_trajectory_controller]: Accepted new action goal
4: [INFO] [1687809125.931574512] [test_joint_trajectory_controller]: Goal reached, success!
4: Stack trace (most recent call last) in thread 78980:
4: #6    Object "", at 0xffffffffffffffff, in 
4: #5    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f867bb059ff, in 
4: #4    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f867ba73b42, in 
4: #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f867bd032b2, in 
4: #2    Source "/workspaces/ros2_rolling_ws/src/ros2_controllers/joint_trajectory_controller/test/test_trajectory_actions.cpp", line 99, in operator() [0x55e624efb8fd]
4:          96:         auto end_time = start_time + wait;
4:          97:         while (clock.now() < end_time)
4:          98:         {
4:       >  99:           traj_controller_->update(clock.now(), clock.now() - start_time);
4:         100:         }
4:         101:       });
4: #1    Source "/workspaces/ros2_rolling_ws/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp", line 355, in update [0x7f867c33c9e8]
4:         352:           }
4:         353:           else
4:         354:           {
4:       > 355:             assign_interface_from_point(joint_command_interface_[3], state_desired_.effort);
4:         356:           }
4:         357:         }
4: #0    Source "/workspaces/ros2_rolling_ws/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp", line 166, in operator()<std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>, std::allocator<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > > > [0x7f867c2ff5b0]
4:         163:   {
4:         164:     for (size_t index = 0; index < dof_; ++index)
4:         165:     {
4:       > 166:       joint_interface[index].get().set_value(trajectory_point_interface[index]);
4:         167:     }
4:         168:   };
4: Segmentation fault (Address not mapped to object [(nil)])

To Reproduce
Steps to reproduce the behavior:
1.Checkout #607
2. Run colcon test --packages-select joint_trajectory_controller

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version jtc-features

[christophfroehlich]

After reverting 9ce288b the segmentation fault does not occur any more (which still doesn't mean that the error doesn't lie somewhere else)

[christophfroehlich]

I narrowed it down to state_desired_.effort.size() being zero, which causes the segfault.
https://github.com/christophfroehlich/ros2_controllers/blob/dd80bc9acb13e273a0d9b31170d396e2979b621a/joint_trajectory_controller/src/joint_trajectory_controller.cpp#L353-L357

#682 changed the condition of the if-branch, this is why it doesn't segfault before because use_closed_loop_pid_adapter_ is true and tmp_command_ is not empty.

• use_closed_loop_pid_adapter_ is always true if has_effort_command_interface_
  

  ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp

  Lines 682 to 685 in 9ce288b

  	use_closed_loop_pid_adapter_ =
  	(has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&
  	!params_.open_loop_control) ||
  	has_effort_command_interface_;

• Trajectory class does not interpolate effort fields

• But JointTrajectoryPoint message has an effort field.
  https://github.com/ros2/common_interfaces/blob/f4eac72f0bbd70f7955a5f709d4a6705eb6ca7e8/trajectory_msgs/msg/JointTrajectoryPoint.msg#L20-L23

So the questions are:

• Should JTC take effort commands from the message? Makes sense for feedforward operation only.
• If not, this else branch must never be reached
  

  ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp

  Lines 353 to 356 in 9ce288b

  	else
  	{
  	assign_interface_from_point(joint_command_interface_[3], state_desired_.effort);
  	}

What do you think @bmagyar @destogl ?

[christophfroehlich]

@egordon @tingelst As you were commenting on the PR/linked issue, could you please comment on this one?

[egordon]

Apologies for introducing the segfault, we didn't encounter it on our end since we don't use effort control, not sure how it squeezed by the tests.

The reasoning for the if statement change was to catch the new case introduced by #565 where the trajectory pointer is set to null upon successful execution. In that case, it doesn't make sense to continuously "replay" the final tmp_command (e.g. with velocity control unless velocity was identically 0 upon completion [unlikely], it would cause the robot to drift), and instead just use the final waypoint of the previously-commanded trajectory.

I naively applied that same reasoning to effort and didn't check the desired vector size. My b.

A fix to #514 and #671 that reverts the behavior to continuously commanding the final waypoint as the PID setpoint would make #682 irrelevant anyway.

[christophfroehlich]

So the questions are:

• Should JTC take effort commands from the message? Makes sense for feedforward operation only.
• If not, this else branch must never be reached

We decided in the last WG meeting to reject incoming trajectory messages having effort entries for now. If someone needs this, we have to discuss how to interpolate effort.