Fixup JTC and Trajectory to compile.

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -291,7 +291,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   bool has_active_trajectory() const;
 
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  void publish_state(
+  virtual void publish_state(
     const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
     const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error);
 
@@ -331,6 +331,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
       joint_interface[index].get().set_value(trajectory_point_interface[index]);
     }
   };
+
+  void resize_joint_trajectory_point(
+    trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
   // END: helper methods
 
 private:
@@ -340,8 +343,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     const std::vector<std::string> & interface_type_list, const std::string & interface_type);
 
   void init_hold_position_msg();
-  void resize_joint_trajectory_point(
-    trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
   void resize_joint_trajectory_point_command(
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
 

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

@@ -161,6 +161,7 @@ class Trajectory
   trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_;
 
   bool sampled_already_ = false;
+  trajectory_msgs::msg::JointTrajectoryPoint previous_state_;
 };
 
 /**

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -410,9 +410,7 @@ controller_interface::return_type JointTrajectoryController::update(
     }
   }
 
-  publish_state(
-    time, state_desired_, state_current_, state_error_, splines_state_, ruckig_state_,
-    ruckig_input_state_);
+  publish_state(time, state_desired_, state_current_, state_error_);
   return controller_interface::return_type::OK;
 }
 
@@ -935,12 +933,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
       dof_, {false, std::numeric_limits<double>::quiet_NaN(),
              std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN()});
 
-    // Here we read current reset dofs flags and clear it. This is done so we can add this new request
-    // to the existing reset flags. This logic prevents this new request from overwriting any previous request
-    // that hasn't been processed yet.
-    // The one assumption made here is that the current reset flags are not going to be processed
-    // between the two calls here to read and reset, which is a highly unlikely scenario. Even if it was,
-    // the behavior is fairly benign in that the dofs in the previous request will be reset twice.
+    // Here we read current reset dofs flags and clear it. This is done so we can add this new
+    // request to the existing reset flags. This logic prevents this new request from overwriting
+    // any previous request that hasn't been processed yet. The one assumption made here is that the
+    // current reset flags are not going to be processed between the two calls here to read and
+    // reset, which is a highly unlikely scenario. Even if it was, the behavior is fairly benign in
+    // that the dofs in the previous request will be reset twice.
     auto reset_flags = *reset_dofs_flags_.readFromNonRT();
     reset_dofs_flags_.writeFromNonRT(reset_flags_reset);
 
@@ -1182,9 +1180,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_shutdown(
 
 void JointTrajectoryController::publish_state(
   const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
-  const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error,
-  const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target,
-  const JointTrajectoryPoint & ruckig_input)
+  const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error)
 {
   if (state_publisher_->trylock())
   {
@@ -1211,39 +1207,6 @@ void JointTrajectoryController::publish_state(
 
     state_publisher_->unlockAndPublish();
   }
-
-  if (splines_output_publisher_ && splines_output_publisher_->trylock())
-  {
-    splines_output_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp;
-    splines_output_publisher_->msg_.actual.positions = splines_output.positions;
-    splines_output_publisher_->msg_.actual.velocities = splines_output.velocities;
-    splines_output_publisher_->msg_.actual.accelerations = splines_output.accelerations;
-    splines_output_publisher_->msg_.actual.effort = splines_output.effort;
-
-    splines_output_publisher_->unlockAndPublish();
-  }
-
-  if (ruckig_input_publisher_ && ruckig_input_publisher_->trylock())
-  {
-    ruckig_input_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp;
-    ruckig_input_publisher_->msg_.actual.positions = ruckig_input.positions;
-    ruckig_input_publisher_->msg_.actual.velocities = ruckig_input.velocities;
-    ruckig_input_publisher_->msg_.actual.accelerations = ruckig_input.accelerations;
-    ruckig_input_publisher_->msg_.actual.effort = ruckig_input.effort;
-
-    ruckig_input_publisher_->unlockAndPublish();
-  }
-
-  if (ruckig_input_target_publisher_ && ruckig_input_target_publisher_->trylock())
-  {
-    ruckig_input_target_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp;
-    ruckig_input_target_publisher_->msg_.actual.positions = ruckig_input_target.positions;
-    ruckig_input_target_publisher_->msg_.actual.velocities = ruckig_input_target.velocities;
-    ruckig_input_target_publisher_->msg_.actual.accelerations = ruckig_input_target.accelerations;
-    ruckig_input_target_publisher_->msg_.actual.effort = ruckig_input_target.effort;
-
-    ruckig_input_target_publisher_->unlockAndPublish();
-  }
 }
 
 void JointTrajectoryController::topic_callback(

joint_trajectory_controller/src/trajectory.cpp

@@ -24,14 +24,14 @@
 namespace joint_trajectory_controller
 {
 Trajectory::Trajectory()
-: trajectory_start_time_(0), time_before_traj_msg_(0), sampled_already_(false),
+: trajectory_start_time_(0), time_before_traj_msg_(0), sampled_already_(false)
 {
 }
 
 Trajectory::Trajectory(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)
 : trajectory_msg_(joint_trajectory),
   trajectory_start_time_(static_cast<rclcpp::Time>(joint_trajectory->header.stamp)),
-  sampled_already_(false),
+  sampled_already_(false)
 {
 }
 
@@ -217,10 +217,7 @@ bool Trajectory::sample(
     }
   }
 
-  // do not do splines when trajectory has finished because the time is achieved
-  interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state)
-
-    if (joint_limiter)
+  if (joint_limiter)
   {
     // When running Joint Limiter we might not get to the last_point in time - so SplineIt!
     interpolate_between_points(