Make sure we acknowledge a new trajectory before sending points

If we miss the new trajectory, wen cannot resize the vectors
appropriately.

Author: urfeex

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

@@ -81,6 +81,7 @@ namespace ur_controllers
  * 5.0: The robot finished executing the trajectory.
  */
 const double TRANSFER_STATE_IDLE = 0.0;
+const double TRANSFER_STATE_NEW_TRAJECTORY = 6.0;
 const double TRANSFER_STATE_WAITING_FOR_POINT = 1.0;
 const double TRANSFER_STATE_TRANSFERRING = 2.0;
 const double TRANSFER_STATE_TRANSFER_DONE = 3.0;

ur_controllers/src/passthrough_trajectory_controller.cpp

@@ -268,7 +268,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
       active_trajectory_elapsed_time_ = rclcpp::Duration(0, 0);
       max_trajectory_time_ =
           rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start));
-      write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_WAITING_FOR_POINT);
+      write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_NEW_TRAJECTORY);
       write_success &=
           trajectory_size_command_interface_->get().set_value(static_cast<double>(active_joint_traj_.points.size()));
     }

ur_robot_driver/src/hardware_interface.cpp

@@ -1340,7 +1340,7 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller()
   // We should abort and are not in state IDLE
   if (passthrough_trajectory_abort_ == 1.0 && passthrough_trajectory_transfer_state_ != 0.0) {
     ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL);
-  } else if (passthrough_trajectory_transfer_state_ == 1.0) {
+  } else if (passthrough_trajectory_transfer_state_ == 6.0) {
     if (passthrough_trajectory_size_ != trajectory_joint_positions_.size()) {
       RCLCPP_INFO(get_logger(), "Got a new trajectory with %lu points.",
                   static_cast<size_t>(passthrough_trajectory_size_));
@@ -1352,6 +1352,7 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller()
       point_index_sent = 0;
       trajectory_started = false;
       last_time = 0.0;
+      passthrough_trajectory_transfer_state_ = 1.0;
     }
   } else if (passthrough_trajectory_transfer_state_ == 2.0) {
     passthrough_trajectory_abort_ = 0.0;