Diff: HLComms.cpp

Revision:

28:8e0c502c1a50

Parent:

19:e5acb2183d4e

Child:

29:10a5cf37a875

--- a/HLComms.cpp	Tue Jan 29 14:58:45 2019 +0000
+++ b/HLComms.cpp	Wed Feb 06 16:10:18 2019 +0000
@@ -1,156 +1,102 @@
 // HLComms.cpp
 
-// Error codes: https://github.com/ARMmbed/mbed-os/blob/master/features/netsocket/nsapi_types.h
-// http://man7.org/linux/man-pages/man3/errno.3.html
-
 #include "HLComms.h"
 
-HLComms::HLComms(short int port) { // Constructor
-    _port = port;
+HLComms::HLComms(unsigned short int freq_hz) : // Constructor
+    newData(1),
+    _freq_hz(freq_hz),
+    _threadROS(osPriorityBelowNormal),
+    _rosReady(1),
+    _semSpin(1)
+{
+    _threadROS.start(callback(this, &HLComms::ros_main));// Start ROS thread
+    _rosReady.wait();
 }
 
-int HLComms::setup_server(void) {
-    int error_code;
-    if(IS_PRINT_OUTPUT) printf("Starting TCP server...\n\r");
-    if( !IS_DHCP ) {
-        const char* ip = "192.168.1.5"; //"192.168.1.2"; //"10.101.81.135"; //"10.101.79.125"; // Get first 3 parts from "ifconfig" on PC
-        const char* mask = "255.255.255.0"; // Get from "ifconfig" on PC
-        const char* gateway =  "192.168.1.1"; //"10.101.81.1"; //"10.101.79.1"; // Get from "route -n" on PC
-        error_code = interfaces.eth.set_network(ip, mask, gateway); // Dont use DHCP
-        if( error_code < 0 ) {
-            if(IS_PRINT_OUTPUT) printf("Error %i. Could not network interface to use a static IP address. "
-                "Perhaps the network is already connected?\n\r", error_code);
-            return -1;
-        }
-    }
-    error_code = interfaces.eth.connect();
-    if( error_code < 0 ) {
-        if(IS_PRINT_OUTPUT) printf("Error %i. Could not start the Ethernet interface.\n\r", error_code);
-        return -1;
-    }
-    const char* server_ip = interfaces.eth.get_ip_address();
-    if( server_ip == NULL ) {
-       error_code = -1;
-        if(IS_PRINT_OUTPUT) printf("Error %i. Ethernet interface is not yet connected.\n\r", error_code);
-        return -1;
-    }
-    if(IS_PRINT_OUTPUT) printf("The target IP address is '%s'\n\r", server_ip);
-    error_code = interfaces.srv.open(&interfaces.eth); // Open the server on ethernet stack
-    if( error_code < 0 ) {
-        if(IS_PRINT_OUTPUT) printf("Error %i. Could not open server socket.\n\r", error_code);
-        return -1;
+/*HLComms::~HLComms(void) { // Destructor
+    delete *_sensor_pub;
+    delete *_duration_pub;
+}*/
+
+// ROS
+
+void HLComms::ros_main(void) {
+    //printf("HELLO\r\n");
+    _nh.getHardware()->setBaud(BAUD_RATE);
+    _nh.initNode();
+    wait_ms(1000);
+    
+    ros::Subscriber<std_msgs::Float32MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
+    _nh.subscribe(demands_sub);
+    
+    _sensor_msg.data_length = N_CHANNELS*2;
+    _sensor_pub = new ros::Publisher("ML_Sensors", &_sensor_msg);
+    _nh.advertise(*_sensor_pub);
+    
+    _duration_pub = new ros::Publisher("ML_Duration", &_duration_msg);
+    _nh.advertise(*_duration_pub);
+    
+    wait_ms(1000);
+    _rosReady.release();
+    
+    Ticker spinTicker;
+    spinTicker.attach(callback(this,&HLComms::releaseSemSpin), 1/(float)_freq_hz); // Set up HL comms to recur at fixed intervals
+    
+    while (1) {
+        _semSpin.wait();
+        _nh.spinOnce();
     }
-    error_code = interfaces.srv.bind(interfaces.eth.get_ip_address(), _port); // Bind the HTTP port (TCP 80) to the server
-    if( error_code < 0 ) {
-        if(IS_PRINT_OUTPUT) printf("Error %i. Could not bind server socket to port '%d'.\n\r", error_code, _port);
-        return -1;
-    }
-    error_code = interfaces.srv.listen(1); // Listen for 1 simultaneous connection
-    if( error_code < 0 ) {
-        if(IS_PRINT_OUTPUT) printf("Error %i. Could not put server socket into listening mode.\n\r", error_code);
-        return -1;
-    }
-    return 0;
-}
-
-
-int HLComms::accept_connection(void) {
-    if(IS_PRINT_OUTPUT) printf("Waiting to accept a connection on the TCP socket.\n\r");
-    int error_code;
-    error_code = interfaces.srv.accept(&interfaces.clt_sock, &interfaces.clt_addr); // Blocks until data is sent
-    if( error_code < 0 ) {
-        if(IS_PRINT_OUTPUT) printf("Error %i. Could not create a network socket using the specified socket instance. "
-            "Perhaps the socket is set to non-blocking or timed out?\n\r", error_code);
-        return -1;
-    }
-    if(IS_PRINT_OUTPUT) printf("Accepted %s:%d\n\r", interfaces.clt_addr.get_ip_address(), interfaces.clt_addr.get_port());
-    return 0;
-}
-
-
-void HLComms::close_server(void) {
-    if(IS_PRINT_OUTPUT) printf("Closing server...\n\r");
-    interfaces.clt_sock.close();
-    interfaces.srv.close();
-    interfaces.eth.disconnect();
-    return;
 }
 
-
-int HLComms::receive_message(void) {
-    memset(recv_buffer, 0, sizeof(recv_buffer));
-    int error_code = interfaces.clt_sock.recv(recv_buffer, sizeof(recv_buffer)-1); // Blocks until data is received
-    if(IS_PRINT_OUTPUT) printf("Message received.\r\n");
-    return error_code;
+void HLComms::releaseSemSpin(void) {
+    _semSpin.release();
 }
 
+// INPUT
 
-msg_format HLComms::process_message(void) { //HLComms::msg_format
-    unsigned char * msg = recv_buffer;
-    // Break message string into chunks at each comma
-    vector<string> tokens;
-    char * delim = ",";
-    char * token = strtok((char *)msg, delim);
-    while(token != NULL) {
-        tokens.push_back(string(token));
-        //if(IS_PRINT_OUTPUT) printf("%s\n\r",token);
-        token = strtok(NULL, delim);
+void HLComms::receive_demands(const std_msgs::Float32MultiArray &demands_array) {
+    struct demands_struct _protected_input;
+    recv_mutex.lock();
+    /*for(int i=0; i<N_CHANNELS-1; i++) {
+        _protected_input.psi_mm[i] = demands_array.data[i];
     }
-    // Cast into doubles and assign to variables
-    struct msg_format input;
-    stringstream(tokens[0]) >> input.psi[0][0];
-    stringstream(tokens[1]) >> input.psi[0][1];
-    stringstream(tokens[2]) >> input.psi[0][2];
-    stringstream(tokens[3]) >> input.psi[1][0];
-    stringstream(tokens[4]) >> input.psi[1][1];
-    stringstream(tokens[5]) >> input.psi[1][2];
-    stringstream(tokens[6]) >> input.psi[2][0];
-    stringstream(tokens[7]) >> input.psi[2][1];
-    stringstream(tokens[8]) >> input.psi[2][2];
-    stringstream(tokens[9]) >> input.speed;
-    
-    /*if(input.psi[0][0] < 0.0) input.psi[0][0] = 0.0;
-    if(input.psi[0][1] < 0.0) input.psi[0][1] = 0.0;
-    if(input.psi[0][2] < 0.0) input.psi[0][2] = 0.0;
-    if(input.psi[1][0] < 0.0) input.psi[1][0] = 0.0;
-    if(input.psi[1][1] < 0.0) input.psi[1][1] = 0.0;
-    if(input.psi[1][2] < 0.0) input.psi[1][2] = 0.0;
-    if(input.psi[2][0] < 0.0) input.psi[2][0] = 0.0;
-    if(input.psi[2][1] < 0.0) input.psi[2][1] = 0.0;
-    if(input.psi[2][2] < 0.0) input.psi[2][2] = 0.0;
-    if(input.duration < 0.0) input.duration = 0.0;*/
-    /*if( input.psi[2][0] > 0.14 ) {
-        if(IS_PRINT_OUTPUT) printf("Hi\n\r");
-    }*/
-    //throw std::invalid_argument( "received negative value" );
-    //return 0;
-    return input;
-} // End of consume_message()
-
-int HLComms::send_duration_message(double *dblTime) {
-    send_mutex.lock();
-    memset(send_buffer, 0, sizeof(send_buffer));
-    _snprintf(send_buffer,sizeof(send_buffer),"0,%f",*dblTime);
-    int error_code = send_message();
-    send_mutex.unlock();
-    return error_code;
+    short int demands_array_length = sizeof(demands_array.data)/sizeof(demands_array.data[0]);
+    _protected_input.speed_mmps = demands_array.data[demands_array_length-1];*/
+    _protected_input.psi_mm[0] = demands_array.data[0];
+    _protected_input.psi_mm[1] = demands_array.data[1];
+    _protected_input.psi_mm[2] = demands_array.data[2];
+    _protected_input.psi_mm[3] = demands_array.data[3];
+    _protected_input.psi_mm[4] = demands_array.data[4];
+    _protected_input.psi_mm[5] = demands_array.data[5];
+    _protected_input.psi_mm[6] = demands_array.data[6];
+    _protected_input.psi_mm[7] = demands_array.data[7];
+    _protected_input.psi_mm[8] = demands_array.data[8];
+    _protected_input.speed_mmps = demands_array.data[9];
+    recv_mutex.unlock();
+    _input = _protected_input;
+    newData.release();
 }
 
-int HLComms::send_sensor_message(double positions[], double pressures[]) {
-    send_mutex.lock();
-    memset(send_buffer, 0, sizeof(send_buffer));
-    _snprintf(send_buffer,sizeof(send_buffer),"1,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f",
-        positions[0],positions[1],positions[2],positions[3],positions[4],positions[5],positions[6],positions[7],
-        pressures[0],pressures[1],pressures[2],pressures[3],pressures[4],pressures[5],pressures[6],pressures[7]);
-    int error_code = send_message();
-    send_mutex.unlock();
-    return error_code;
+demands_struct HLComms::get_demands(void) {
+    return _input;
+}
+
+// OUTPUT
+
+void HLComms::send_duration_message(double dblTime) {
+    _duration_msg.data = dblTime;
+    _duration_pub->publish(&_duration_msg);
 }
 
-int HLComms::send_message(void) {
-    if(IS_PRINT_OUTPUT) printf("Sending message...\r\n");
-    char * msg = send_buffer;
-    int error_code = interfaces.clt_sock.send(msg, strlen(msg));
-    if(IS_PRINT_OUTPUT) printf("Message sent.\r\n");
-    return error_code;
+void HLComms::send_sensor_message(double positions[], double pressures[]) {
+    short int i_channel;
+    for(short int i=0; i<N_CHANNELS*2; i++) {
+        i_channel = i%N_CHANNELS;
+        if(i<N_CHANNELS) {
+            _sensor_msg.data[i] = positions[i_channel];
+        } else {
+            _sensor_msg.data[i] = pressures[i_channel];
+        }
+    }
+    _sensor_pub->publish(&_sensor_msg);
 }
\ No newline at end of file