File content as of revision 14:b972d0d0f63d:

#include "PROJ515.hpp" // Contains Libraries, Definitions & Function Prototypes

float cfDist1 = 0;     // Distance returned by cliff u_sensor1 (m)
float cfDist2 = 0;     // Distance returned by cliff u_sensor2 (m)
float cfDist3 = 0;     // Distance returned by cliff u_sensor3 (m)
float cfDist4 = 0;     // Distance returned by cliff u_sensor4 (m)

float cbDist1 = 0;     // Distance returned by curb  u_sensor1 (m)
float cbDist2 = 0;     // Distance returned by curb  u_sensor2 (m)
float cbDist3 = 0;     // Distance returned by curb  u_sensor3 (m)
float cbDist4 = 0;     // Distance returned by curb  u_sensor4 (m)
float cbDist5 = 0;     // Distance returned by curb  u_sensor5 (m)
float cbDist6 = 0;     // Distance returned by curb  u_sensor6 (m)

float wPeriod = 10;    // Wait period(ms) for ROS publisher to finish publishing

char gps_c = 0;                                    // GPS serial character
int sats_n = 0;                                    // GPS Satelite count

int main()
{
    nh.getHardware()->setBaud(ROS_Baud);    // Set Baud Rate for ROS Serial
    nh.initNode();                          // Initialise ROS Node Handler
    
    advRosPub();                            // Adverstise Ultrasound Topic
    nh.advertise(gps_odom_pub);             // Adverstise Odometry topic
    
    setupRosMsg();                          // Setup the message constant values
    gps_odom_msg.header.frame_id = frameID_g;        // Pack ROS frame ID
    gps_odom_msg.status.status = UNAGUMENTED;      // Location fix method
    gps_odom_msg.status.service = GPS_CONSTELL;    // Satelite constellation used
    gps_odom_msg.altitude  = 0;                    // Altitude (Always fixed)
    gps_odom_msg.position_covariance[ 0] = CVX;    // X pos covariance
    gps_odom_msg.position_covariance[ 1] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 2] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 3] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 4] = CVY;    // Y pos covariance
    gps_odom_msg.position_covariance[ 5] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 6] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 7] = CVO;    // Zero
    gps_odom_msg.position_covariance[ 8] = CVZ;    // Z pos covariance
    gps_odom_msg.position_covariance_type = UNNWN; // Covariance type
    gpsSer.baud(GPS_Baud);                         // Set Baud rate for GPS Serial
    
    
    while(!nh.connected()) {                // While node handler is !connected
        nh.spinOnce();                      // Attempt to connect & synchronise
    }

    while(true) {
        startSensors();                     // Start the Sensors
        recordDist();                       // Record the Distances (meters)
        checkCliff();                       // Check the Cliff Sensors Readings
        setRosDist();                       // Put distance in ROS messages
        setRosStamp();                      // Put time into ROS messages
        publishRosMsg();                    // Publish ROS messages
        
        if(gpsSer.readable()) {                    // If serial buffer has character
            gps_c = gpsSer.getc();                 // Read serial buffer and store character
            gpsModule.encode(gps_c);               // Encode character from GPS

        } else {
            leds = leds & 0b100;                   // Flash LED[0,1] bus OFF. Leave LED[2] on if its on to indicate that the signal was present
        }
        if (gpsModule.location.isValid()) {        // If GPS location is Valid
            gps_odom_msg.latitude  = gpsModule.location.lng(); // Get Longtitude
            gps_odom_msg.longitude = gpsModule.location.lat(); // Get Latitude
            nh.spinOnce(); // Reccuring connect and synchronise
            gps_odom_msg.header.stamp = nh.now();      // Get current time
            gps_odom_pub.publish(&gps_odom_msg);       // Publish the Odometry message
        }
        sats_n = gpsModule.satellites.value();     // Aquire satelite number
        if (sats_n > 0){
            leds = LEDS_ON;
        }
    }
}

/*============================================================================*/
/*                                  FUNCTIONS                                 */
/*============================================================================*/


