Keepalive freezes trajectory execution

[gbartyzel]

During the integration of urcl with my custom control software for UR robots I faced a problem with trajectory interface and keepalive. Sending keepalive to the robot during trajectory execution stops the move. Moreover, if I freeze the keepalive until the trajectory is finished, the program on the robot is stopped by the socket error. Is this a bug?

[fmauch]

Could you please provide a short code snippet that explains what you are trying to do? That would help very much in answering your question.

[gbartyzel]

This snippet is a part of the project, but I think that it should be enough. Method connect() is responsible for acquiring a connection with RTDE and also for starting the keep-alive thread. This thread writes a keep-alive thread when the robot is not moving. Method handle_trajectory() is responsible for sending a trajectory to the robot. When the trajectory is received, this method blocks the keep-alive thread.

#include <robotic_manager_core/ur/ur_interface.hpp>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>

namespace roboticmanager {

URInterface::URInterface(std::string robot_ip, std::string &host_ip,
                         std::string script_file, std::string &output_recipe,
                         std::string input_recipe, bool headless_mode,
                         bool use_tool_comm, std::string calibration_checksum,
                         double servoj_gain, double servoj_lookahead_time,
                         bool non_blocking_read,
                         UrToolParameters tool_parameters,
                         uint32_t reverse_port, uint32_t script_sender_port,
                         uint32_t trajectory_port)
    : commonlibraries::LoggingInterface("URInterface",
                                        spdlog::get("rm_logger")),
      robot_ip_(std::move(robot_ip)), host_ip_(std::move(host_ip)),
      script_file_(std::move(script_file)),
      output_recipe_(std::move(output_recipe)),
      input_recipe_(std::move(input_recipe)), headless_mode_(headless_mode),
      use_tool_com_(use_tool_comm),
      calibration_checksum_(std::move(calibration_checksum)),
      servoj_gain_(servoj_gain), servoj_lookahead_time_(servoj_lookahead_time),
      tool_parameters_(std::move(tool_parameters)), reverse_port_(reverse_port),
      script_sender_port_(script_sender_port),
      trajectory_port_(trajectory_port), non_blocking_read_(non_blocking_read),
      trajectory_received_(false),
      trajectory_status_(
          urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE),
      joints_pos_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
      joints_vel_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
      joints_effort_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
      tcp_pose_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
      tcp_vel_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}, tcp_ft_cur_{{0.0, 0.0, 0.0,
                                                                 0.0, 0.0,
                                                                 0.0}} {}
URInterface::URInterface(std::string robot_ip, std::string host_ip,
                         std::string script_file, std::string output_recipe,
                         std::string input_recipe, bool headless_mode,
                         bool use_tool_comm, std::string calibration_checksum,
                         double servoj_gain, double servoj_lookahead_time,
                         bool non_blocking_read,
                         UrToolParameters tool_parameters)
    : URInterface(robot_ip, host_ip, script_file, output_recipe, input_recipe,
                  headless_mode, use_tool_comm, calibration_checksum,
                  servoj_gain, servoj_lookahead_time, non_blocking_read,
                  tool_parameters, 50001, 50002, 50003){};

URInterface::~URInterface() { disconnect(); }

std::unique_ptr<urcl::ToolCommSetup> URInterface::create_tool_communication() {
  std::unique_ptr<urcl::ToolCommSetup> tool_comm_setup;
  if (use_tool_com_) {
    tool_comm_setup = std::make_unique<urcl::ToolCommSetup>();

    using ToolVoltageT = std::underlying_type<urcl::ToolVoltage>::type;
    ToolVoltageT tool_voltage;
    tool_voltage = tool_parameters_.tool_voltage;

    tool_comm_setup->setToolVoltage(
        static_cast<urcl::ToolVoltage>(tool_voltage));

    using ParityT = std::underlying_type<urcl::Parity>::type;
    ParityT parity;
    parity = tool_parameters_.tool_parity;
    tool_comm_setup->setParity(static_cast<urcl::Parity>(parity));

    tool_comm_setup->setBaudRate(
        static_cast<uint32_t>(tool_parameters_.tool_baud_rate));

    tool_comm_setup->setStopBits(
        static_cast<uint32_t>(tool_parameters_.tool_stop_bits));

    tool_comm_setup->setRxIdleChars(tool_parameters_.tool_rx_idle_chars);

    tool_comm_setup->setTxIdleChars(tool_parameters_.tool_tx_idle_chars);
  }
  return tool_comm_setup;
}

