franka_combined_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 <combined_robot_hw/combined_robot_hw.h>
#include <franka_hw/franka_combinable_hw.h>
#include <franka_msgs/ErrorRecoveryAction.h>

#include <actionlib/server/simple_action_server.h>
#include <ros/node_handle.h>
#include <ros/time.h>

#include <memory>

namespace franka_hw {

class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW {
 public:
  FrankaCombinedHW();

  ~FrankaCombinedHW() override = default;  // NOLINT (clang-analyzer-optin.cplusplus.VirtualCall)

  bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;

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

  bool controllerNeedsReset();

  void connect();

  bool disconnect();

  bool hasError();

 protected:
  std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>
      combined_recovery_action_server_;
  ros::ServiceServer connect_server_;
  ros::ServiceServer disconnect_server_;

 private:
  void handleError();
  void triggerError();
  bool is_recovering_{false};
};

}  // namespace franka_hw