Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.cpp@8:49306a01c52a, 2019-05-07 (annotated)

Committer:
Luka_Danilovic
Date:
Tue May 07 15:57:01 2019 +0000
Revision:
8:49306a01c52a
Parent:
7:6c26fdb1d226
Child:
9:2d9a0c9e5456
- ROS with functions; - All sensors working (With tuned timings); - Custom SRF05 class; - Functions not separated

Who changed what in which revision?

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