diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt
index 4b0ca82df8..7fc2a2a8d8 100644
--- a/joint_trajectory_controller/CMakeLists.txt
+++ b/joint_trajectory_controller/CMakeLists.txt
@@ -59,7 +59,9 @@ if(BUILD_TESTING)
 
   ament_add_gmock(test_trajectory test/test_trajectory.cpp)
   target_link_libraries(test_trajectory joint_trajectory_controller)
-  target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES)
+
+  ament_add_gmock(test_trajectory_operations test/test_trajectory_operations.cpp)
+  target_link_libraries(test_trajectory_operations joint_trajectory_controller)
 
   ament_add_gmock(test_trajectory_controller
     test/test_trajectory_controller.cpp)
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
index ae370df0a6..4d60790138 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -30,6 +30,7 @@
 #include "joint_trajectory_controller/interpolation_methods.hpp"
 #include "joint_trajectory_controller/tolerances.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
+#include "joint_trajectory_controller/trajectory_operations.hpp"
 #include "joint_trajectory_controller/visibility_control.h"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/subscription.hpp"
@@ -225,24 +226,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void compute_error_for_joint(
     JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current,
     const JointTrajectoryPoint & desired) const;
-  // fill trajectory_msg so it matches joints controlled by this controller
-  // positions set to current position, velocities, accelerations and efforts to 0.0
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  void fill_partial_goal(
-    std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const;
-  // sorts the joints of the incoming message to our local order
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  void sort_to_local_joint_order(
-    std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const;
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void add_new_trajectory_msg(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  bool validate_trajectory_point_field(
-    size_t joint_names_size, const std::vector<double> & vector_field,
-    const std::string & string_for_vector_field, size_t i, bool allow_empty) const;
 
   SegmentTolerances default_tolerances_;
 
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
index b00d79481c..59ac1d3319 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp
@@ -162,38 +162,6 @@ class Trajectory
   bool sampled_already_ = false;
 };
 
-/**
- * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2
- * indices. If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated
- * mapping vector is <tt>"{2, 1}"</tt>.
- */
-template <class T>
-inline std::vector<size_t> mapping(const T & t1, const T & t2)
-{
-  // t1 must be a subset of t2
-  if (t1.size() > t2.size())
-  {
-    return std::vector<size_t>();
-  }
-
-  std::vector<size_t> mapping_vector(t1.size());  // Return value
-  for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
-  {
-    auto t2_it = std::find(t2.begin(), t2.end(), *t1_it);
-    if (t2.end() == t2_it)
-    {
-      return std::vector<size_t>();
-    }
-    else
-    {
-      const size_t t1_dist = std::distance(t1.begin(), t1_it);
-      const size_t t2_dist = std::distance(t2.begin(), t2_it);
-      mapping_vector[t1_dist] = t2_dist;
-    }
-  }
-  return mapping_vector;
-}
-
 /**
  * \param current_position The current position given from the controller, which will be adapted.
  * \param next_position Next position from which to compute the wraparound offset, i.e.,
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp
new file mode 100644
index 0000000000..2411670caa
--- /dev/null
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory_operations.hpp
@@ -0,0 +1,335 @@
+// Copyright (c) 2024 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_OPERATIONS_HPP_
+#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_OPERATIONS_HPP_
+
+#include <chrono>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "joint_trajectory_controller/visibility_control.h"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/time.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+
+// auto-generated by generate_parameter_library
+#include "joint_trajectory_controller_parameters.hpp"
+
+namespace joint_trajectory_controller
+{
+using namespace std::chrono_literals;  // NOLINT
+
+/**
+ * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2
+ * indices. If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated
+ * mapping vector is <tt>"{2, 1}"</tt>.
+ */
+template <class T>
+inline std::vector<size_t> mapping(const T & t1, const T & t2)
+{
+  // t1 must be a subset of t2
+  if (t1.size() > t2.size())
+  {
+    return std::vector<size_t>();
+  }
+
+  std::vector<size_t> mapping_vector(t1.size());  // Return value
+  for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
+  {
+    auto t2_it = std::find(t2.begin(), t2.end(), *t1_it);
+    if (t2.end() == t2_it)
+    {
+      return std::vector<size_t>();
+    }
+    else
+    {
+      const size_t t1_dist = std::distance(t1.begin(), t1_it);
+      const size_t t2_dist = std::distance(t2.begin(), t2_it);
+      mapping_vector[t1_dist] = t2_dist;
+    }
+  }
+  return mapping_vector;
+}
+
+// sorts the joints of the incoming message to our local order
+JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+void sort_to_local_joint_order(
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg, rclcpp::Logger logger,
+  const Params & params)
+{
+  // rearrange all points in the trajectory message based on mapping
+  std::vector<size_t> mapping_vector = mapping(trajectory_msg->joint_names, params.joints);
+  auto remap = [logger, params](
+                 const std::vector<double> & to_remap,
+                 const std::vector<size_t> & mapping) -> std::vector<double>
+  {
+    if (to_remap.empty())
+    {
+      return to_remap;
+    }
+    if (to_remap.size() != mapping.size())
+    {
+      RCLCPP_WARN(logger, "Invalid input size (%zu) for sorting", to_remap.size());
+      return to_remap;
+    }
+    static std::vector<double> output(params.joints.size(), 0.0);
+    // Only resize if necessary since it's an expensive operation
+    if (output.size() != mapping.size())
+    {
+      output.resize(mapping.size(), 0.0);
+    }
+    for (size_t index = 0; index < mapping.size(); ++index)
+    {
+      auto map_index = mapping[index];
+      output[map_index] = to_remap[index];
+    }
+    return output;
+  };
+
+  // might not be used any more, but to get a genuine msg
+  trajectory_msg->joint_names = params.joints;
+
+  for (size_t index = 0; index < trajectory_msg->points.size(); ++index)
+  {
+    trajectory_msg->points[index].positions =
+      remap(trajectory_msg->points[index].positions, mapping_vector);
+
+    trajectory_msg->points[index].velocities =
+      remap(trajectory_msg->points[index].velocities, mapping_vector);
+
+    trajectory_msg->points[index].accelerations =
+      remap(trajectory_msg->points[index].accelerations, mapping_vector);
+
+    trajectory_msg->points[index].effort =
+      remap(trajectory_msg->points[index].effort, mapping_vector);
+  }
+}
+
+// fill trajectory_msg so it matches joints controlled by this controller
+// positions set to current position, velocities, accelerations and efforts to 0.0
+JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+void fill_partial_goal(
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg,
+  std::function<double(size_t)> get_default_position, const Params & params)
+{
+  const auto dof = params.joints.size();
+  // joint names in the goal are a subset of existing joints, as checked in goal_callback
+  // so if the size matches, the goal contains all controller joints
+  if (dof == trajectory_msg->joint_names.size())
+  {
+    return;
+  }
+
+  trajectory_msg->joint_names.reserve(dof);
+
+  for (size_t index = 0; index < dof; ++index)
+  {
+    {
+      if (
+        std::find(
+          trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(),
+          params.joints[index]) != trajectory_msg->joint_names.end())
+      {
+        // joint found on msg
+        continue;
+      }
+      trajectory_msg->joint_names.push_back(params.joints[index]);
+
+      for (auto & it : trajectory_msg->points)
+      {
+        // Assume hold position with 0 velocity and acceleration for missing joints
+        if (!it.positions.empty())
+        {
+          it.positions.push_back(get_default_position(index));
+        }
+        if (!it.velocities.empty())
+        {
+          it.velocities.push_back(0.0);
+        }
+        if (!it.accelerations.empty())
+        {
+          it.accelerations.push_back(0.0);
+        }
+        if (!it.effort.empty())
+        {
+          it.effort.push_back(0.0);
+        }
+      }
+    }
+  }
+}
+
+JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+bool validate_trajectory_point_field(
+  size_t joint_names_size, const std::vector<double> & vector_field,
+  const std::string & string_for_vector_field, size_t i, const bool allow_empty,
+  rclcpp::Logger logger)
+{
+  if (allow_empty && vector_field.empty())
+  {
+    return true;
+  }
+  if (joint_names_size != vector_field.size())
+  {
+    RCLCPP_ERROR(
+      logger, "Mismatch between joint_names size (%zu) and %s (%zu) at point #%zu.",
+      joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i);
+    return false;
+  }
+  return true;
+}
+
+JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+bool validate_trajectory_msg(
+  const trajectory_msgs::msg::JointTrajectory & trajectory, rclcpp::Logger logger, rclcpp::Time now,
+  const Params & params)
+{
+  // If partial joints goals are not allowed, goal should specify all controller joints
+  if (!params.allow_partial_joints_goal)
+  {
+    if (trajectory.joint_names.size() != params.joints.size())
+    {
+      RCLCPP_ERROR(logger, "Joints on incoming trajectory don't match the controller joints.");
+      return false;
+    }
+  }
+
+  if (trajectory.joint_names.empty())
+  {
+    RCLCPP_ERROR(logger, "Empty joint names on incoming trajectory.");
+    return false;
+  }
+
+  const auto trajectory_start_time = static_cast<rclcpp::Time>(trajectory.header.stamp);
+  // If the starting time it set to 0.0, it means the controller should start it now.
+  // Otherwise we check if the trajectory ends before the current time,
+  // in which case it can be ignored.
+  if (trajectory_start_time.seconds() != 0.0)
+  {
+    auto trajectory_end_time = trajectory_start_time;
+    for (const auto & p : trajectory.points)
+    {
+      trajectory_end_time += p.time_from_start;
+    }
+    if (trajectory_end_time < now)
+    {
+      RCLCPP_ERROR(
+        logger, "Received trajectory with non-zero start time (%f) that ends in the past (%f)",
+        trajectory_start_time.seconds(), trajectory_end_time.seconds());
+      return false;
+    }
+  }
+
+  for (size_t i = 0; i < trajectory.joint_names.size(); ++i)
+  {
+    const std::string & incoming_joint_name = trajectory.joint_names[i];
+
+    auto it = std::find(params.joints.begin(), params.joints.end(), incoming_joint_name);
+    if (it == params.joints.end())
+    {
+      RCLCPP_ERROR(
+        logger, "Incoming joint %s doesn't match the controller's joints.",
+        incoming_joint_name.c_str());
+      return false;
+    }
+  }
+
+  if (trajectory.points.empty())
+  {
+    RCLCPP_ERROR(logger, "Empty trajectory received.");
+    return false;
+  }
+
+  if (!params.allow_nonzero_velocity_at_trajectory_end)
+  {
+    for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
+    {
+      if (
+        std::fabs(trajectory.points.back().velocities.at(i)) >
+        std::numeric_limits<float>::epsilon())
+      {
+        RCLCPP_ERROR(
+          logger, "Velocity of last trajectory point of joint %s is not zero: %.15f",
+          trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i));
+        return false;
+      }
+    }
+  }
+
+  rclcpp::Duration previous_traj_time(0ms);
+  for (size_t i = 0; i < trajectory.points.size(); ++i)
+  {
+    if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time))
+    {
+      RCLCPP_ERROR(
+        logger,
+        "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively",
+        i - 1, i, previous_traj_time.seconds(),
+        rclcpp::Duration(trajectory.points[i].time_from_start).seconds());
+      return false;
+    }
+    previous_traj_time = trajectory.points[i].time_from_start;
+
+    const size_t joint_count = trajectory.joint_names.size();
+    const auto & points = trajectory.points;
+    // 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();
+      const bool position_error =
+        !points[i].positions.empty() &&
+        !validate_trajectory_point_field(
+          joint_count, points[i].positions, "positions", i, false, logger);
+      const bool velocity_error =
+        !points[i].velocities.empty() &&
+        !validate_trajectory_point_field(
+          joint_count, points[i].velocities, "velocities", i, false, logger);
+      const bool acceleration_error =
+        !points[i].accelerations.empty() &&
+        !validate_trajectory_point_field(
+          joint_count, points[i].accelerations, "accelerations", i, false, logger);
+      if (all_empty || position_error || velocity_error || acceleration_error)
+      {
+        return false;
+      }
+    }
+    else if (
+      !validate_trajectory_point_field(
+        joint_count, points[i].positions, "positions", i, false, logger) ||
+      !validate_trajectory_point_field(
+        joint_count, points[i].velocities, "velocities", i, true, logger) ||
+      !validate_trajectory_point_field(
+        joint_count, points[i].accelerations, "accelerations", i, true, logger))
+    {
+      return false;
+    }
+    // reject effort entries
+    if (!points[i].effort.empty())
+    {
+      RCLCPP_ERROR(logger, "Trajectories with effort fields are currently not supported.");
+      return false;
+    }
+  }
+  return true;
+}
+
+}  // namespace joint_trajectory_controller
+
+#endif  // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_OPERATIONS_HPP_
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 4a4b63cb75..b96b6d2414 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -150,8 +150,30 @@ controller_interface::return_type JointTrajectoryController::update(
     current_external_msg != *new_external_msg &&
     (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false)
   {
-    fill_partial_goal(*new_external_msg);
-    sort_to_local_joint_order(*new_external_msg);
+    fill_partial_goal(
+      *new_external_msg,
+      [this](size_t index)
+      {
+        if (
+          has_position_command_interface_ &&
+          !std::isnan(joint_command_interface_[0][index].get().get_value()))
+        {
+          // copy last command if cmd interface exists
+          return joint_command_interface_[0][index].get().get_value();
+        }
+        else if (has_position_state_interface_)
+        {
+          // copy current state if state interface exists
+          return joint_state_interface_[0][index].get().get_value();
+        }
+        else
+        {
+          // this should never happen
+          return std::numeric_limits<double>::quiet_NaN();
+        }
+      },
+      params_);
+    sort_to_local_joint_order(*new_external_msg, get_node()->get_logger(), params_);
     // TODO(denis): Add here integration of position and velocity
     traj_external_point_ptr_->update(*new_external_msg);
   }
@@ -1103,7 +1125,7 @@ void JointTrajectoryController::publish_state(
 void JointTrajectoryController::topic_callback(
   const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg)
 {
-  if (!validate_trajectory_msg(*msg))
+  if (!validate_trajectory_msg(*msg, get_node()->get_logger(), get_node()->now(), params_))
   {
     return;
   }
@@ -1129,7 +1151,8 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback(
     return rclcpp_action::GoalResponse::REJECT;
   }
 
-  if (!validate_trajectory_msg(goal->trajectory))
+  if (!validate_trajectory_msg(
+        goal->trajectory, get_node()->get_logger(), get_node()->now(), params_))
   {
     return rclcpp_action::GoalResponse::REJECT;
   }
@@ -1221,266 +1244,6 @@ void JointTrajectoryController::compute_error_for_joint(
   }
 }
 
-void JointTrajectoryController::fill_partial_goal(
-  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const
-{
-  // joint names in the goal are a subset of existing joints, as checked in goal_callback
-  // so if the size matches, the goal contains all controller joints
-  if (dof_ == trajectory_msg->joint_names.size())
-  {
-    return;
-  }
-
-  trajectory_msg->joint_names.reserve(dof_);
-
-  for (size_t index = 0; index < dof_; ++index)
-  {
-    {
-      if (
-        std::find(
-          trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(),
-          params_.joints[index]) != trajectory_msg->joint_names.end())
-      {
-        // joint found on msg
-        continue;
-      }
-      trajectory_msg->joint_names.push_back(params_.joints[index]);
-
-      for (auto & it : trajectory_msg->points)
-      {
-        // Assume hold position with 0 velocity and acceleration for missing joints
-        if (!it.positions.empty())
-        {
-          if (
-            has_position_command_interface_ &&
-            !std::isnan(joint_command_interface_[0][index].get().get_value()))
-          {
-            // copy last command if cmd interface exists
-            it.positions.push_back(joint_command_interface_[0][index].get().get_value());
-          }
-          else if (has_position_state_interface_)
-          {
-            // copy current state if state interface exists
-            it.positions.push_back(joint_state_interface_[0][index].get().get_value());
-          }
-        }
-        if (!it.velocities.empty())
-        {
-          it.velocities.push_back(0.0);
-        }
-        if (!it.accelerations.empty())
-        {
-          it.accelerations.push_back(0.0);
-        }
-        if (!it.effort.empty())
-        {
-          it.effort.push_back(0.0);
-        }
-      }
-    }
-  }
-}
-
-void JointTrajectoryController::sort_to_local_joint_order(
-  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const
-{
-  // rearrange all points in the trajectory message based on mapping
-  std::vector<size_t> mapping_vector = mapping(trajectory_msg->joint_names, params_.joints);
-  auto remap = [this](
-                 const std::vector<double> & to_remap,
-                 const std::vector<size_t> & mapping) -> std::vector<double>
-  {
-    if (to_remap.empty())
-    {
-      return to_remap;
-    }
-    if (to_remap.size() != mapping.size())
-    {
-      RCLCPP_WARN(
-        get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size());
-      return to_remap;
-    }
-    static std::vector<double> output(dof_, 0.0);
-    // Only resize if necessary since it's an expensive operation
-    if (output.size() != mapping.size())
-    {
-      output.resize(mapping.size(), 0.0);
-    }
-    for (size_t index = 0; index < mapping.size(); ++index)
-    {
-      auto map_index = mapping[index];
-      output[map_index] = to_remap[index];
-    }
-    return output;
-  };
-
-  for (size_t index = 0; index < trajectory_msg->points.size(); ++index)
-  {
-    trajectory_msg->points[index].positions =
-      remap(trajectory_msg->points[index].positions, mapping_vector);
-
-    trajectory_msg->points[index].velocities =
-      remap(trajectory_msg->points[index].velocities, mapping_vector);
-
-    trajectory_msg->points[index].accelerations =
-      remap(trajectory_msg->points[index].accelerations, mapping_vector);
-
-    trajectory_msg->points[index].effort =
-      remap(trajectory_msg->points[index].effort, mapping_vector);
-  }
-}
-
-bool JointTrajectoryController::validate_trajectory_point_field(
-  size_t joint_names_size, const std::vector<double> & vector_field,
-  const std::string & string_for_vector_field, size_t i, bool allow_empty) const
-{
-  if (allow_empty && vector_field.empty())
-  {
-    return true;
-  }
-  if (joint_names_size != vector_field.size())
-  {
-    RCLCPP_ERROR(
-      get_node()->get_logger(),
-      "Mismatch between joint_names size (%zu) and %s (%zu) at point #%zu.", joint_names_size,
-      string_for_vector_field.c_str(), vector_field.size(), i);
-    return false;
-  }
-  return true;
-}
-
-bool JointTrajectoryController::validate_trajectory_msg(
-  const trajectory_msgs::msg::JointTrajectory & trajectory) const
-{
-  // If partial joints goals are not allowed, goal should specify all controller joints
-  if (!params_.allow_partial_joints_goal)
-  {
-    if (trajectory.joint_names.size() != dof_)
-    {
-      RCLCPP_ERROR(
-        get_node()->get_logger(),
-        "Joints on incoming trajectory don't match the controller joints.");
-      return false;
-    }
-  }
-
-  if (trajectory.joint_names.empty())
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory.");
-    return false;
-  }
-
-  const auto trajectory_start_time = static_cast<rclcpp::Time>(trajectory.header.stamp);
-  // If the starting time it set to 0.0, it means the controller should start it now.
-  // Otherwise we check if the trajectory ends before the current time,
-  // in which case it can be ignored.
-  if (trajectory_start_time.seconds() != 0.0)
-  {
-    auto trajectory_end_time = trajectory_start_time;
-    for (const auto & p : trajectory.points)
-    {
-      trajectory_end_time += p.time_from_start;
-    }
-    if (trajectory_end_time < get_node()->now())
-    {
-      RCLCPP_ERROR(
-        get_node()->get_logger(),
-        "Received trajectory with non-zero start time (%f) that ends in the past (%f)",
-        trajectory_start_time.seconds(), trajectory_end_time.seconds());
-      return false;
-    }
-  }
-
-  for (size_t i = 0; i < trajectory.joint_names.size(); ++i)
-  {
-    const std::string & incoming_joint_name = trajectory.joint_names[i];
-
-    auto it = std::find(params_.joints.begin(), params_.joints.end(), incoming_joint_name);
-    if (it == params_.joints.end())
-    {
-      RCLCPP_ERROR(
-        get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.",
-        incoming_joint_name.c_str());
-      return false;
-    }
-  }
-
-  if (trajectory.points.empty())
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received.");
-    return false;
-  }
-
-  if (!params_.allow_nonzero_velocity_at_trajectory_end)
-  {
-    for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
-    {
-      if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits<float>::epsilon())
-      {
-        RCLCPP_ERROR(
-          get_node()->get_logger(),
-          "Velocity of last trajectory point of joint %s is not zero: %.15f",
-          trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i));
-        return false;
-      }
-    }
-  }
-
-  rclcpp::Duration previous_traj_time(0ms);
-  for (size_t i = 0; i < trajectory.points.size(); ++i)
-  {
-    if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time))
-    {
-      RCLCPP_ERROR(
-        get_node()->get_logger(),
-        "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively",
-        i - 1, i, previous_traj_time.seconds(),
-        rclcpp::Duration(trajectory.points[i].time_from_start).seconds());
-      return false;
-    }
-    previous_traj_time = trajectory.points[i].time_from_start;
-
-    const size_t joint_count = trajectory.joint_names.size();
-    const auto & points = trajectory.points;
-    // 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();
-      const bool position_error =
-        !points[i].positions.empty() &&
-        !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false);
-      const bool velocity_error =
-        !points[i].velocities.empty() &&
-        !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, false);
-      const bool acceleration_error =
-        !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)
-      {
-        return false;
-      }
-    }
-    else if (
-      !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) ||
-      !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) ||
-      !validate_trajectory_point_field(
-        joint_count, points[i].accelerations, "accelerations", i, true))
-    {
-      return false;
-    }
-    // reject effort entries
-    if (!points[i].effort.empty())
-    {
-      RCLCPP_ERROR(
-        get_node()->get_logger(), "Trajectories with effort fields are currently not supported.");
-      return false;
-    }
-  }
-  return true;
-}
-
 void JointTrajectoryController::add_new_trajectory_msg(
   const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)
 {
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 0b00bd5380..8669d2b347 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -1317,169 +1317,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe
   executor.cancel();
 }
 
