franka_combined_control_node.cpp

Go to the documentation of this file.

// Copyright (c) 2019 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
 
#include <controller_manager/controller_manager.h>
#include <franka_hw/franka_combined_hw.h>
#include <ros/ros.h>
 
#include <franka/control_tools.h>
#include <sched.h>
#include <string>
 
int main(int argc, char** argv) {
  ros::init(argc, argv, "franka_combined_control_node");
 
  ros::AsyncSpinner spinner(4);
  spinner.start();
 
  ros::NodeHandle private_node_handle("~");
  franka_hw::FrankaCombinedHW franka_control;
  if (!franka_control.init(private_node_handle, private_node_handle)) {
    ROS_ERROR("franka_combined_control_node:: Initialization of FrankaCombinedHW failed!");
    return 1;
  }
 
  // set current thread to real-time priority
  std::string error_message;
  if (!franka::setCurrentThreadToHighestSchedulerPriority(&error_message)) {
    ROS_ERROR("franka_combined_control_node: Failed to set thread priority to real-time. Error: %s",
              error_message.c_str());
    return 1;
  }
 
  controller_manager::ControllerManager cm(&franka_control, private_node_handle);
  ros::Duration period(0.001);
  ros::Rate rate(period);
 
  while (ros::ok()) {
    rate.sleep();
    ros::Time now = ros::Time::now();
    franka_control.read(now, period);
    cm.update(now, period, franka_control.controllerNeedsReset());
    if (!franka_control.hasError()) {
      franka_control.write(now, period);
    } else {
      ROS_DEBUG_THROTTLE(5,
                         "franka_combined_control_node: The HW is in error state."
                         "To recover, call the recovery action.");
    }
  }
 
  return 0;
}