File content as of revision 8:49306a01c52a:

#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 wPeriod = 10;    // Wait period(ms) for ROS publisher to finish publishing

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

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

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

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

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

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

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

int main()
{
    nh.getHardware()->setBaud(ROS_Baud);    // Set Baud Rate for ROS Serial
    nh.initNode();                          // Initialise ROS Node Handler
    advRosPub();                            // Adverstise Ultrasound Topic
    setupRosMsg();                          // Setup the message constant values

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