roch_hardware.h

Go to the documentation of this file.

#ifndef roch_BASE_roch_HARDWARE_H
#define roch_BASE_roch_HARDWARE_H

#include "roch_base/roch_diagnostics.h"
#include "diagnostic_updater/diagnostic_updater.h"
#include "hardware_interface/joint_state_interface.h"
#include "hardware_interface/joint_command_interface.h"
#include "hardware_interface/imu_sensor_interface.h"
#include "hardware_interface/robot_hw.h"
#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "roch_msgs/RochStatus.h"
#include "roch_msgs/CliffEvent.h"
#include "roch_msgs/UltEvent.h"
#include "roch_msgs/PSDEvent.h"
#include "roch_msgs/SensorState.h"
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>
#include <string>

#include <angles/angles.h>
#include <std_msgs/String.h>
#include "roch_base/core/Message.h"
#include "roch_base/core/serial.h"
namespace roch_base
{

  class rochHardware :
    public hardware_interface::RobotHW
  {
    
  public:
    rochHardware(ros::NodeHandle nh, ros::NodeHandle private_nh, double target_control_freq);

    void updateJointsFromHardware();

    void writeCommandsToHardware();

    void updateDiagnostics();

    void reportLoopDuration(const ros::Duration &duration);
    
    void writeOverallSpeedCommandsToHardware();
    
    void getRangefinderData();
    
    void getPlatformName();
    
    void getPlatAccData();
    
    void getDifferentControlConstantData();
    
    void showRawData();
    
    void publishRawData();

  public:
    
    hardware_interface::ImuSensorHandle::Data imuMsgData;
    double orientation[4];
    double orientation_covariance [9];
    double angular_velocity_covariance[9] ;
    double linear_acceleration_covariance[9] ;
    double angular_velocity[3]   ;
    double linear_acceleration[3];
  private:

    void initializeDiagnostics();

    void resetTravelOffset();

    void registerControlInterfaces();

    double linearToAngular(const double &travel) const;

    double angularToLinear(const double &angle) const;

    void limitDifferentialSpeed(double &travel_speed_left, double &travel_speed_right);
    
    void publishCliffEvent(const double &left, const double &right);
    
    void publishUltEvent(const double &left, const double &center, const double &right);
    
    void publishPSDEvent(const double &left, const double &center, const double &right);
    
    void publishWheelEvent(const float &leftOffset, const float &rightOffset);
    
    void publishSensorState();

    ros::NodeHandle nh_, private_nh_;

    // ROS Control interfaces
    hardware_interface::JointStateInterface joint_state_interface_;
    hardware_interface::VelocityJointInterface velocity_joint_interface_;
    hardware_interface::ImuSensorInterface imu_sensor_interface_;

    // Diagnostics
    ros::Publisher diagnostic_publisher_;
    ros::Publisher raw_data_command_publisher_;
    ros::Publisher cliff_event_publisher_,ult_event_publisher_,psd_event_publisher_;
    ros::Publisher sensor_state_publisher_;
    roch_msgs::RochStatus roch_status_msg_;
    diagnostic_updater::Updater diagnostic_updater_;
    rochHardwareDiagnosticTask<sawyer::DataSystemStatus> system_status_task_;
    rochHardwareDiagnosticTask<sawyer::DataPowerSystem> power_status_task_;
    rochHardwareDiagnosticTask<sawyer::DataSafetySystemStatus> safety_status_task_;
    rochSoftwareDiagnosticTask software_status_task_;
  
      // ROS Parameters
    double wheel_diameter_, max_accel_, max_speed_;

    double polling_timeout_;
    
    double cliff_height_; //the height of cliff can scan (meter)
    
    
    double ult_length_; //the lengthh of ult can detection (meter)
    
    double PSD_length_; //the lengthh of PSD can detection (meter)
    std::string gyro_link_frame_;
    std::vector<double> cliffbottom;
    std::vector<double> ultbottom;
    std::vector<double> psdbottom;
    struct Joint
    {
      double position;
      double position_offset;
      double velocity;
      double effort;
      double velocity_command;

      Joint() :
        position(0), velocity(0), effort(0), velocity_command(0)
      { }
    } joints_[2];
    
    struct ThreeAxisGyro{
      double X;
      double X_Offset;
      double Y;
      double Y_Offset;
      double Z;
      double Z_Offset;
      
      ThreeAxisGyro() :
        X(0), X_Offset(0), Y(0), Y_Offset(0), Z(0), Z_Offset(0)
      { }
    }gyroData;
    
    struct SixAxisGyro{
      double angle;
      double angle_rate;   // rad/s
      double angle_rate_offset;
      double angle_offset;
      ThreeAxisGyro acc;
     
      SixAxisGyro() :
        angle(0), angle_rate(0), angle_rate_offset(0), angle_offset(0)
      { }
    }sixGyro;
    
    struct CliffEvent{
      enum State {
          Floor,
          Cliff
        } state;
      enum Sensor {
        Left,
        Right
      } sensor;
      double leftbottom;
      double rightbottom;
    };
    
    struct UltEvent{
      enum State {
          Normal,
          Near
        } state;
      enum Sensor {
        Left,
        Center,
        Right
      } sensor;
      double leftbottom;
      double centerbottom;
      double rightbottom;
    };
    
    struct PSDEvent{
      enum State {
          Normal,
          Near
        } state;
      enum Sensor {
        Left,
        Center,
        Right
      } sensor;
      double leftbottom;
      double centerbottom;
      double rightbottom;
    };
    
    CliffEvent leftcliffevent,rightcliffevent;
    
    UltEvent leftultevent, centerultevent, rightultevent;
    
    PSDEvent leftpsdevent, centerpsdevent, rightpsdevent;
};
    
};

 // namespace roch_base
#endif  // ROCH_BASE_ROCH_HARDWARE_H