Skip to content

Commit

Permalink
Add an error msg if empty message is received (#1424)
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich authored Dec 17, 2024
1 parent c9c8343 commit 337127a
Showing 1 changed file with 10 additions and 3 deletions.
13 changes: 10 additions & 3 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1457,8 +1457,15 @@ bool JointTrajectoryController::validate_trajectory_msg(
// This currently supports only position, velocity and acceleration inputs
if (params_.allow_integration_in_goal_trajectories)
{
const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() &&
points[i].accelerations.empty();
if (
points[i].positions.empty() && points[i].velocities.empty() &&
points[i].accelerations.empty())
{
RCLCPP_ERROR(
get_node()->get_logger(),
"The given trajectory has no position, velocity, or acceleration points.");
return false;
}
const bool position_error =
!points[i].positions.empty() &&
!validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false);
Expand All @@ -1469,7 +1476,7 @@ bool JointTrajectoryController::validate_trajectory_msg(
!points[i].accelerations.empty() &&
!validate_trajectory_point_field(
joint_count, points[i].accelerations, "accelerations", i, false);
if (all_empty || position_error || velocity_error || acceleration_error)
if (position_error || velocity_error || acceleration_error)
{
return false;
}
Expand Down

0 comments on commit 337127a

Please sign in to comment.