void advRosPub()
{
    // Adverstise Ultrasound topic
    nh.advertise(ultrasound_pub_cf1);
    nh.advertise(ultrasound_pub_cf2);
    nh.advertise(ultrasound_pub_cf3);
    nh.advertise(ultrasound_pub_cf4);
    nh.advertise(ultrasound_pub_cb1);
    nh.advertise(ultrasound_pub_cb2);
    nh.advertise(ultrasound_pub_cb3);
    nh.advertise(ultrasound_pub_cb4);
    nh.advertise(ultrasound_pub_cb5);
    nh.advertise(ultrasound_pub_cb6);
}

void setupRosMsg()
{
    // Setup the message constant values

    // Frame ID
    ultrasound_msg_cf1.header.frame_id = "uss_cf1";
    ultrasound_msg_cf2.header.frame_id = "uss_cf2";
    ultrasound_msg_cf3.header.frame_id = "uss_cf3";
    ultrasound_msg_cf4.header.frame_id = "uss_cf4";
    ultrasound_msg_cb1.header.frame_id = "uss_cb1";
    ultrasound_msg_cb2.header.frame_id = "uss_cb2";
    ultrasound_msg_cb3.header.frame_id = "uss_cb3";
    ultrasound_msg_cb4.header.frame_id = "uss_cb4";
    ultrasound_msg_cb5.header.frame_id = "uss_cb5";
    ultrasound_msg_cb6.header.frame_id = "uss_cb6";

    // Radiation Type
    ultrasound_msg_cf1.radiation_type = ULTRASOUND;
    ultrasound_msg_cf2.radiation_type = ULTRASOUND;
    ultrasound_msg_cf3.radiation_type = ULTRASOUND;
    ultrasound_msg_cf4.radiation_type = ULTRASOUND;
    ultrasound_msg_cb1.radiation_type = ULTRASOUND;
    ultrasound_msg_cb2.radiation_type = ULTRASOUND;
    ultrasound_msg_cb3.radiation_type = ULTRASOUND;
    ultrasound_msg_cb4.radiation_type = ULTRASOUND;
    ultrasound_msg_cb5.radiation_type = ULTRASOUND;
    ultrasound_msg_cb6.radiation_type = ULTRASOUND;

    // Field of View
    ultrasound_msg_cf1.field_of_view = FOV;
    ultrasound_msg_cf2.field_of_view = FOV;
    ultrasound_msg_cf3.field_of_view = FOV;
    ultrasound_msg_cf4.field_of_view = FOV;
    ultrasound_msg_cb1.field_of_view = FOV;
    ultrasound_msg_cb2.field_of_view = FOV;
    ultrasound_msg_cb3.field_of_view = FOV;
    ultrasound_msg_cb4.field_of_view = FOV;
    ultrasound_msg_cb5.field_of_view = FOV;
    ultrasound_msg_cb6.field_of_view = FOV;
    
    // Minumum Range
    ultrasound_msg_cf1.min_range = MIN_RANGE;
    ultrasound_msg_cf2.min_range = MIN_RANGE;
    ultrasound_msg_cf3.min_range = MIN_RANGE;
    ultrasound_msg_cf4.min_range = MIN_RANGE;
    ultrasound_msg_cb1.min_range = MIN_RANGE;
    ultrasound_msg_cb2.min_range = MIN_RANGE;
    ultrasound_msg_cb3.min_range = MIN_RANGE;
    ultrasound_msg_cb4.min_range = MIN_RANGE;
    ultrasound_msg_cb5.min_range = MIN_RANGE;
    ultrasound_msg_cb6.min_range = MIN_RANGE;

    // Maximum Range
    ultrasound_msg_cf1.max_range = MAX_RANGE;
    ultrasound_msg_cf2.max_range = MAX_RANGE;
    ultrasound_msg_cf3.max_range = MAX_RANGE;
    ultrasound_msg_cf4.max_range = MAX_RANGE;
    ultrasound_msg_cb1.max_range = MAX_RANGE;
    ultrasound_msg_cb2.max_range = MAX_RANGE;
    ultrasound_msg_cb3.max_range = MAX_RANGE;
    ultrasound_msg_cb4.max_range = MAX_RANGE;
    ultrasound_msg_cb5.max_range = MAX_RANGE;
    ultrasound_msg_cb6.max_range = MAX_RANGE;
}