-/**
- * @brief invalid_message Test mismatched joint and reference vector sizes
- */
-TEST_P(TrajectoryControllerTestParameterized, invalid_message)
-{
-  rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false);
-  rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false);
-  rclcpp::executors::SingleThreadedExecutor executor;
-  SetUpAndActivateTrajectoryController(
-    executor, {partial_joints_parameters, allow_integration_parameters});
-
-  trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;
-
-  good_traj_msg.joint_names = joint_names_;
-  good_traj_msg.header.stamp = rclcpp::Time(0);
-  good_traj_msg.points.resize(1);
-  good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25);
-  good_traj_msg.points[0].positions.resize(1);
-  good_traj_msg.points[0].positions = {1.0, 2.0, 3.0};
-  good_traj_msg.points[0].velocities.resize(1);
-  good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0};
-  EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg));
-
-  // Incompatible joint names
-  traj_msg = good_traj_msg;
-  traj_msg.joint_names = {"bad_name"};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // empty message
-  traj_msg = good_traj_msg;
-  traj_msg.points.clear();
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // No position data
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].positions.clear();
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Incompatible data sizes, too few positions
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].positions = {1.0, 2.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Incompatible data sizes, too many positions
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Incompatible data sizes, too few velocities
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].velocities = {1.0, 2.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Incompatible data sizes, too few accelerations
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].accelerations = {1.0, 2.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Effort is not supported in trajectory message
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].effort = {1.0, 2.0, 3.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Non-strictly increasing waypoint times
-  traj_msg = good_traj_msg;
-  traj_msg.points.push_back(traj_msg.points.front());
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-}
-
-/**
- * @brief Test invalid velocity at trajectory end with parameter set to false
- */
-TEST_P(
-  TrajectoryControllerTestParameterized,
-  expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false)
-{
-  rclcpp::Parameter nonzero_vel_parameters("allow_nonzero_velocity_at_trajectory_end", false);
-  rclcpp::executors::SingleThreadedExecutor executor;
-  SetUpAndActivateTrajectoryController(executor, {nonzero_vel_parameters});
-
-  trajectory_msgs::msg::JointTrajectory traj_msg;
-  traj_msg.joint_names = joint_names_;
-  traj_msg.header.stamp = rclcpp::Time(0);
-
-  // empty message (no throw!)
-  ASSERT_NO_THROW(traj_controller_->validate_trajectory_msg(traj_msg));
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Nonzero velocity at trajectory end!
-  traj_msg.points.resize(1);
-  traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25);
-  traj_msg.points[0].positions.resize(1);
-  traj_msg.points[0].positions = {1.0, 2.0, 3.0};
-  traj_msg.points[0].velocities.resize(1);
-  traj_msg.points[0].velocities = {-1.0, -2.0, -3.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-}
-
-/**
- * @brief missing_positions_message_accepted Test mismatched joint and reference vector sizes
- *
- * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or
- * velocities are accepted
- */
-TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted)
-{
-  rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true);
-  rclcpp::executors::SingleThreadedExecutor executor;
-  SetUpAndActivateTrajectoryController(executor, {allow_integration_parameters});
-
-  trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;
-
-  good_traj_msg.joint_names = joint_names_;
-  good_traj_msg.header.stamp = rclcpp::Time(0);
-  good_traj_msg.points.resize(1);
-  good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25);
-  good_traj_msg.points[0].positions.resize(1);
-  good_traj_msg.points[0].positions = {1.0, 2.0, 3.0};
-  good_traj_msg.points[0].velocities.resize(1);
-  good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0};
-  good_traj_msg.points[0].accelerations.resize(1);
-  good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0};
-  EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg));
-
-  // No position data
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].positions.clear();
-  EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // No position and velocity data
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].positions.clear();
-  traj_msg.points[0].velocities.clear();
-  EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // All empty
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].positions.clear();
-  traj_msg.points[0].velocities.clear();
-  traj_msg.points[0].accelerations.clear();
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Incompatible data sizes, too few positions
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].positions = {1.0, 2.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Incompatible data sizes, too many positions
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Incompatible data sizes, too few velocities
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].velocities = {1.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-
-  // Incompatible data sizes, too few accelerations
-  traj_msg = good_traj_msg;
-  traj_msg.points[0].accelerations = {2.0};
-  EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));
-}
-
 /**
  * @brief test_trajectory_replace Test replacing an existing trajectory
  */
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index f049bc65a9..c7106417ca 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -52,7 +52,6 @@ class TestableJointTrajectoryController
 {
 public:
   using joint_trajectory_controller::JointTrajectoryController::JointTrajectoryController;
-  using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg;
 
   controller_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override
diff --git a/joint_trajectory_controller/test/test_trajectory_operations.cpp b/joint_trajectory_controller/test/test_trajectory_operations.cpp
new file mode 100644
index 0000000000..ff0fa3eb99
--- /dev/null
+++ b/joint_trajectory_controller/test/test_trajectory_operations.cpp
@@ -0,0 +1,416 @@
+// Copyright 2024 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <cmath>
+#include <memory>
+#include <vector>
+
+#include "gmock/gmock.h"
+
+#include "builtin_interfaces/msg/duration.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "joint_trajectory_controller/trajectory_operations.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/duration.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/time.hpp"
+#include "std_msgs/msg/header.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+
+#include "test_trajectory_controller_utils.hpp"
+
+using joint_trajectory_controller::fill_partial_goal;
+using joint_trajectory_controller::sort_to_local_joint_order;
+using joint_trajectory_controller::validate_trajectory_msg;
+
+std::string vectorToString(const std::vector<double> & vec)
+{
+  std::ostringstream oss;
+  for (const auto & val : vec)
+  {
+    oss << val << " ";
+  }
+  return oss.str();
+}
+
+// Custom matcher to check if a trajectory matches the expected joint names and values
+MATCHER_P2(IsTrajMsgSorted, joint_names, vector_val, "")
+{
+  if (arg->joint_names != joint_names)
+  {
+    *result_listener << "Joint names mismatch";
+    return false;
+  }
+
+  if (arg->points.size() == 0)
+  {
+    *result_listener << "Trajectory has no points";
+    return false;
+  }
+
+  const auto & point = arg->points[0];
+  if (
+    point.positions != vector_val || point.velocities != vector_val ||
+    point.accelerations != vector_val || point.effort != vector_val)
+  {
+    *result_listener << "Point values mismatch:";
+    if (point.positions != vector_val)
+    {
+      *result_listener << "\npositions: " << vectorToString(point.positions) << " ne "
+                       << vectorToString(vector_val);
+    }
+    if (point.velocities != vector_val)
+    {
+      *result_listener << "\nvelocities: " << vectorToString(point.velocities) << " ne "
+                       << vectorToString(vector_val);
+    }
+    if (point.accelerations != vector_val)
+    {
+      *result_listener << "\naccelerations: " << vectorToString(point.accelerations) << " ne "
+                       << vectorToString(vector_val);
+    }
+    if (point.effort != vector_val)
+    {
+      *result_listener << "\neffort: " << vectorToString(point.effort) << " ne "
+                       << vectorToString(vector_val);
+    }
+    return false;
+  }
+
+  return true;
+}
+
+template <typename T>
+auto get_jumbled_values = [](const std::vector<size_t> & jumble_map, const std::vector<T> & values)
+{
+  std::vector<T> result;
+  for (const auto & index : jumble_map)
+  {
+    result.push_back(values[index]);
+  }
+  return result;
+};
+
+// The fixture for testing class.
+class TrajectoryOperationsTest : public testing::Test
+{
+protected:
+  // You can remove any or all of the following functions if their bodies would
+  // be empty.
+
+  TrajectoryOperationsTest()
+  {
+    // You can do set-up work for each test here.
+  }
+
+  ~TrajectoryOperationsTest() override
+  {
+    // You can do clean-up work that doesn't throw exceptions here.
+  }
+
+  // If the constructor and destructor are not enough for setting up
+  // and cleaning up each test, you can define the following methods:
+
+  void SetUp() override
+  {
+    // Code here will be called immediately after the constructor (right
+    // before each test).
+    joint_names_ = {"joint1", "joint2", "joint3"};
+    logger_ = std::make_shared<rclcpp::Logger>(rclcpp::get_logger("test_trajectory_operations"));
+
+    params_.joints = joint_names_;
+    params_.allow_nonzero_velocity_at_trajectory_end = true;
+  }
+
+  void TearDown() override
+  {
+    // Code here will be called immediately after each test (right
+    // before the destructor).
+  }
+
+  // Class members declared here can be used by all tests in the test suite
+  std::vector<std::string> joint_names_;
+  rclcpp::Clock clock_;
+  joint_trajectory_controller::Params params_;
+  std::shared_ptr<rclcpp::Logger> logger_;
+};
+
+/**
+ * @brief invalid_message Test mismatched joint and reference vector sizes
+ */
+TEST_F(TrajectoryOperationsTest, invalid_message)
+{
+  params_.allow_partial_joints_goal = false;
+  params_.allow_integration_in_goal_trajectories = false;
+
+  trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;
+
+  good_traj_msg.joint_names = joint_names_;
+  good_traj_msg.header.stamp = rclcpp::Time(0);
+  good_traj_msg.points.resize(1);
+  good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25);
+  good_traj_msg.points[0].positions = {1.0, 2.0, 3.0};
+  good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0};
+  EXPECT_TRUE(validate_trajectory_msg(good_traj_msg, *logger_, clock_.now(), params_));
+
+  // Incompatible joint names
+  traj_msg = good_traj_msg;
+  traj_msg.joint_names = {"bad_name"};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+  traj_msg = good_traj_msg;
+  traj_msg.joint_names = {"bad_name1", "bad_name2", "bad_name3"};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Empty joint names
+  traj_msg = good_traj_msg;
+  traj_msg.joint_names.clear();
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // empty message
+  traj_msg = good_traj_msg;
+  traj_msg.points.clear();
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // No position data
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].positions.clear();
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Incompatible data sizes, too few positions
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].positions = {1.0, 2.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Incompatible data sizes, too many positions
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Incompatible data sizes, too few velocities
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].velocities = {1.0, 2.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Incompatible data sizes, too few accelerations
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].accelerations = {1.0, 2.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Effort is not supported in trajectory message
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].effort = {1.0, 2.0, 3.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Non-strictly increasing waypoint times
+  traj_msg = good_traj_msg;
+  traj_msg.points.push_back(traj_msg.points.front());
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+}
+
+/**
+ * @brief Test invalid velocity at trajectory end with parameter set to false
+ */
+TEST_F(
+  TrajectoryOperationsTest,
+  expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false)
+{
+  params_.allow_nonzero_velocity_at_trajectory_end = false;
+
+  trajectory_msgs::msg::JointTrajectory traj_msg;
+  traj_msg.joint_names = joint_names_;
+  traj_msg.header.stamp = rclcpp::Time(0);
+
+  // empty message (no throw!)
+  ASSERT_NO_THROW(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Nonzero velocity at trajectory end!
+  traj_msg.points.resize(1);
+  traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25);
+  traj_msg.points[0].positions = {1.0, 2.0, 3.0};
+  traj_msg.points[0].velocities = {-1.0, -2.0, -3.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+}
+
+/**
+ * @brief missing_positions_message_accepted Test mismatched joint and reference vector sizes
+ *
+ * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or
+ * velocities are accepted
+ */
+TEST_F(TrajectoryOperationsTest, missing_positions_message_accepted)
+{
+  params_.allow_integration_in_goal_trajectories = true;
+
+  trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;
+
+  good_traj_msg.joint_names = joint_names_;
+  good_traj_msg.header.stamp = rclcpp::Time(0);
+  good_traj_msg.points.resize(1);
+  good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25);
+  good_traj_msg.points[0].positions = {1.0, 2.0, 3.0};
+  good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0};
+  good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0};
+  EXPECT_TRUE(validate_trajectory_msg(good_traj_msg, *logger_, clock_.now(), params_));
+
+  // No position data
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].positions.clear();
+  EXPECT_TRUE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // No position and velocity data
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].positions.clear();
+  traj_msg.points[0].velocities.clear();
+  EXPECT_TRUE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // All empty
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].positions.clear();
+  traj_msg.points[0].velocities.clear();
+  traj_msg.points[0].accelerations.clear();
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Incompatible data sizes, too few positions
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].positions = {1.0, 2.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Incompatible data sizes, too many positions
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Incompatible data sizes, too few velocities
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].velocities = {1.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+
+  // Incompatible data sizes, too few accelerations
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].accelerations = {2.0};
+  EXPECT_FALSE(validate_trajectory_msg(traj_msg, *logger_, clock_.now(), params_));
+}
+
+/**
+ * @brief test_msg_sorting test sort_to_local_joint_order()
+ *
+ */
+TEST_F(TrajectoryOperationsTest, test_msg_sorting)
+{
+  std::vector<size_t> jumble_map = {1, 2, 0};
+  auto vector_val = std::vector<double>{1.0, 2.0, 3.0};
+
+  trajectory_msgs::msg::JointTrajectory good_traj_msg, traj_msg;
+
+  good_traj_msg.joint_names = joint_names_;
+  good_traj_msg.header.stamp = rclcpp::Time(0);
+  good_traj_msg.points.resize(1);
+  good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25);
+  good_traj_msg.points[0].positions = vector_val;
+  good_traj_msg.points[0].velocities = vector_val;
+  good_traj_msg.points[0].accelerations = vector_val;
+  good_traj_msg.points[0].effort = vector_val;
+
+  // test if the joint order is not changed
+  {
+    auto traj_msg_ptr = std::make_shared<trajectory_msgs::msg::JointTrajectory>(good_traj_msg);
+    EXPECT_THAT(traj_msg_ptr, IsTrajMsgSorted(joint_names_, vector_val));
+    ASSERT_NO_THROW(sort_to_local_joint_order(traj_msg_ptr, *logger_, params_));
+    EXPECT_THAT(traj_msg_ptr, IsTrajMsgSorted(joint_names_, vector_val));
+  }
+
+  // test if the joint order is sorted
+  traj_msg = good_traj_msg;
+  traj_msg.joint_names = get_jumbled_values<std::string>(jumble_map, joint_names_);
+  traj_msg.points[0].positions = get_jumbled_values<double>(jumble_map, vector_val);
+  traj_msg.points[0].velocities = get_jumbled_values<double>(jumble_map, vector_val);
+  traj_msg.points[0].accelerations = get_jumbled_values<double>(jumble_map, vector_val);
+  traj_msg.points[0].effort = get_jumbled_values<double>(jumble_map, vector_val);
+  {
+    auto traj_msg_ptr = std::make_shared<trajectory_msgs::msg::JointTrajectory>(traj_msg);
+    EXPECT_THAT(traj_msg_ptr, testing::Not(IsTrajMsgSorted(joint_names_, vector_val)));
+    ASSERT_NO_THROW(sort_to_local_joint_order(traj_msg_ptr, *logger_, params_));
+    EXPECT_THAT(traj_msg_ptr, IsTrajMsgSorted(joint_names_, vector_val));
+  }
+
+  // test if no issue if called with different size (should not happen, because no valid_traj_msg)
+  traj_msg = good_traj_msg;
+  traj_msg.joint_names = get_jumbled_values<std::string>({1, 2}, joint_names_);
+  ASSERT_NO_THROW(sort_to_local_joint_order(
+    std::make_shared<trajectory_msgs::msg::JointTrajectory>(traj_msg), *logger_, params_));
+  traj_msg = good_traj_msg;
+  traj_msg.points[0].positions.resize(1);
+  traj_msg.points[0].velocities.resize(1);
+  traj_msg.points[0].accelerations.resize(1);
+  traj_msg.points[0].effort.resize(1);
+  ASSERT_NO_THROW(sort_to_local_joint_order(
+    std::make_shared<trajectory_msgs::msg::JointTrajectory>(traj_msg), *logger_, params_));
+}
+
+TEST_F(TrajectoryOperationsTest, test_fill_partial_goal)
+{
+  // Create a trajectory message
+  auto vector_val = std::vector<double>{1.0, 2.0, 3.0};
+  trajectory_msgs::msg::JointTrajectory traj_msg_partial;
+  std::vector<size_t> jumble_map = {0, 2};
+  traj_msg_partial.joint_names = get_jumbled_values<std::string>(jumble_map, joint_names_);
+  traj_msg_partial.header.stamp = rclcpp::Time(0);
+  traj_msg_partial.points.resize(1);
+  traj_msg_partial.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25);
+  traj_msg_partial.points[0].positions = get_jumbled_values<double>(jumble_map, vector_val);
+  traj_msg_partial.points[0].velocities = get_jumbled_values<double>(jumble_map, vector_val);
+  traj_msg_partial.points[0].accelerations = get_jumbled_values<double>(jumble_map, vector_val);
+  traj_msg_partial.points[0].effort = get_jumbled_values<double>(jumble_map, vector_val);
+  auto traj_msg_ptr = std::make_shared<trajectory_msgs::msg::JointTrajectory>(traj_msg_partial);
+
+  // Function to return default position for a joint
+  auto get_default_position = [](size_t index) { return 10. + static_cast<double>(index); };
+
+  // Call fill_partial_goal function
+  fill_partial_goal(traj_msg_ptr, get_default_position, params_);
+
+  // Check if the fields are correctly filled (missing joints are added at the end)
+  EXPECT_THAT(
+    traj_msg_ptr->joint_names,
+    testing::ContainerEq(get_jumbled_values<std::string>({0, 2, 1}, joint_names_)));
+  ASSERT_THAT(traj_msg_ptr->points, testing::SizeIs(1));
+  const auto point = traj_msg_ptr->points[0];
+  ASSERT_THAT(point.positions, testing::SizeIs(params_.joints.size()));
+  ASSERT_THAT(point.velocities, testing::SizeIs(params_.joints.size()));
+  ASSERT_THAT(point.accelerations, testing::SizeIs(params_.joints.size()));
+  ASSERT_THAT(point.effort, testing::SizeIs(params_.joints.size()));
+  for (size_t i = 0; i < jumble_map.size(); ++i)
+  {
+    // should be original points
+    EXPECT_EQ(point.positions[i], traj_msg_partial.points[0].positions[i]);
+    EXPECT_EQ(point.velocities[i], traj_msg_partial.points[0].velocities[i]);
+    EXPECT_EQ(point.accelerations[i], traj_msg_partial.points[0].accelerations[i]);
+    EXPECT_EQ(point.effort[i], traj_msg_partial.points[0].effort[i]);
+  }
+  for (size_t i = jumble_map.size(); i < params_.joints.size(); ++i)
+  {
+    // Check if positions are set to default position, and velocities, accelerations, and efforts
+    // are set to 0.0
+    auto it = std::find(params_.joints.begin(), params_.joints.end(), traj_msg_ptr->joint_names[i]);
+    ASSERT_NE(it, params_.joints.end()) << "Joint " << traj_msg_ptr->joint_names[i] << " not found";
+    auto index = std::distance(params_.joints.begin(), it);
+    EXPECT_EQ(point.positions[i], get_default_position(index));
+    EXPECT_EQ(point.velocities[i], 0.0);
+    EXPECT_EQ(point.accelerations[i], 0.0);
+    EXPECT_EQ(point.effort[i], 0.0);
+  }
+}