ThirdRobotInterface.h

Go to the documentation of this file.

#ifndef THIRD_ROBOT_INTERFACE_H_
#define THIRD_ROBOT_INTERFACE_H_

// For ROS
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"                // cmd_vel

// iMCs01
#include "imcs01_driver/driver/urbtc.h"
#include "imcs01_driver/driver/urobotc.h"

// For std
#include <string>
#include <mutex>

// For old c
#include <termios.h>

enum
{
  FRONT,
  REAR
};

enum
{
  ROBOT_STASIS_FORWARD,
  ROBOT_STASIS_FORWARD_STOP,
  ROBOT_STASIS_BACK,
  ROBOT_STASIS_BACK_STOP,
  ROBOT_STASIS_OTHERWISE
};

enum
{
  FORWARD_MODE,
  FORWARD_STOP_MODE,
  BACK_MODE,
  BACK_STOP_MODE,
  STOP_MODE
};


template<typename N, typename M>
inline double MIN(const N& a, const M& b)
{
  return a < b ? a : b;
}

template<typename N, typename M>
inline double MAX(const N& a, const M& b)
{
  return a > b ? a : b;
}

template<typename T>
inline double NORMALIZE(const T& z)
{
  return atan2(sin(z), cos(z));
}

int plus_or_minus(double value);

namespace cirkit
{

class ThirdRobotInterface
{
public:
  ThirdRobotInterface(const std::string& new_serial_port_imcs01, int new_baudrate_imcs01);
  ~ThirdRobotInterface();
  virtual int openSerialPort();
  virtual int setSerialPort();
  virtual int closeSerialPort();
  virtual void setParams(double pulse_rate, double geer_rate, double wheel_diameter_right, double wheel_diameter_left, double tred_width);
  virtual geometry_msgs::Twist drive(double linear_speed, double angular_speed);
  virtual geometry_msgs::Twist driveDirect(double front_angular, double rear_speed);// front_angular in [deg]
  virtual int getEncoderPacket();
  virtual void calculateOdometry();
  virtual void resetOdometry();
  virtual void setOdometry(double new_x, double new_y, double new_yaw);
  virtual void writeCmd(ccmd cmd);

  double odometry_x_;
  double odometry_y_;
  double odometry_yaw_;
  double steer_angle;
  int stasis_;

protected:

  int parseEncoderPackets();
  int parseFrontEncoderCounts();
  int parseRearEncoderCounts();
  geometry_msgs::Twist fixFrontAngle(double angular_diff);

  struct uin cmd_uin;
  //struct uout cmd_uout;
  struct ccmd cmd_ccmd;
  std::string imcs01_port_name;
  int fd_imcs01;
  int baudrate_imcs01;
  termios oldtio_imcs01;
  termios newtio_imcs01;
  int delta_rear_encoder_counts[2] {0, 0};
  int last_rear_encoder_counts[2] {0, 0};
  double last_rear_encoder_time;
  double delta_rear_encoder_time;
  double delta_dist[2] {0, 0};
  double last_delta_dist[2] {0, 0};
  double last_odometry_yaw = 0.0;
  double PulseRate = 40.0;
  double GeerRate = 33.0;
  double WheelDiameter[2] {0.2705, 0.275};
  double TredWidth = 0.595;
  double linear_velocity;
  int runmode;
  std::mutex communication_mutex_;
};
}
#endif // THIRD_ROBOT_INTERFACE_H_