Extend trajectory tests to check velocities and accelerations

Remove duplicated code
Make JTC members protected for easier extension and tests
Add invalid_message test
Remove sleeps and cleanup tests
Fix test errors detected with valgrind
Cleanup tests, extend invalid_names and add replace trajectory test
Reject old trajectories
Add ignore old trajectories test

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -107,7 +107,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
 
-private:
+protected:
   std::vector<std::string> joint_names_;
   std::vector<std::string> write_op_names_;
 
@@ -143,7 +143,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
 
   rclcpp_action::Server<FollowJTrajAction>::SharedPtr action_server_;
-  bool allow_partial_joints_goal_;
+  bool allow_partial_joints_goal_ = false;
   RealtimeGoalHandlePtr rt_active_goal_;     ///< Currently active action goal, if any.
   rclcpp::TimerBase::SharedPtr goal_handle_timer_;
   rclcpp::Duration action_monitor_period_ = rclcpp::Duration(RCUTILS_MS_TO_NS(50));

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -525,7 +525,6 @@ void JointTrajectoryController::publish_state(
 
   if (state_publisher_ && state_publisher_->trylock()) {
     last_state_publish_time_ = lifecycle_node_->now();
-
     state_publisher_->msg_.header.stamp = last_state_publish_time_;
     state_publisher_->msg_.desired.positions = desired_state.positions;
     state_publisher_->msg_.desired.velocities = desired_state.velocities;
@@ -712,6 +711,21 @@ bool JointTrajectoryController::validate_trajectory_msg(
     return false;
   }
 
+  const auto trajectory_start_time = static_cast<rclcpp::Time>(trajectory.header.stamp);
+  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 < lifecycle_node_->now()) {
+      RCLCPP_ERROR(
+        lifecycle_node_->get_logger(),
+        "Received trajectory with non zero time start time (%f) that ends on the past (%f)",
+        trajectory_start_time.seconds(), trajectory_end_time.seconds());
+      return false;
+    }
+  }
+
   for (auto i = 0ul; i < trajectory.joint_names.size(); ++i) {
     const std::string & incoming_joint_name = trajectory.joint_names[i];
 
@@ -724,6 +738,57 @@ bool JointTrajectoryController::validate_trajectory_msg(
       return false;
     }
   }
+
+  rclcpp::Duration previous_traj_time(0);
+  for (auto i = 0ul; i < trajectory.points.size(); ++i) {
+    if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) {
+      RCLCPP_ERROR(
+        lifecycle_node_->get_logger(),
+        "Time between points %u and %u 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;
+
+    if (trajectory.joint_names.size() != trajectory.points[i].positions.size()) {
+      RCLCPP_ERROR(
+        lifecycle_node_->get_logger(),
+        "Mismatch between joint_names (%u) and positions (%u) at point #%u.",
+        trajectory.joint_names.size(), trajectory.points[i].positions.size(), i);
+      return false;
+    }
+
+    if (!trajectory.points[i].velocities.empty() &&
+      trajectory.joint_names.size() != trajectory.points[i].velocities.size())
+    {
+      RCLCPP_ERROR(
+        lifecycle_node_->get_logger(),
+        "Mismatch between joint_names (%u) and velocities (%u) at point #%u.",
+        trajectory.joint_names.size(), trajectory.points[i].velocities.size(), i);
+      return false;
+    }
+
+    if (!trajectory.points[i].accelerations.empty() &&
+      trajectory.joint_names.size() != trajectory.points[i].accelerations.size())
+    {
+      RCLCPP_ERROR(
+        lifecycle_node_->get_logger(),
+        "Mismatch between joint_names (%u) and accelerations (%u) at point #%u.",
+        trajectory.joint_names.size(), trajectory.points[i].accelerations.size(), i);
+      return false;
+    }
+
+    if (!trajectory.points[i].effort.empty() &&
+      trajectory.joint_names.size() != trajectory.points[i].effort.size())
+    {
+      RCLCPP_ERROR(
+        lifecycle_node_->get_logger(),
+        "Mismatch between joint_names (%u) and effort (%u) at point #%u.",
+        trajectory.joint_names.size(), trajectory.points[i].effort.size(), i);
+      return false;
+    }
+  }
   return true;
 }
 

joint_trajectory_controller/test/test_trajectory.cpp

