Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.cpp@6:40fc84f50432, 2019-05-07 (annotated)

Committer:
Luka_Danilovic
Date:
Tue May 07 09:24:58 2019 +0000
Revision:
6:40fc84f50432
Parent:
4:f74d88f5f629
Child:
7:6c26fdb1d226
ROS - Working; cf1    - Working (With cliff detection); cb1   - Working

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 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 6:40fc84f50432 13 float wPeriod = 18; // 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 4:f74d88f5f629 20 //ultrasound_msg_cf2.header.frame_id = "uss_cf2";
Luka_Danilovic 4:f74d88f5f629 21 // ultrasound_msg_cf3.header.frame_id = "uss_cf3";
Luka_Danilovic 4:f74d88f5f629 22 // ultrasound_msg_cf4.header.frame_id = "uss_cf4";
Luka_Danilovic 3:cd1f2bde7ac2 23 ultrasound_msg_cb1.header.frame_id = "uss_cb1";
Luka_Danilovic 4:f74d88f5f629 24 // ultrasound_msg_cb2.header.frame_id = "uss_cb2";
Luka_Danilovic 4:f74d88f5f629 25 // ultrasound_msg_cb3.header.frame_id = "uss_cb3";
Luka_Danilovic 4:f74d88f5f629 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 4:f74d88f5f629 30 // ultrasound_msg_cf2.radiation_type = ULTRASOUND;
Luka_Danilovic 4:f74d88f5f629 31 // ultrasound_msg_cf3.radiation_type = ULTRASOUND;
Luka_Danilovic 4:f74d88f5f629 32 // ultrasound_msg_cf4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 33 ultrasound_msg_cb1.radiation_type = ULTRASOUND;
Luka_Danilovic 4:f74d88f5f629 34 // ultrasound_msg_cb2.radiation_type = ULTRASOUND;
Luka_Danilovic 4:f74d88f5f629 35 // ultrasound_msg_cb3.radiation_type = ULTRASOUND;
Luka_Danilovic 4:f74d88f5f629 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 4:f74d88f5f629 40 // ultrasound_msg_cf2.field_of_view = FOV;
Luka_Danilovic 4:f74d88f5f629 41 // ultrasound_msg_cf3.field_of_view = FOV;
Luka_Danilovic 4:f74d88f5f629 42 // ultrasound_msg_cf4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 43 ultrasound_msg_cb1.field_of_view = FOV;
Luka_Danilovic 4:f74d88f5f629 44 // ultrasound_msg_cb2.field_of_view = FOV;
Luka_Danilovic 4:f74d88f5f629 45 // ultrasound_msg_cb3.field_of_view = FOV;
Luka_Danilovic 4:f74d88f5f629 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 4:f74d88f5f629 50 // ultrasound_msg_cf2.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 51 // ultrasound_msg_cf3.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 52 // ultrasound_msg_cf4.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 53 ultrasound_msg_cb1.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 54 // ultrasound_msg_cb2.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 55 // ultrasound_msg_cb3.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 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 4:f74d88f5f629 60 // ultrasound_msg_cf2.max_range = MAX_RANGE;
Luka_Danilovic 4:f74d88f5f629 61 // ultrasound_msg_cf3.max_range = MAX_RANGE;
Luka_Danilovic 4:f74d88f5f629 62 // ultrasound_msg_cf4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 63 ultrasound_msg_cb1.max_range = MAX_RANGE;
Luka_Danilovic 4:f74d88f5f629 64 // ultrasound_msg_cb2.max_range = MAX_RANGE;
Luka_Danilovic 4:f74d88f5f629 65 // ultrasound_msg_cb3.max_range = MAX_RANGE;
Luka_Danilovic 4:f74d88f5f629 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 6:40fc84f50432 74 cfDist1 = CLIFF_ZRO;
Luka_Danilovic 3:cd1f2bde7ac2 75 } else {
Luka_Danilovic 6:40fc84f50432 76 cfDist1 = EXCL_ZONE;
Luka_Danilovic 3:cd1f2bde7ac2 77 }
Luka_Danilovic 4:f74d88f5f629 78 // if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
Luka_Danilovic 4:f74d88f5f629 79 // cfDist2 = EXCL_ZONE;
Luka_Danilovic 4:f74d88f5f629 80 // } else {
Luka_Danilovic 4:f74d88f5f629 81 // cfDist2 = CLIFF_ZRO;
Luka_Danilovic 4:f74d88f5f629 82 // }
Luka_Danilovic 4:f74d88f5f629 83 // if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
Luka_Danilovic 4:f74d88f5f629 84 // cfDist3 = EXCL_ZONE;
Luka_Danilovic 4:f74d88f5f629 85 // } else {
Luka_Danilovic 4:f74d88f5f629 86 // cfDist3 = CLIFF_ZRO;
Luka_Danilovic 4:f74d88f5f629 87 // }
Luka_Danilovic 4:f74d88f5f629 88 // if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
Luka_Danilovic 4:f74d88f5f629 89 // cfDist4 = EXCL_ZONE;
Luka_Danilovic 4:f74d88f5f629 90 // } else {
Luka_Danilovic 4:f74d88f5f629 91 // cfDist4 = CLIFF_ZRO;
Luka_Danilovic 4:f74d88f5f629 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 4:f74d88f5f629 99 // ultrasound_msg_cf2.range = cfDist2;
Luka_Danilovic 4:f74d88f5f629 100 // ultrasound_msg_cf3.range = cfDist3;
Luka_Danilovic 4:f74d88f5f629 101 // ultrasound_msg_cf4.range = cfDist4;
Luka_Danilovic 3:cd1f2bde7ac2 102 ultrasound_msg_cb1.range = cbDist1;
Luka_Danilovic 4:f74d88f5f629 103 // ultrasound_msg_cb2.range = cbDist2;
Luka_Danilovic 4:f74d88f5f629 104 // ultrasound_msg_cb3.range = cbDist3;
Luka_Danilovic 4:f74d88f5f629 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 4:f74d88f5f629 112 // ultrasound_msg_cf2.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 113 // ultrasound_msg_cf3.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 114 // ultrasound_msg_cf4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 115 ultrasound_msg_cb1.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 116 // ultrasound_msg_cb2.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 117 // ultrasound_msg_cb3.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 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 4:f74d88f5f629 124 ultrasound_msg_cf1.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 125 ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
Luka_Danilovic 4:f74d88f5f629 126 wait_ms(wPeriod);
Luka_Danilovic 4:f74d88f5f629 127 // ultrasound_msg_cf2.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 128 // ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
Luka_Danilovic 4:f74d88f5f629 129 // wait_ms(wPeriod);
Luka_Danilovic 4:f74d88f5f629 130 // ultrasound_msg_cf3.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 131 // ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
Luka_Danilovic 4:f74d88f5f629 132 // wait_ms(wPeriod);
Luka_Danilovic 4:f74d88f5f629 133 // ultrasound_msg_cf4.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 134 // ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
Luka_Danilovic 4:f74d88f5f629 135 // wait_ms(wPeriod);
Luka_Danilovic 4:f74d88f5f629 136 ultrasound_msg_cb1.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 137 ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
Luka_Danilovic 3:cd1f2bde7ac2 138 wait_ms(wPeriod);
Luka_Danilovic 4:f74d88f5f629 139 // ultrasound_msg_cb2.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 140 // ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
Luka_Danilovic 4:f74d88f5f629 141 // wait_ms(wPeriod);
Luka_Danilovic 4:f74d88f5f629 142 // ultrasound_msg_cb3.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 143 // ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
Luka_Danilovic 4:f74d88f5f629 144 // wait_ms(wPeriod);
Luka_Danilovic 4:f74d88f5f629 145 // ultrasound_msg_cb4.header.stamp = nh.now();
Luka_Danilovic 4:f74d88f5f629 146 // ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
Luka_Danilovic 4:f74d88f5f629 147 // wait_ms(wPeriod);
Luka_Danilovic 3:cd1f2bde7ac2 148 }
Luka_Danilovic 0:215bdd87b602 149
Luka_Danilovic 0:215bdd87b602 150 int main()
Luka_Danilovic 0:215bdd87b602 151 {
Luka_Danilovic 3:cd1f2bde7ac2 152 nh.getHardware()->setBaud(ROS_Baud); // Set Baud Rate for ROS Serial
Luka_Danilovic 3:cd1f2bde7ac2 153 nh.initNode(); // Initialise ROS Node Handler
Luka_Danilovic 3:cd1f2bde7ac2 154
Luka_Danilovic 3:cd1f2bde7ac2 155 nh.advertise(ultrasound_pub_cf1); // Adverstise Ultrasound topic
Luka_Danilovic 4:f74d88f5f629 156 // nh.advertise(ultrasound_pub_cf2); // Adverstise Ultrasound topic
Luka_Danilovic 4:f74d88f5f629 157 // nh.advertise(ultrasound_pub_cf3); // Adverstise Ultrasound topic
Luka_Danilovic 4:f74d88f5f629 158 // nh.advertise(ultrasound_pub_cf4); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 159 nh.advertise(ultrasound_pub_cb1); // Adverstise Ultrasound topic
Luka_Danilovic 4:f74d88f5f629 160 // nh.advertise(ultrasound_pub_cb2); // Adverstise Ultrasound topic
Luka_Danilovic 4:f74d88f5f629 161 // nh.advertise(ultrasound_pub_cb3); // Adverstise Ultrasound topic
Luka_Danilovic 4:f74d88f5f629 162 // nh.advertise(ultrasound_pub_cb4); // Adverstise Ultrasound topic
Luka_Danilovic 4:f74d88f5f629 163
Luka_Danilovic 4:f74d88f5f629 164 setupRosMsg();
Luka_Danilovic 3:cd1f2bde7ac2 165
Luka_Danilovic 3:cd1f2bde7ac2 166 while(!nh.connected()) { // While node handler is not connected
Luka_Danilovic 3:cd1f2bde7ac2 167 nh.spinOnce(); // Attempt to connect and synchronise
Luka_Danilovic 3:cd1f2bde7ac2 168 }
Luka_Danilovic 3:cd1f2bde7ac2 169
Luka_Danilovic 0:215bdd87b602 170 while(true) {
Luka_Danilovic 3:cd1f2bde7ac2 171
Luka_Danilovic 3:cd1f2bde7ac2 172 // Start the Sensors
Luka_Danilovic 2:7288dd12186e 173 USS_CF1.start();
Luka_Danilovic 4:f74d88f5f629 174 // USS_CF2.start();
Luka_Danilovic 4:f74d88f5f629 175 // USS_CF3.start();
Luka_Danilovic 4:f74d88f5f629 176 // USS_CF4.start();
Luka_Danilovic 2:7288dd12186e 177 USS_CB1.start();
Luka_Danilovic 4:f74d88f5f629 178 // USS_CB2.start();
Luka_Danilovic 4:f74d88f5f629 179 // USS_CB3.start();
Luka_Danilovic 4:f74d88f5f629 180 // USS_CB4.start();
Luka_Danilovic 4:f74d88f5f629 181
Luka_Danilovic 3:cd1f2bde7ac2 182 // Record the Distances (meters)
Luka_Danilovic 2:7288dd12186e 183 cfDist1 = USS_CF1.get_dist_cm();
Luka_Danilovic 4:f74d88f5f629 184 // cfDist2 = USS_CF2.get_dist_cm();
Luka_Danilovic 4:f74d88f5f629 185 // cfDist3 = USS_CF3.get_dist_cm();
Luka_Danilovic 4:f74d88f5f629 186 // cfDist4 = USS_CF4.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 187 cbDist1 = USS_CB1.get_dist_cm();
Luka_Danilovic 4:f74d88f5f629 188 // cbDist2 = USS_CB2.get_dist_cm();
Luka_Danilovic 4:f74d88f5f629 189 // cbDist3 = USS_CB3.get_dist_cm();
Luka_Danilovic 4:f74d88f5f629 190 // cbDist4 = USS_CB4.get_dist_cm();
Luka_Danilovic 3:cd1f2bde7ac2 191
Luka_Danilovic 3:cd1f2bde7ac2 192 /* Check the Cliff Sensors (If distance more than cliff treshold,
Luka_Danilovic 3:cd1f2bde7ac2 193 hardcode the cliff value in, else no obstacle)*/
Luka_Danilovic 3:cd1f2bde7ac2 194 checkCliff();
Luka_Danilovic 2:7288dd12186e 195
Luka_Danilovic 4:f74d88f5f629 196 //populateRosDist(); // Put distance in ROS messages
Luka_Danilovic 4:f74d88f5f629 197 ultrasound_msg_cf1.range = cfDist1;
Luka_Danilovic 4:f74d88f5f629 198 ultrasound_msg_cb1.range = cbDist1;
Luka_Danilovic 3:cd1f2bde7ac2 199 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 6:40fc84f50432 200 //populateRosStamp(); // Put time into ROS messages
Luka_Danilovic 6:40fc84f50432 201 //publishRosMsg(); // Publish ROS messages
Luka_Danilovic 6:40fc84f50432 202
Luka_Danilovic 6:40fc84f50432 203 ultrasound_msg_cf1.header.stamp = nh.now();
Luka_Danilovic 6:40fc84f50432 204 ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
Luka_Danilovic 6:40fc84f50432 205 wait_ms(wPeriod);
Luka_Danilovic 6:40fc84f50432 206 // wait(1);
Luka_Danilovic 6:40fc84f50432 207
Luka_Danilovic 6:40fc84f50432 208 ultrasound_msg_cb1.header.stamp = nh.now();
Luka_Danilovic 6:40fc84f50432 209 ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
Luka_Danilovic 6:40fc84f50432 210 wait_ms(wPeriod);
Luka_Danilovic 6:40fc84f50432 211 // wait(1);
Luka_Danilovic 6:40fc84f50432 212
Luka_Danilovic 0:215bdd87b602 213 }
Luka_Danilovic 0:215bdd87b602 214 }