Skip to content

[JTC] Return INVALID_GOAL error code for rejected trajectories (#700)#2237

Open
AdvityaCode wants to merge 1 commit intoros-controls:masterfrom
AdvityaCode:issue/trajectory_rejections
Open

[JTC] Return INVALID_GOAL error code for rejected trajectories (#700)#2237
AdvityaCode wants to merge 1 commit intoros-controls:masterfrom
AdvityaCode:issue/trajectory_rejections

Conversation

@AdvityaCode
Copy link
Copy Markdown

Problem
When the Joint Trajectory Controller rejects an invalid trajectory (empty points, wrong joint names, non-monotonic timestamps, etc.), the action client receives an opaque REJECT response with no structured feedback. The only signal available to the client is a log line in the controller's terminal — which is not a viable error-handling strategy for automated systems.

This is a limitation of action protocol (tracked upstream at ros2/rclc#271): GoalResponse::REJECT provides no mechanism to set error_code or error_string on the result.

Closes #700.

Proposed Solution
This is my suggested approach — I'm open to feedback and happy to revise the implementation. It is just what I thought would be the best approach.

The workaround discussed in #699 is to always accept goals at the action protocol level, then immediately abort with a populated error_code and error_string if validation fails. I followed that direction here, but I wasn't sure on some of the design choices so please let me know if any of these should be done differently:

  • goal_received_callback now unconditionally returns ACCEPT_AND_EXECUTE. Validation is deferred to goal_accepted_callback, where a structured result can be returned to the client.
  • goal_accepted_callback performs two checks before processing a goal:
    - Whether the controller is active (PRIMARY_STATE_INACTIVE → abort with INVALID_GOAL)
    - Whether the trajectory message is valid (all existing checks → abort with INVALID_GOAL + descriptive error_string)
  • validate_trajectory_msg and validate_trajectory_point_field now return std::string instead of bool. An empty string means valid; a non-empty string is the human-readable rejection reason. Each path still logs via RCLCPP_ERROR for operator visibility.
  • The topic-based trajectory interface (~/joint_trajectory) is unaffected in behavior.

I wasn't certain whether changing the return type of validate_trajectory_msg to std::string was the right call, or whether an out-parameter or a separate struct would be preferred. Happy to change this if there's a style the project prefers.

Tests

  • All existing validate_trajectory_msg unit tests updated for the new return type.
  • Two new parameterized action tests added:
    • test_goal_aborted_invalid_trajectory_empty_points
    • test_goal_aborted_invalid_trajectory_wrong_joint_names

Copy link
Copy Markdown
Member

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for picking this.

We will have to discuss this in the next PMC meeting if the accept-and-immediately-reject pattern is something everyone is happy with.

I propose that we use tl::expected (until std::expected is available) instead of returning an empty string.

bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
/// Validate a trajectory message. Returns an empty string if valid, or a
/// human-readable error description if the trajectory should be rejected.
std::string validate_trajectory_msg(
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what do you think of using tl-expected here? Similar to the validators in https://github.com/PickNikRobotics/generate_parameter_library?tab=readme-ov-file#custom-validator-functions

@github-project-automation github-project-automation bot moved this from Needs discussion to WIP in Review triage Mar 28, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

Status: WIP

Development

Successfully merging this pull request may close these issues.

[JTC] Error codes for trajectory message rejections

2 participants