@@ -72,22 +72,22 @@ TEST(TestTrajectory, sample_trajectory) {
 
   trajectory_msgs::msg::JointTrajectoryPoint p1;
   p1.positions.push_back(1.0);
-  p1.velocities.push_back(0.0);
-  p1.accelerations.push_back(0.0);
+  p1.velocities.push_back(-1.0);
+  p1.accelerations.push_back(3.0);
   p1.time_from_start.sec = 1;
   p1.time_from_start.nanosec = 0;
 
   trajectory_msgs::msg::JointTrajectoryPoint p2;
   p2.positions.push_back(2.0);
-  p2.velocities.push_back(0.0);
-  p2.accelerations.push_back(0.0);
+  p2.velocities.push_back(-2.0);
+  p2.accelerations.push_back(6.0);
   p2.time_from_start.sec = 2;
   p2.time_from_start.nanosec = 0;
 
   trajectory_msgs::msg::JointTrajectoryPoint p3;
   p3.positions.push_back(3.0);
-  p3.velocities.push_back(0.0);
-  p3.accelerations.push_back(0.0);
+  p3.velocities.push_back(-3.0);
+  p3.accelerations.push_back(9.0);
   p3.time_from_start.sec = 3;
   p3.time_from_start.nanosec = 0;
 
@@ -115,37 +115,53 @@ TEST(TestTrajectory, sample_trajectory) {
   ASSERT_EQ(traj.begin(), start);
   ASSERT_EQ(traj.begin(), end);
   EXPECT_EQ(0.0, expected_state.positions[0]);
+  EXPECT_EQ(0.0, expected_state.velocities[0]);
+  EXPECT_EQ(0.0, expected_state.accelerations[0]);
 
   // sample before trajectory starts
   traj.sample(time_now - rclcpp::Duration::from_seconds(0.5), expected_state, start, end);
   ASSERT_EQ(traj.begin(), start);
   ASSERT_EQ(traj.begin(), end);
   EXPECT_EQ(0.0, expected_state.positions[0]);
+  EXPECT_EQ(0.0, expected_state.velocities[0]);
+  EXPECT_EQ(0.0, expected_state.accelerations[0]);
 
   traj.sample(time_now + rclcpp::Duration::from_seconds(0.5), expected_state, start, end);
   ASSERT_EQ(traj.begin(), start);
   ASSERT_EQ(traj.begin(), end);
   EXPECT_EQ(0.5, expected_state.positions[0]);
+  EXPECT_EQ(-0.5, expected_state.velocities[0]);
+  EXPECT_EQ(1.5, expected_state.accelerations[0]);
 
   traj.sample(time_now + rclcpp::Duration::from_seconds(1.0), expected_state, start, end);
   ASSERT_EQ(traj.begin(), start);
   ASSERT_EQ((++traj.begin()), end);
   EXPECT_EQ(1.0, expected_state.positions[0]);
+  EXPECT_EQ(-1.0, expected_state.velocities[0]);
+  EXPECT_EQ(3.0, expected_state.accelerations[0]);
 
   traj.sample(time_now + rclcpp::Duration::from_seconds(1.5), expected_state, start, end);
   ASSERT_EQ(traj.begin(), start);
   ASSERT_EQ((++traj.begin()), end);
   EXPECT_EQ(1.5, expected_state.positions[0]);
+  EXPECT_EQ(-1.5, expected_state.velocities[0]);
+  EXPECT_EQ(4.5, expected_state.accelerations[0]);
 
   traj.sample(time_now + rclcpp::Duration::from_seconds(2.5), expected_state, start, end);
   EXPECT_EQ(2.5, expected_state.positions[0]);
+  EXPECT_EQ(-2.5, expected_state.velocities[0]);
+  EXPECT_EQ(7.5, expected_state.accelerations[0]);
 
   traj.sample(time_now + rclcpp::Duration::from_seconds(3.0), expected_state, start, end);
   EXPECT_EQ(3.0, expected_state.positions[0]);
+  EXPECT_EQ(-3.0, expected_state.velocities[0]);
+  EXPECT_EQ(9.0, expected_state.accelerations[0]);
 
   // sample past given points
   traj.sample(time_now + rclcpp::Duration::from_seconds(3.125), expected_state, start, end);
   ASSERT_EQ((--traj.end()), start);
   ASSERT_EQ(traj.end(), end);
   EXPECT_EQ(3.0, expected_state.positions[0]);
+  EXPECT_EQ(-3.0, expected_state.velocities[0]);
+  EXPECT_EQ(9.0, expected_state.accelerations[0]);
 }