dgm_teststand.hpp

#ifndef DGM_TESTSTAND_HH
#define DGM_TESTSTAND_HH

#include <dynamic_graph_manager/dynamic_graph_manager.hh>
#include <blmc_robots/teststand.hpp>
#include "dg_blmc_robots/JointCalibration.h"
#include "yaml_cpp_catkin/yaml_cpp_fwd.hpp"

namespace dg_blmc_robots
{

  class DGMTeststand : public dynamic_graph::DynamicGraphManager
  {
  public:
    DGMTeststand();

    ~DGMTeststand();

    void initialize_hardware_communication_process();

    void get_sensors_to_map(dynamic_graph::VectorDGMap& map);

    void set_motor_controls_from_map(const dynamic_graph::VectorDGMap& map);

    virtual bool is_in_safety_mode();

    bool calibrate_joint_position_callback(
      dg_blmc_robots::JointCalibration::Request& req,
      dg_blmc_robots::JointCalibration::Response& res);

  private:
    void calibrate_joint_position(const Eigen::Vector2d& zero_to_index_angle);

    blmc_robots::Teststand teststand_;

    Eigen::Vector2d ctrl_joint_torques_;

    Eigen::Vector2d zero_to_index_angle_from_file_;

    bool was_in_safety_mode_;
  };


} // namespace dg_blmc_robots

#endif // DGM_TEST_BENCH_8_MOTORS_HH