RMStatusCode URInterface::move(const JointTarget &target) {
  vector6d_t joints;
  trajectory_t traj;
  std::copy_n(target.joints.begin(), 6, joints.begin());
  traj.push_back(joints);
  return handle_trajectory(
      traj, std::vector<bool>{false},
      std::vector<float>{static_cast<float>(target.goal_time)},
      std::vector<float>{static_cast<float>(target.radius)});
}

RMStatusCode URInterface::handle_trajectory(
    const trajectory_t &target, const std::vector<bool> cartesian,
    const std::vector<float> goal_time, const std::vector<float> blend_radius) {
  trajectory_received_ = true;
  trajectory_status_ =
      urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE;
  auto ctrl_msg_res = ur_driver_->writeTrajectoryControlMessage(
      urcl::control::TrajectoryControlMessage::TRAJECTORY_START, target.size());
  if (!ctrl_msg_res) {
    return RMStatusCode::FAILED;
  }

  for (size_t i = 0; i < target.size(); i++) {
    auto exec_res = ur_driver_->writeTrajectoryPoint(
        target[i], cartesian[i], goal_time[i], blend_radius[i]);
    if (!exec_res) {
      return RMStatusCode::FAILED;
    }
  }

  while (trajectory_status_ !=
         urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) {
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }
  trajectory_received_ = false;
  return RMStatusCode::SUCCESS;
}

void URInterface::trajectory_callabck(urcl::control::TrajectoryResult res) {
  trajectory_status_ = res;
}

RMStatusCode URInterface::connect() {
  auto tool_comm_setup = create_tool_communication();
  ur_driver_ = std::make_unique<urcl::UrDriver>(
      robot_ip_, script_file_, output_recipe_, input_recipe_,
      std::bind(&URInterface::handle_robot_program, this,
                std::placeholders::_1),
      headless_mode_, std::move(tool_comm_setup), calibration_checksum_,
      reverse_port_, script_sender_port_, servoj_gain_, servoj_lookahead_time_,
      non_blocking_read_, host_ip_, trajectory_port_);
  ur_driver_->registerTrajectoryDoneCallback(
      std::bind(&URInterface::trajectory_callabck, this, _1));

  driver_stopped_ = false;
  std::thread t([&]() {
    while (!driver_stopped_) {
      if (!trajectory_received_) {
        ur_driver_->writeKeepalive();
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(2));
    }
  });
  t.detach();
  ur_driver_->startRTDECommunication();
  return RMStatusCode::SUCCESS;
}

RMStatusCode URInterface::disconnect() {
  driver_stopped_ = true;
  // Wait for joining detached thread.
  std::this_thread::sleep_for(std::chrono::milliseconds(100));
  ur_driver_->stopControl();
  ur_driver_.reset();
  return RMStatusCode::SUCCESS;
}

RMStatusCode URInterface::reconnect() {
  log_info("Reconnecting to UR Robot");
  auto disconnected = disconnect();
  if (disconnected != RMStatusCode::SUCCESS)
    return RMStatusCode::FAILED;
  return connect();
}

void URInterface::handle_robot_program(bool program_running) {
  robot_program_running_ = program_running;
}

} // namespace roboticmanager 

[gavanderhoorn]

  std::thread t([&]() {
    while (!driver_stopped_) {
      if (!trajectory_received_) {
        ur_driver_->writeKeepalive();
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(2));
    }
  });

is it really necessary to send keepalive msgs at 2 ms intervals?

[gbartyzel]

I think not, but I was experimenting with different interval values. The same behaviour was with a 100 ms interval etc.

[fmauch]

I'll try to have a look at this once I'm back in the office. @stefanscherzinger assigning this to me so it pops up in my To-Do-List. If you want to give it a shot before me, please reassign.

[gbartyzel]

@fmauch Sorry for the late resposne, holidays etc. I can try to look at this bug, but it will not be as quick as possible.

[haellingsen]

@gbartyzel @fmauch Any update on this?