tcp_pump_client.cpp 4.16 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133
#include "tcp_pump_client.h"

using namespace CLA;

tcp_pump_client::tcp_pump_client() : m_logger() {
  /* we can only talk to rosbrige server using json strings; here we want to
   * 'subscribe' to 'chatter topic' publishing messages of type 'std_msgs/String'
   */
    json_message = "{\"op\":\"subscribe\", \"topic\":\"/chatter\", \"type\":\"std_msgs/Float64\"}";
    m_host = "localhost"; /* rosbridge server runs on localhost */
    port = 9090; /* rosbridge server uses this port number */
    SOCKET_TIMEOUT_SEC = 1;
    SOCKET_TIMEOUT_uSEC = 500;
    m_bpdrug_last_sent_rate = 0;
    m_last_sent_rate = 0;
    m_new_rate = 0;
    m_current_rate = 0;
}

tcp_pump_client::tcp_pump_client(const string& topic, const string& type): m_topic(move(topic)),
m_type(move(type)), m_logger() {
    json_message = "{\"op\":\"subscribe\", \"topic\": \""+m_topic+"\", \"type\": \""+m_type+"\"}";
    m_host = "localhost"; /* rosbridge server runs on localhost */
    port = 9090; /* rosbridge server uses this port number */
    SOCKET_TIMEOUT_SEC = 1;
    SOCKET_TIMEOUT_uSEC = 500;
    m_bpdrug_last_sent_rate = 0;
    m_last_sent_rate = 0;
    m_new_rate = 0;
    m_current_rate = 0;
}

void tcp_pump_client::initialize() {
  /* create a UDP socket */
  fd = socket(AF_INET, SOCK_STREAM, 0);
  if (fd < 0) {
    fprintf(stderr, "can not create socket\n");
    m_logger.error("Failed to create socket");
    exit(EXIT_FAILURE);
  }
  /* look up the address of the server given its name */
  hp = gethostbyname(m_host);
  if(!hp) {
    fprintf(stderr, "could not obtain address %s\n", m_host);
    exit(EXIT_FAILURE);
  }
  /* initialize sockaddr_in servaddr*/
  memset((char*)&servaddr, 0, sizeof(servaddr));
  /* connect to remote host (rosbridge_server in our case)*/
  servaddr.sin_family = AF_INET;
  servaddr.sin_port = htons(port); /* convert to network byte order */
  /* put host's address into th server address struct */
  memcpy((void*)&servaddr.sin_addr, hp->h_addr_list[0], hp->h_length);
  addrlen = sizeof(servaddr);
  /* initialize buffer s*/
  memset(buff, 0 , sizeof(buff));
  /*connect to server */
  if (connect(fd, (struct sockaddr*)&servaddr, addrlen) < 0) {
     cout << "Connection Failed \n";
     m_logger.error("TCP Connection Failed");
     exit(EXIT_FAILURE);

  }
  /* send json message to server */
  int n = write(fd, json_message.c_str(), strlen(json_message.c_str()));
  if (n < 0) {
      cout <<"Can not write to socket\n";
      m_logger.error("Can not write to socket");
      exit(EXIT_FAILURE);
  }
}

double tcp_pump_client::getCurrentInfusionRate(const string& substance) {
  timeout.tv_sec = SOCKET_TIMEOUT_SEC;
  timeout.tv_usec = SOCKET_TIMEOUT_uSEC;
  FD_ZERO(&set); /* initialize set */
  FD_SET(fd, &set);/* add descriptor to the read set */
  rec_value = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
  if (rec_value == -1 ) {
    cout << "socket error\n";
    exit(EXIT_FAILURE);
  }
  else if (rec_value == 0) {
    /* on timeout -> keep calling the waiting_for_new_rate function
     * to return the last sent value until topic publishes a new rate
     */
    m_current_rate = waiting_for_new_rate(substance);
  }
  else {
    m_current_rate = get_new_rate(substance);
  }
  return m_current_rate;
}

double tcp_pump_client::get_new_rate(const string& substance) {
  recvlen = read(fd, buff, BUFF_SIZE);
  if (recvlen > 0) {
    buff[recvlen]  = '\0'; /* adding EOL */
    document.Parse(buff);
    if (substance == "Norepinephrine") {
      m_new_rate = document["msg"]["data"].GetDouble();
      m_bpdrug_last_sent_rate = m_new_rate;
    }
    else if (substance == "Saline") {
      m_new_rate = document["msg"]["data"].GetDouble();
      m_saline_last_sent_rate = m_new_rate;
    }
  }
  return m_new_rate;
}

double tcp_pump_client::waiting_for_new_rate(const string& substance) {
  /* while topic is not publishing, keep
   * returning the last sent rate value
   */
  if (substance == "Norepinephrine") {
    m_last_sent_rate = m_bpdrug_last_sent_rate;
  }
  else if (substance == "Saline") {
    m_last_sent_rate = m_saline_last_sent_rate;
  }
  return m_last_sent_rate;
}

void tcp_pump_client::clear() {
  hp = nullptr;
}

tcp_pump_client::~tcp_pump_client() {
  close(fd);
  clear();
  delete hp;
}