File content as of revision 6:40fc84f50432:

#include "PROJ515.hpp" // Contains all 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 wPeriod = 18;   // Wait period(ms) for ROS publisher to finish publishing


void setupRosMsg()
{
    // 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";

    // 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;

    // 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;

    // 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;

    // 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;
}

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 = EXCL_ZONE;
//    } else {
//        cfDist2 = CLIFF_ZRO;
//    }
//    if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
//        cfDist3 = EXCL_ZONE;
//    } else {
//        cfDist3 = CLIFF_ZRO;
//    }
//    if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
//        cfDist4 = EXCL_ZONE;
//    } else {
//        cfDist4 = CLIFF_ZRO;
//    }
}

void populateRosDist()
{
    // 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;
}

void populateRosStamp()
{
    // Get current time and put into ROS messages
    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();
}

void publishRosMsg()
{
    // Publish ROS messages
    ultrasound_msg_cf1.header.stamp = nh.now();
    ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
    wait_ms(wPeriod);
//    ultrasound_msg_cf2.header.stamp = nh.now();
//    ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
//    wait_ms(wPeriod);
//    ultrasound_msg_cf3.header.stamp = nh.now();
//    ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
//    wait_ms(wPeriod);
//    ultrasound_msg_cf4.header.stamp = nh.now();
//    ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
//    wait_ms(wPeriod);
    ultrasound_msg_cb1.header.stamp = nh.now();
    ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
    wait_ms(wPeriod);
//    ultrasound_msg_cb2.header.stamp = nh.now();
//    ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
//    wait_ms(wPeriod);
//    ultrasound_msg_cb3.header.stamp = nh.now();
//    ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
//    wait_ms(wPeriod);
//    ultrasound_msg_cb4.header.stamp = nh.now();
//    ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
//    wait_ms(wPeriod);
}

int main()
{
    nh.getHardware()->setBaud(ROS_Baud);         // Set Baud Rate for ROS Serial
    nh.initNode();                               // Initialise ROS Node Handler

    nh.advertise(ultrasound_pub_cf1);            // Adverstise Ultrasound topic
//    nh.advertise(ultrasound_pub_cf2);            // Adverstise Ultrasound topic
//    nh.advertise(ultrasound_pub_cf3);            // Adverstise Ultrasound topic
//    nh.advertise(ultrasound_pub_cf4);            // Adverstise Ultrasound topic
    nh.advertise(ultrasound_pub_cb1);            // Adverstise Ultrasound topic
//    nh.advertise(ultrasound_pub_cb2);            // Adverstise Ultrasound topic
//    nh.advertise(ultrasound_pub_cb3);            // Adverstise Ultrasound topic
//    nh.advertise(ultrasound_pub_cb4);            // Adverstise Ultrasound topic

    setupRosMsg();

    while(!nh.connected()) {                     // While node handler is not connected
        nh.spinOnce();                           // Attempt to connect and synchronise
    }

    while(true) {

        // 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();
        
        // Record the Distances (meters)
        cfDist1 = USS_CF1.get_dist_cm();
//        cfDist2 = USS_CF2.get_dist_cm();
//        cfDist3 = USS_CF3.get_dist_cm();
//        cfDist4 = USS_CF4.get_dist_cm();
        cbDist1 = USS_CB1.get_dist_cm();
//        cbDist2 = USS_CB2.get_dist_cm();
//        cbDist3 = USS_CB3.get_dist_cm();
//        cbDist4 = USS_CB4.get_dist_cm();

        /* Check the Cliff Sensors (If distance more than cliff treshold,
        hardcode the cliff value in, else no obstacle)*/
        checkCliff();

        //populateRosDist();                       // Put distance in ROS messages
        ultrasound_msg_cf1.range = cfDist1;
        ultrasound_msg_cb1.range = cbDist1;
        nh.spinOnce();                           // Reccuring connect and synchronise
        //populateRosStamp();                      // Put time into ROS messages
        //publishRosMsg();                         // Publish ROS messages
        
        ultrasound_msg_cf1.header.stamp = nh.now();
    ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
    wait_ms(wPeriod);
       // wait(1);
        
            ultrasound_msg_cb1.header.stamp = nh.now();
    ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
    wait_ms(wPeriod);
      //  wait(1);
        
    }
}