message: fixed freezing issues, debug prints
diff: // Use of this source code is governed by the Apache-2.0 license, see LICENSE
  #include <algorithm>
  #include <atomic>
  +#include <thread>
  +#include <chrono>
  #include <actionlib/server/simple_action_server.h>
  #include <controller_manager/controller_manager.h>
  #include <std_srvs/Trigger.h>
  using franka_hw::ServiceContainer;
  +using namespace std::chrono_literals;
  int main(int argc, char** argv) {
  ros::init(argc, argv, "franka_control_node");
  @@ -35,31 +38,44 @@ int main(int argc, char** argv) {
  auto disconnect = [&](std_srvs::Trigger::Request& request,
  std_srvs::Trigger::Response& response) -> bool {
  + ROS_INFO("franka_control, disconnect, db1");
  if (franka_control.controllerActive()) {
  response.success = false;
  response.message = "Controller is active. Cannont disconnect while a controller is running.";
  return true;
  }
  + ROS_INFO("franka_control, disconnect, db2");
  response.success = true;
  response.message = "";
  services.reset();
  + ROS_INFO("franka_control, disconnect, db3");
  recovery_action_server.reset();
  - return franka_control.disconnect();
  + ROS_INFO("franka_control, disconnect, db4");
  + auto result = franka_control.disconnect();
  + ROS_INFO("franka_control, disconnect, db5 finished destroying robot");
  + return true;
  };
  auto connect = [&](std_srvs::Trigger::Request& request,
  std_srvs::Trigger::Response& response) -> bool {
  + ROS_INFO("Connect db1");
  if (franka_control.connected()) {
  response.success = false;
  response.message = "Already conneceted to robot. Cannot connect twice.";
  return true;
  }
  +ROS_INFO("Connect db2");
  franka_control.connect();
  +ROS_INFO("Connect db3");
  std::lock_guard<std::mutex> lock(franka_control.robotMutex());
  +ROS_INFO("Connect db4");
  auto& robot = franka_control.robot();
  +ROS_INFO("Connect db5");
  // ServiceContainer services;
  + services = std::make_unique<ServiceContainer>();
  franka_hw::setupServices(robot, franka_control.robotMutex(), node_handle, *services);
  +ROS_INFO("Connect db6");
  recovery_action_server =
  std::make_unique<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>(
  @@ -76,12 +92,14 @@ int main(int argc, char** argv) {
  }
  },
  false);
  +ROS_INFO("Connect db7");
  recovery_action_server->start();
  +ROS_INFO("Connect db8");
  // Initialize robot state before loading any controller
  franka_control.update(robot.readOnce());
  -
  +ROS_INFO("Connect db9");
  response.success = true;
  response.message = "";
  return true;
  @@ -115,11 +133,14 @@ int main(int argc, char** argv) {
  if (franka_control.connected()) {
  std::lock_guard<std::mutex> lock(franka_control.robotMutex());
  franka_control.update(franka_control.robot().readOnce());
  - }
  +
  ros::Time now = ros::Time::now();
  control_manager.update(now, now - last_time);
  franka_control.checkJointLimits();
  last_time = now;
  + } else {
  + std::this_thread::sleep_for(10ms);
  + }
  if (!ros::ok()) {
  return 0;
  @@ -147,7 +168,7 @@ int main(int argc, char** argv) {
  has_error = true;
  }
  }
  - ros::Duration(0.001).sleep();
  + ROS_INFO_THROTTLE(1, "franka_control, main loop");
  }
  return 0;
  