modbridge_node.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <modelica_bridge/ModComm.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <stdio.h>
#include <unistd.h>
#include <netinet/in.h>
#include <signal.h>
#include <functional>

#include <stdlib.h>
#include <string.h>

#define MAX_BUF 1024 // define maximum length of character buffer
#define MAX_ARRAY 256 // define maximum size of internal buffer arrays

class ModBridge
{
    public:
        ModBridge();
        void spin();

    private:
        void controllerCallback(const modelica_bridge::ModComm::ConstPtr& inVal);
        
        void readFromBuffer();
        
        void writeToBuffer();

        void error(const char *msg);

        ros::NodeHandle nh_; 
        ros::Subscriber sub_; 
        ros::Publisher pub_; 

        int socket_fd_; 
        int new_socket_fd_; 
        int port_num_; 
        struct sockaddr_in serv_addr; 
        socklen_t client_len_; 
        struct sockaddr_in client_address_; 

        char buffer_[MAX_BUF]; 
        double socket_buffer_[MAX_ARRAY]; 
        double ros_buffer_[MAX_ARRAY]; 
        int update_rate_; 

        int error_check_; 
};

ModBridge::ModBridge()
{
    sub_ = nh_.subscribe("control_values", 1, &ModBridge::controllerCallback, this);
    pub_ = nh_.advertise<modelica_bridge::ModComm>("model_values", 1);

    ros::NodeHandle nh_param("~");
    nh_param.param<int>("port_num", port_num_, 9091);
    nh_param.param<int>("update_rate", update_rate_, 20);

    socket_fd_ = socket(AF_INET, SOCK_STREAM, 0);
    if (socket_fd_ < 0) {
        error("ERROR opening socket");
    }

    bzero((char *) &serv_addr, sizeof(serv_addr));

    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = INADDR_ANY;
    serv_addr.sin_port = htons(port_num_);
    if (bind(socket_fd_, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
        error("ERROR on binding");
    }
}

void ModBridge::spin() 
{
    ros::Rate loop(update_rate_);

    while(ros::ok())
    {
        modelica_bridge::ModComm outVal;

        listen(socket_fd_,5);
        client_len_ = sizeof(client_address_);

        new_socket_fd_ = accept(socket_fd_, (struct sockaddr *) &client_address_, &client_len_); 
        if (new_socket_fd_ < 0) {
            error("ERROR on accept");
        }

        error_check_ = read(new_socket_fd_, buffer_, MAX_BUF-1); 
        if (error_check_ < 0) {
            error("ERROR reading from socket");
        }

        readFromBuffer();
        writeToBuffer();

        error_check_ = write(new_socket_fd_, buffer_, strlen(buffer_));
        if (error_check_ < 0) {
            error("ERROR writing to socket");
        }

        close(new_socket_fd_);

        outVal.data.clear();
        outVal.size = ros_buffer_[0];
        for (int rosI = 1; rosI <= outVal.size; rosI ++) {
            outVal.data.push_back(ros_buffer_[rosI]);
        }
        pub_.publish(outVal);

        ros::spinOnce();
        loop.sleep();
    }
    close(socket_fd_);
}

void ModBridge::controllerCallback(const modelica_bridge::ModComm::ConstPtr& inVal)
{
    int i;
    for(i = 0; i < inVal->size; i++) {
        socket_buffer_[i] = inVal->data[i];
    }
}

void ModBridge::readFromBuffer()
{
    int j, i = 0;
    char *token[80];
    const char * delim = ",";

    token[0] = strtok(buffer_, delim);
    while (token[i] != NULL) {
        i++;
        token[i] = strtok(NULL, delim);
    }

    for (j=0; j<=i-1; j++) {
        ros_buffer_[j] = atof(token[j]);
    }
}

void ModBridge::writeToBuffer()
{
    buffer_[0] = '\0';
        char s1[100];
        for (int i = 0; i< sizeof(*socket_buffer_); i++)
        {
                sprintf(s1,"%f,",socket_buffer_[i]);
                strcat(buffer_,s1);
        }   
}

void ModBridge::error(const char *msg)
{
    perror(msg);
    exit(1);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "modros_node");

    ModBridge modbridge;
    
    modbridge.spin();
    
    return 0;
}