File content as of revision 33:9877ca32e43c:

// HLComms.cpp

#include "HLComms.h"

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();
}

/*HLComms::~HLComms(void) { // Destructor
    delete *_text_pub;
    delete *_sensor_pub;
    delete *_durations_pub;
}*/

// ROS

void HLComms::ros_main(void) {
    _nh.getHardware()->setBaud(BAUD_RATE);
    _nh.initNode();
    wait_ms(1000);
    
    //ros::Subscriber<std_msgs::Int16MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
    ros::Subscriber<std_msgs::Float32MultiArray,HLComms> demands_sub("ML_Demands", &HLComms::receive_demands, this);
    _nh.subscribe(demands_sub);
    
    _text_pub = new ros::Publisher("ML_TextOut", &_text_msg);
    _nh.advertise(*_text_pub);
    
    _sensor_msg.data_length = N_CHANNELS*2;
    _sensor_pub = new ros::Publisher("ML_Sensors", &_sensor_msg);
    _nh.advertise(*_sensor_pub);
    
    _durations_msg.data_length = 3;
    _durations_pub = new ros::Publisher("ML_Durations", &_durations_msg);
    _nh.advertise(*_durations_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();
    }
}

void HLComms::releaseSemSpin(void) {
    _semSpin.release();
}

// INPUT

/*void HLComms::receive_demands(const std_msgs::Int16MultiArray &demands_array) {
    struct demands_struct _protected_input;
    recv_mutex.lock();
    for(int i=0; i<N_CHANNELS; i++) {
        _protected_input.psi_mm[i] = ((MAX_ACTUATOR_LIMIT_MM+1.0)*((double)demands_array.data[i]/65535)-1.0);
        if( _protected_input.psi_mm[i]<-0.5 ) {
            _protected_input.psi_mm[i] = -1.0;
        } else {
            _protected_input.psi_mm[i] = min( max(_protected_input.psi_mm[i],0.0) , (double)MAX_ACTUATOR_LIMIT_MM );
        }
    }
    //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    //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.speed_mmps = MAX_SPEED_MMPS*((double)demands_array.data[9]/65535);
    _protected_input.speed_mmps = min( max(_protected_input.speed_mmps,0.0) , (double)MAX_SPEED_MMPS );
    recv_mutex.unlock();
    _input = _protected_input;
    //char array[20];
    //sprintf(array, "Speed: %f", _input.speed_mmps);
    //send_text_message(array);
    newData.release();
}*/

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; i++) {
        _protected_input.psi_mm[i] = (double)demands_array.data[i];
        //_protected_input.psi_mm[i] = min( max(_protected_input.psi_mm[i],0.0) , (double)MAX_ACTUATOR_LIMIT_MM );
    }
    // demands_array.data[8] ignored
    _protected_input.speeds_mmps[0] = (double)demands_array.data[9];
    _protected_input.speeds_mmps[0] = min( max(_protected_input.speeds_mmps[0],0.0) , (double)MAX_SPEED_MMPS );
    _protected_input.speeds_mmps[1] = (double)demands_array.data[10];
    _protected_input.speeds_mmps[1] = min( max(_protected_input.speeds_mmps[1],0.0) , (double)MAX_SPEED_MMPS );
    _protected_input.speeds_mmps[2] = (double)demands_array.data[11];
    _protected_input.speeds_mmps[2] = min( max(_protected_input.speeds_mmps[2],0.0) , (double)MAX_SPEED_MMPS );
    _protected_input.utitilies_bool[0] = fabs(demands_array.data[12]) > numeric_limits<float>::epsilon();
    _protected_input.utitilies_bool[1] = fabs(demands_array.data[13]) > numeric_limits<float>::epsilon();
    _protected_input.utitilies_bool[2] = fabs(demands_array.data[14]) > numeric_limits<float>::epsilon();
    _protected_input.utitilies_bool[3] = fabs(demands_array.data[15]) > numeric_limits<float>::epsilon();
    
    recv_mutex.unlock();
    _input = _protected_input;
    newData.release();
}

demands_struct HLComms::get_demands(void) {
    return _input;
}

// OUTPUT

void HLComms::send_text_message(char text[]) {
    _text_msg.data = text;
    _text_pub->publish(&_text_msg);
}

void HLComms::send_durations_message(double dblTimes[]) {
    for(short int i=0; i<3; i++) {
        _durations_msg.data[i] = dblTimes[i];
    }
    _durations_pub->publish(&_durations_msg);
}

void HLComms::send_sensor_message(unsigned int positions[], unsigned int 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);
}