franka_combinable_hw.h

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
#pragma once

#include <atomic>
#include <functional>
#include <memory>
#include <string>
#include <thread>

#include <franka/exception.h>
#include <franka/robot_state.h>

#include <actionlib/server/simple_action_server.h>
#include <franka_hw/franka_hw.h>
#include <franka_hw/services.h>
#include <franka_msgs/ErrorRecoveryAction.h>
#include <ros/ros.h>

namespace franka_hw {

class FrankaCombinableHW : public FrankaHW {
 public:
  FrankaCombinableHW();

  void initROSInterfaces(ros::NodeHandle& robot_hw_nh) override;

  void control(const std::function<bool(const ros::Time&, const ros::Duration&)>&
                   ros_callback =  // NOLINT (google-default-arguments)
               [](const ros::Time&, const ros::Duration&) {
                 return true;
               }) const override;  // NOLINT (google-default-arguments)

  bool checkForConflict(const std::list<hardware_interface::ControllerInfo>& info) const override;

  void read(const ros::Time& /*time*/, const ros::Duration& /*period*/) override;

  void write(const ros::Time& /*time*/, const ros::Duration& period) override;

  std::string getArmID() const noexcept;

  void triggerError();

  bool hasError() const noexcept;

  void resetError();

  bool controllerNeedsReset() const noexcept;

 private:
  template <typename T>
  T libfrankaUpdateCallback(const T& command,
                            const franka::RobotState& robot_state,
                            franka::Duration time_step) {
    if (commandHasNaN(command)) {
      std::string error_message = "FrankaCombinableHW: Got NaN value in command!";
      ROS_FATAL("%s", error_message.c_str());
      throw std::invalid_argument(error_message);
    }
    checkJointLimits();
    {
      std::lock_guard<std::mutex> state_lock(libfranka_state_mutex_);
      robot_state_libfranka_ = robot_state;
    }

    std::lock_guard<std::mutex> command_lock(libfranka_cmd_mutex_);
    T current_cmd = command;
    if (has_error_ || !controller_active_) {
      return franka::MotionFinished(current_cmd);
    }
    return current_cmd;
  }

  void publishErrorState(bool error);

  void setupServicesAndActionServers(ros::NodeHandle& node_handle);

  void initRobot() override;

  bool setRunFunction(const ControlMode& requested_control_mode,
                      bool limit_rate,
                      double cutoff_frequency,
                      franka::ControllerMode internal_controller) override;

  void controlLoop();

  std::unique_ptr<std::thread> control_loop_thread_;
  ServiceContainer services_;
  std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>
      recovery_action_server_;
  std::atomic_bool has_error_{false};
  ros::Publisher has_error_pub_;
  std::atomic_bool error_recovered_{false};
  std::atomic_bool controller_needs_reset_{false};
};

}  // namespace franka_hw