void startSensors()
{
    // Start the Sensors
    USS_CF1.start();
    USS_CF2.start();
    USS_CF3.start();
    USS_CF4.start();
    USS_CB1.start();
    USS_CB2.start();
    USS_CB3.start();
    USS_CB4.start();
    USS_CB5.start();
    USS_CB6.start();
}

void recordDist()
{
    // Record the Distances (meters)
    cfDist1 = USS_CF1.get_dist();
    cfDist2 = USS_CF2.get_dist();
    cfDist3 = USS_CF3.get_dist();
    cfDist4 = USS_CF4.get_dist();
    cbDist1 = USS_CB1.get_dist();
    cbDist2 = USS_CB2.get_dist();
    cbDist3 = USS_CB3.get_dist();
    cbDist4 = USS_CB4.get_dist();
    cbDist5 = USS_CB5.get_dist();
    cbDist6 = USS_CB6.get_dist();
}

void checkCliff()
{
    /* Check the Cliff Sensors (If distance more than clif treshold,
    hardcode the cliff value in, else no obstacle)*/
    if (static_cast<double>(cfDist1) <= CLIFF_TRH) {
        cfDist1 = CLIFF_ZRO;
    } else {
        cfDist1 = EXCL_ZONE;
    }
    if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
        cfDist2 = CLIFF_ZRO;
    } else {
        cfDist2 = EXCL_ZONE;
    }
    if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
        cfDist3 = CLIFF_ZRO;
    } else {
        cfDist3 = EXCL_ZONE;
    }
    if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
        cfDist4 = CLIFF_ZRO;
    } else {
        cfDist4 = EXCL_ZONE;
    }
}

void setRosDist()
{
    // Put distance in ROS messages
    ultrasound_msg_cf1.range = cfDist1;
    ultrasound_msg_cf2.range = cfDist2;
    ultrasound_msg_cf3.range = cfDist3;
    ultrasound_msg_cf4.range = cfDist4;
    ultrasound_msg_cb1.range = cbDist1;
    ultrasound_msg_cb2.range = cbDist2;
    ultrasound_msg_cb3.range = cbDist3;
    ultrasound_msg_cb4.range = cbDist4;
    ultrasound_msg_cb5.range = cbDist5;
    ultrasound_msg_cb6.range = cbDist6;
}

void setRosStamp()
{
    // Get current time and put into ROS messages
    nh.spinOnce(); // Reccuring connect and synchronise
    ultrasound_msg_cf1.header.stamp = nh.now();
    ultrasound_msg_cf2.header.stamp = nh.now();
    ultrasound_msg_cf3.header.stamp = nh.now();
    ultrasound_msg_cf4.header.stamp = nh.now();
    ultrasound_msg_cb1.header.stamp = nh.now();
    ultrasound_msg_cb2.header.stamp = nh.now();
    ultrasound_msg_cb3.header.stamp = nh.now();
    ultrasound_msg_cb4.header.stamp = nh.now();
    ultrasound_msg_cb5.header.stamp = nh.now();
    ultrasound_msg_cb6.header.stamp = nh.now();
}

void publishRosMsg()
{
    // Publish ROS messages
    ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
    wait_ms(wPeriod);
    ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
    wait_ms(wPeriod);
    ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
    wait_ms(wPeriod);
    ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
    wait_ms(wPeriod);
    ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
    wait_ms(wPeriod);
    ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
    wait_ms(wPeriod);
    ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
    wait_ms(wPeriod);
    ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
    wait_ms(wPeriod);
    ultrasound_pub_cb5.publish(&ultrasound_msg_cb5);
    wait_ms(wPeriod);
    ultrasound_pub_cb6.publish(&ultrasound_msg_cb6);
    wait_ms(wPeriod);
}