[JTC] Account for edge case

[mechwiz]

I noticed an edge case (probably rare, though I have seen it happen) where if a trajectory point has a zero time-from-start duration, JTC will sample it and output a command that can only ever get checked by goal tolerances but never state tolerances since it's considered to be the last point. I think goal tolerances (and goal_time) are mainly designed for accounting for slight timing error between hardware and commands at the end of a trajectory, but not for handling the first sampled point which can be very off the current joint position in this scenario and could potentially lead to unexpected motion. This PR fixes this by always checking the state tolerance of the first sampled point. I have created a test case that shows this as well.

Before the fix

[ RUN ] TestTrajectoryActions.test_no_time_from_start_state_tolerance_fail
[INFO] [1651618255.484381928] [test_joint_trajectory_controller]: Command interfaces are [position] and and state interfaces are [position velocity].
[INFO] [1651618255.484616674] [test_joint_trajectory_controller]: Controller state will be published at 50.00 Hz.
[INFO] [1651618255.484817827] [test_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[INFO] [1651618255.786766016] [test_joint_trajectory_controller]: Received new action goal
[INFO] [1651618255.786813400] [test_joint_trajectory_controller]: Accepted new action goal
[INFO] [1651618255.786991848] [test_joint_trajectory_controller]: Goal reached, success!
/opt/colcon_ws/src/bro_system/ros2_controllers/joint_trajectory_controller/test/test_trajectory_actions.cpp:509: Failure
Expected equality of these values:
rclcpp_action::ResultCode::ABORTED
Which is: 1-byte object <06>
common_resultcode_
Which is: 1-byte object <04>
/opt/colcon_ws/src/bro_system/ros2_controllers/joint_trajectory_controller/test/test_trajectory_actions.cpp:510: Failure
Expected equality of these values:
control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED
Which is: -4
common_action_result_code_
Which is: 0
/opt/colcon_ws/src/bro_system/ros2_controllers/joint_trajectory_controller/test/test_trajectory_actions.cpp:517: Failure
The difference between init_pos1 and joint_pos_[0] is 2.8999999999999999, which exceeds COMMON_THRESHOLD, where
init_pos1 evaluates to 1.1000000000000001,
joint_pos_[0] evaluates to 4, and
COMMON_THRESHOLD evaluates to 0.0011000000000000001.
/opt/colcon_ws/src/bro_system/ros2_controllers/joint_trajectory_controller/test/test_trajectory_actions.cpp:518: Failure
The difference between init_pos2 and joint_pos_[1] is 2.8999999999999999, which exceeds COMMON_THRESHOLD, where
init_pos2 evaluates to 2.1000000000000001,
joint_pos_[1] evaluates to 5, and
COMMON_THRESHOLD evaluates to 0.0011000000000000001.
/opt/colcon_ws/src/bro_system/ros2_controllers/joint_trajectory_controller/test/test_trajectory_actions.cpp:519: Failure
The difference between init_pos3 and joint_pos_[2] is 2.8999999999999999, which exceeds COMMON_THRESHOLD, where
init_pos3 evaluates to 3.1000000000000001,
joint_pos_[2] evaluates to 6, and
COMMON_THRESHOLD evaluates to 0.0011000000000000001.
[ FAILED ] TestTrajectoryActions.test_no_time_from_start_state_tolerance_fail (2010 ms)

After the fix

[ RUN ] TestTrajectoryActions.test_no_time_from_start_state_tolerance_fail
[INFO] [1651618407.385751183] [test_joint_trajectory_controller]: Command interfaces are [position] and and state interfaces are [position velocity].
[INFO] [1651618407.386022047] [test_joint_trajectory_controller]: Controller state will be published at 50.00 Hz.
[INFO] [1651618407.386248723] [test_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[INFO] [1651618407.688211079] [test_joint_trajectory_controller]: Received new action goal
[INFO] [1651618407.688264209] [test_joint_trajectory_controller]: Accepted new action goal
[WARN] [1651618407.688413821] [test_joint_trajectory_controller]: Aborted due to state tolerance violation
[ OK ] TestTrajectoryActions.test_no_time_from_start_state_tolerance_fail (2011 ms)

[AndyZe]

I think it's reasonable to expect PATH_TOLERANCE_VIOLATED if the first waypoint has a time_from_start== 0, since that seems to mean the first waypoint should match the current state of the robot. 👍

The logic here is confusing. It would be nice to rewrite the JTC like a state machine, perhaps.

I also think the variable abort should be renamed to tolerance_violated_while_moving since that's what it seems to do (see L204). That could go in a new PR.