From 337127a1d928055cbcc0a0d7d72e0725c16cf0db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 17 Dec 2024 07:31:41 +0100 Subject: [PATCH] Add an error msg if empty message is received (#1424) --- .../src/joint_trajectory_controller.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 365ce993d0..763d2acd07 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -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); @@ -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; }