SVHDiagnostics.h

Go to the documentation of this file.

//
// © Copyright 2022 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
// © Copyright 2022 FZI Forschungszentrum Informatik, Karlsruhe, Germany
//
// This file is part of the Schunk SVH Driver.
//
// The Schunk SVH Driver is free software: you can redistribute it and/or
// modify it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or (at your
// option) any later version.
//
// The Schunk SVH Driver is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
// Public License for more details.
//
// You should have received a copy of the GNU General Public License along with
// the Schunk SVH Driver. If not, see <https://www.gnu.org/licenses/>.
//
 
//----------------------------------------------------------------------
//----------------------------------------------------------------------
 
#ifndef S5FH_DIAGNOSTICS_H
#define S5FH_DIAGNOSTICS_H
 
// ROS includes.
#include <actionlib/server/simple_action_server.h>
#include <ros/ros.h>
 
#include <functional>
#include <memory>
#include <stdint.h>
#include <string>
 
// Date
#include <ctime>
#include <fstream>
#include <iostream>
 
// Messages
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/MultiArrayLayout.h"
#include <sensor_msgs/JointState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>
 
// FZI includes
#include "schunk_svh_msgs/SVHDiagnosticsAction.h"
#include "schunk_svh_msgs/SVHDiagnosticsToProtocol.h"
 
// Driver Specific things
#include <schunk_svh_library/SVHFirmwareInfo.h>
#include <schunk_svh_library/control/SVHCurrentSettings.h>
#include <schunk_svh_library/control/SVHFingerManager.h>
#include <schunk_svh_library/control/SVHPositionSettings.h>
 
class SVHDiagnostics
{
public:
  SVHDiagnostics(const ros::NodeHandle& nh,
                 std::shared_ptr<driver_svh::SVHFingerManager>& finger_manager,
                 std::function<void(bool)> enable_ros_contol_loop,
                 std::function<void(uint16_t, uint16_t)> init_controller_parameters,
                 std::string name);
 
  ~SVHDiagnostics();
 
  void basicTestCallback(const schunk_svh_msgs::SVHDiagnosticsGoalConstPtr& goal);
 
  void testCallback(const std_msgs::String&);
 
 
private:
  ros::NodeHandle m_priv_nh;
 
  void resetDiagnosticStatus();
 
  void debugOuput();
 
  schunk_svh_msgs::SVHDiagnosticsResult evaluateBasicTest();
 
  void qualityProtocolWritting();
 
  void initializeProtocolMessage();
 
  void initTest();
 
  std::shared_ptr<driver_svh::SVHFingerManager> m_finger_manager;
 
  std::function<void(bool)> m_enable_ros_contol_loop;
 
  std::function<void(uint16_t, uint16_t)> m_init_controller_parameters;
 
  std::string m_serial_device_name;
 
  int m_connect_retry_count;
 
  std::string name_prefix;
 
  sensor_msgs::JointState channel_pos_;
 
  std_msgs::Float64MultiArray channel_currents;
 
  std::vector<bool> reset_success;
 
  std::string m_serial_no;
 
  std::string m_assembly_of;
 
  bool m_execution_R;
  bool m_execution_L;
 
  bool m_communication;
 
  bool m_basic_test_running;
 
  ros::Publisher m_pub_protocol_variables;
 
  schunk_svh_msgs::SVHDiagnosticsToProtocol m_msg_protocol_variable;
 
  actionlib::SimpleActionServer<schunk_svh_msgs::SVHDiagnosticsAction> m_diagnostics_action_server;
 
  // Action Feedback
  schunk_svh_msgs::SVHDiagnosticsFeedback m_action_feedback;
 
  schunk_svh_msgs::SVHDiagnosticsResult m_action_result;
 
  // Action server name
  std::string m_action_name;
 
  enum diag_information
  {
    zero_defect,
    board_one,
    board_two,
    board_one_encoder,
    board_one_motor,
    board_two_encoder,
    board_two_motor,
    encoder_motor,
    encoder,
    motor,
    current_range,
    position_range,
    diag_information_dimension
  };
};
 
#endif // S5FH_CONTROLLER_H