Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.cpp@3:cd1f2bde7ac2, 2019-05-06 (annotated)

Committer:
Luka_Danilovic
Date:
Mon May 06 11:51:18 2019 +0000
Revision:
3:cd1f2bde7ac2
Parent:
2:7288dd12186e
Child:
4:f74d88f5f629
ROS - pre-test, not cleaned up

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Luka_Danilovic 0:215bdd87b602 1 #include "PROJ515.hpp" // Contains all Libraries, Definitions & Function Prototypes
Luka_Danilovic 0:215bdd87b602 2
Luka_Danilovic 2:7288dd12186e 3 float cfDist1 = 0; // Distance returned by cliff u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 4 float cfDist2 = 0; // Distance returned by cliff u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 5 float cfDist3 = 0; // Distance returned by cliff u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 6 float cfDist4 = 0; // Distance returned by cliff u_sensor4 (m)
Luka_Danilovic 2:7288dd12186e 7
Luka_Danilovic 2:7288dd12186e 8 float cbDist1 = 0; // Distance returned by curb u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 9 float cbDist2 = 0; // Distance returned by curb u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 10 float cbDist3 = 0; // Distance returned by curb u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 11 float cbDist4 = 0; // Distance returned by curb u_sensor4 (m)
Luka_Danilovic 2:7288dd12186e 12
Luka_Danilovic 2:7288dd12186e 13 float wPeriod = 5; // Wait period(ms) for ROS publisher to finish publishing
Luka_Danilovic 0:215bdd87b602 14
Luka_Danilovic 0:215bdd87b602 15
Luka_Danilovic 0:215bdd87b602 16 void setupRosMsg()
Luka_Danilovic 3:cd1f2bde7ac2 17 {
Luka_Danilovic 3:cd1f2bde7ac2 18 // Frame ID
Luka_Danilovic 3:cd1f2bde7ac2 19 ultrasound_msg_cf1.header.frame_id = "uss_cf1";
Luka_Danilovic 3:cd1f2bde7ac2 20 ultrasound_msg_cf2.header.frame_id = "uss_cf2";
Luka_Danilovic 3:cd1f2bde7ac2 21 ultrasound_msg_cf3.header.frame_id = "uss_cf3";
Luka_Danilovic 3:cd1f2bde7ac2 22 ultrasound_msg_cf4.header.frame_id = "uss_cf4";
Luka_Danilovic 3:cd1f2bde7ac2 23 ultrasound_msg_cb1.header.frame_id = "uss_cb1";
Luka_Danilovic 3:cd1f2bde7ac2 24 ultrasound_msg_cb2.header.frame_id = "uss_cb2";
Luka_Danilovic 3:cd1f2bde7ac2 25 ultrasound_msg_cb3.header.frame_id = "uss_cb3";
Luka_Danilovic 3:cd1f2bde7ac2 26 ultrasound_msg_cb4.header.frame_id = "uss_cb4";
Luka_Danilovic 3:cd1f2bde7ac2 27
Luka_Danilovic 3:cd1f2bde7ac2 28 // Radiation Type
Luka_Danilovic 3:cd1f2bde7ac2 29 ultrasound_msg_cf1.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 30 ultrasound_msg_cf2.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 31 ultrasound_msg_cf3.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 32 ultrasound_msg_cf4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 33 ultrasound_msg_cb1.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 34 ultrasound_msg_cb2.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 35 ultrasound_msg_cb3.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 36 ultrasound_msg_cb4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 37
Luka_Danilovic 3:cd1f2bde7ac2 38 // Field of View
Luka_Danilovic 3:cd1f2bde7ac2 39 ultrasound_msg_cf1.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 40 ultrasound_msg_cf2.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 41 ultrasound_msg_cf3.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 42 ultrasound_msg_cf4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 43 ultrasound_msg_cb1.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 44 ultrasound_msg_cb2.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 45 ultrasound_msg_cb3.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 46 ultrasound_msg_cb4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 47
Luka_Danilovic 3:cd1f2bde7ac2 48 // Minumum Range
Luka_Danilovic 3:cd1f2bde7ac2 49 ultrasound_msg_cf1.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 50 ultrasound_msg_cf2.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 51 ultrasound_msg_cf3.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 52 ultrasound_msg_cf4.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 53 ultrasound_msg_cb2.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 54 ultrasound_msg_cb2.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 55 ultrasound_msg_cb3.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 56 ultrasound_msg_cb4.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 57
Luka_Danilovic 3:cd1f2bde7ac2 58 // Maximum Range
Luka_Danilovic 3:cd1f2bde7ac2 59 ultrasound_msg_cf1.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 60 ultrasound_msg_cf2.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 61 ultrasound_msg_cf3.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 62 ultrasound_msg_cf4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 63 ultrasound_msg_cb1.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 64 ultrasound_msg_cb2.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 65 ultrasound_msg_cb3.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 66 ultrasound_msg_cb4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 67 }
Luka_Danilovic 3:cd1f2bde7ac2 68
Luka_Danilovic 3:cd1f2bde7ac2 69 void checkCliff()
Luka_Danilovic 3:cd1f2bde7ac2 70 {
Luka_Danilovic 3:cd1f2bde7ac2 71 /* Check the Cliff Sensors (If distance more than clif treshold,
Luka_Danilovic 3:cd1f2bde7ac2 72 hardcode the cliff value in, else no obstacle)*/
Luka_Danilovic 3:cd1f2bde7ac2 73 if (static_cast<double>(cfDist1) <= CLIFF_TRH) {
Luka_Danilovic 3:cd1f2bde7ac2 74 cfDist1 = EXCL_ZONE;
Luka_Danilovic 3:cd1f2bde7ac2 75 } else {
Luka_Danilovic 3:cd1f2bde7ac2 76 cfDist1 = CLIFF_ZRO;
Luka_Danilovic 3:cd1f2bde7ac2 77 }
Luka_Danilovic 3:cd1f2bde7ac2 78 if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
Luka_Danilovic 3:cd1f2bde7ac2 79 cfDist2 = EXCL_ZONE;
Luka_Danilovic 3:cd1f2bde7ac2 80 } else {
Luka_Danilovic 3:cd1f2bde7ac2 81 cfDist2 = CLIFF_ZRO;
Luka_Danilovic 3:cd1f2bde7ac2 82 }
Luka_Danilovic 3:cd1f2bde7ac2 83 if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
Luka_Danilovic 3:cd1f2bde7ac2 84 cfDist3 = EXCL_ZONE;
Luka_Danilovic 3:cd1f2bde7ac2 85 } else {
Luka_Danilovic 3:cd1f2bde7ac2 86 cfDist3 = CLIFF_ZRO;
Luka_Danilovic 3:cd1f2bde7ac2 87 }
Luka_Danilovic 3:cd1f2bde7ac2 88 if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
Luka_Danilovic 3:cd1f2bde7ac2 89 cfDist4 = EXCL_ZONE;
Luka_Danilovic 3:cd1f2bde7ac2 90 } else {
Luka_Danilovic 3:cd1f2bde7ac2 91 cfDist4 = CLIFF_ZRO;
Luka_Danilovic 3:cd1f2bde7ac2 92 }
Luka_Danilovic 3:cd1f2bde7ac2 93 }
Luka_Danilovic 3:cd1f2bde7ac2 94
Luka_Danilovic 3:cd1f2bde7ac2 95 void populateRosDist()
Luka_Danilovic 3:cd1f2bde7ac2 96 {
Luka_Danilovic 3:cd1f2bde7ac2 97 // Put distance in ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 98 ultrasound_msg_cf1.range = cfDist1;
Luka_Danilovic 3:cd1f2bde7ac2 99 ultrasound_msg_cf2.range = cfDist2;
Luka_Danilovic 3:cd1f2bde7ac2 100 ultrasound_msg_cf3.range = cfDist3;
Luka_Danilovic 3:cd1f2bde7ac2 101 ultrasound_msg_cf4.range = cfDist4;
Luka_Danilovic 3:cd1f2bde7ac2 102 ultrasound_msg_cb1.range = cbDist1;
Luka_Danilovic 3:cd1f2bde7ac2 103 ultrasound_msg_cb2.range = cbDist2;
Luka_Danilovic 3:cd1f2bde7ac2 104 ultrasound_msg_cb3.range = cbDist3;
Luka_Danilovic 3:cd1f2bde7ac2 105 ultrasound_msg_cb4.range = cbDist4;
Luka_Danilovic 3:cd1f2bde7ac2 106 }
Luka_Danilovic 3:cd1f2bde7ac2 107
Luka_Danilovic 3:cd1f2bde7ac2 108 void populateRosStamp()
Luka_Danilovic 3:cd1f2bde7ac2 109 {
Luka_Danilovic 3:cd1f2bde7ac2 110 // Get current time and put into ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 111 ultrasound_msg_cf1.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 112 ultrasound_msg_cf2.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 113 ultrasound_msg_cf3.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 114 ultrasound_msg_cf4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 115 ultrasound_msg_cb1.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 116 ultrasound_msg_cb2.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 117 ultrasound_msg_cb3.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 118 ultrasound_msg_cb4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 119 }
Luka_Danilovic 3:cd1f2bde7ac2 120
Luka_Danilovic 3:cd1f2bde7ac2 121 void publishRosMsg()
Luka_Danilovic 3:cd1f2bde7ac2 122 {
Luka_Danilovic 3:cd1f2bde7ac2 123 // Publish ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 124 ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
Luka_Danilovic 3:cd1f2bde7ac2 125 ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
Luka_Danilovic 3:cd1f2bde7ac2 126 ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
Luka_Danilovic 3:cd1f2bde7ac2 127 ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
Luka_Danilovic 3:cd1f2bde7ac2 128 ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
Luka_Danilovic 3:cd1f2bde7ac2 129 ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
Luka_Danilovic 3:cd1f2bde7ac2 130 ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
Luka_Danilovic 3:cd1f2bde7ac2 131 ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
Luka_Danilovic 3:cd1f2bde7ac2 132 wait_ms(wPeriod);
Luka_Danilovic 3:cd1f2bde7ac2 133 }
Luka_Danilovic 0:215bdd87b602 134
Luka_Danilovic 0:215bdd87b602 135 int main()
Luka_Danilovic 0:215bdd87b602 136 {
Luka_Danilovic 3:cd1f2bde7ac2 137 nh.getHardware()->setBaud(ROS_Baud); // Set Baud Rate for ROS Serial
Luka_Danilovic 3:cd1f2bde7ac2 138 nh.initNode(); // Initialise ROS Node Handler
Luka_Danilovic 3:cd1f2bde7ac2 139
Luka_Danilovic 3:cd1f2bde7ac2 140 nh.advertise(ultrasound_pub_cf1); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 141 nh.advertise(ultrasound_pub_cf2); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 142 nh.advertise(ultrasound_pub_cf3); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 143 nh.advertise(ultrasound_pub_cf4); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 144 nh.advertise(ultrasound_pub_cb1); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 145 nh.advertise(ultrasound_pub_cb2); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 146 nh.advertise(ultrasound_pub_cb3); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 147 nh.advertise(ultrasound_pub_cb4); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 148
Luka_Danilovic 3:cd1f2bde7ac2 149 while(!nh.connected()) { // While node handler is not connected
Luka_Danilovic 3:cd1f2bde7ac2 150 nh.spinOnce(); // Attempt to connect and synchronise
Luka_Danilovic 3:cd1f2bde7ac2 151 }
Luka_Danilovic 3:cd1f2bde7ac2 152
Luka_Danilovic 0:215bdd87b602 153 while(true) {
Luka_Danilovic 3:cd1f2bde7ac2 154
Luka_Danilovic 3:cd1f2bde7ac2 155 // Start the Sensors
Luka_Danilovic 2:7288dd12186e 156 USS_CF1.start();
Luka_Danilovic 2:7288dd12186e 157 USS_CF2.start();
Luka_Danilovic 2:7288dd12186e 158 USS_CF3.start();
Luka_Danilovic 3:cd1f2bde7ac2 159 USS_CF4.start();
Luka_Danilovic 2:7288dd12186e 160 USS_CB1.start();
Luka_Danilovic 2:7288dd12186e 161 USS_CB2.start();
Luka_Danilovic 2:7288dd12186e 162 USS_CB3.start();
Luka_Danilovic 3:cd1f2bde7ac2 163 USS_CB4.start();
Luka_Danilovic 3:cd1f2bde7ac2 164
Luka_Danilovic 3:cd1f2bde7ac2 165 // Record the Distances (meters)
Luka_Danilovic 2:7288dd12186e 166 cfDist1 = USS_CF1.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 167 cfDist2 = USS_CF2.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 168 cfDist3 = USS_CF3.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 169 cfDist4 = USS_CF4.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 170 cbDist1 = USS_CB1.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 171 cbDist2 = USS_CB2.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 172 cbDist3 = USS_CB3.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 173 cbDist4 = USS_CB4.get_dist_cm();
Luka_Danilovic 3:cd1f2bde7ac2 174
Luka_Danilovic 3:cd1f2bde7ac2 175 /* Check the Cliff Sensors (If distance more than cliff treshold,
Luka_Danilovic 3:cd1f2bde7ac2 176 hardcode the cliff value in, else no obstacle)*/
Luka_Danilovic 3:cd1f2bde7ac2 177 checkCliff();
Luka_Danilovic 2:7288dd12186e 178
Luka_Danilovic 3:cd1f2bde7ac2 179 populateRosDist(); // Put distance in ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 180 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 3:cd1f2bde7ac2 181 populateRosStamp(); // Put time into ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 182 publishRosMsg(); // Publish ROS messages
Luka_Danilovic 0:215bdd87b602 183 }
Luka_Danilovic 0:215bdd87b602 184 }