Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.cpp@7:6c26fdb1d226, 2019-05-07 (annotated)

Committer:
Luka_Danilovic
Date:
Tue May 07 11:49:14 2019 +0000
Revision:
7:6c26fdb1d226
Parent:
6:40fc84f50432
Child:
8:49306a01c52a
ROS - all working, 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 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 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 7:6c26fdb1d226 20 ultrasound_msg_cf2.header.frame_id = "uss_cf2";
Luka_Danilovic 7:6c26fdb1d226 21 ultrasound_msg_cf3.header.frame_id = "uss_cf3";
Luka_Danilovic 7:6c26fdb1d226 22 ultrasound_msg_cf4.header.frame_id = "uss_cf4";
Luka_Danilovic 3:cd1f2bde7ac2 23 ultrasound_msg_cb1.header.frame_id = "uss_cb1";
Luka_Danilovic 7:6c26fdb1d226 24 ultrasound_msg_cb2.header.frame_id = "uss_cb2";
Luka_Danilovic 7:6c26fdb1d226 25 ultrasound_msg_cb3.header.frame_id = "uss_cb3";
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 30 ultrasound_msg_cf2.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 31 ultrasound_msg_cf3.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 32 ultrasound_msg_cf4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 33 ultrasound_msg_cb1.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 34 ultrasound_msg_cb2.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 35 ultrasound_msg_cb3.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 40 ultrasound_msg_cf2.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 41 ultrasound_msg_cf3.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 42 ultrasound_msg_cf4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 43 ultrasound_msg_cb1.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 44 ultrasound_msg_cb2.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 45 ultrasound_msg_cb3.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 50 ultrasound_msg_cf2.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 51 ultrasound_msg_cf3.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 52 ultrasound_msg_cf4.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 53 ultrasound_msg_cb1.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 54 ultrasound_msg_cb2.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 55 ultrasound_msg_cb3.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 60 ultrasound_msg_cf2.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 61 ultrasound_msg_cf3.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 62 ultrasound_msg_cf4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 63 ultrasound_msg_cb1.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 64 ultrasound_msg_cb2.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 65 ultrasound_msg_cb3.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 78 if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 79 cfDist2 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 80 } else {
Luka_Danilovic 7:6c26fdb1d226 81 cfDist2 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 82 }
Luka_Danilovic 7:6c26fdb1d226 83 if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 84 cfDist3 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 85 } else {
Luka_Danilovic 7:6c26fdb1d226 86 cfDist3 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 87 }
Luka_Danilovic 7:6c26fdb1d226 88 if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 89 cfDist4 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 90 } else {
Luka_Danilovic 7:6c26fdb1d226 91 cfDist4 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 99 ultrasound_msg_cf2.range = cfDist2;
Luka_Danilovic 7:6c26fdb1d226 100 ultrasound_msg_cf3.range = cfDist3;
Luka_Danilovic 7:6c26fdb1d226 101 ultrasound_msg_cf4.range = cfDist4;
Luka_Danilovic 3:cd1f2bde7ac2 102 ultrasound_msg_cb1.range = cbDist1;
Luka_Danilovic 7:6c26fdb1d226 103 ultrasound_msg_cb2.range = cbDist2;
Luka_Danilovic 7:6c26fdb1d226 104 ultrasound_msg_cb3.range = cbDist3;
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 112 ultrasound_msg_cf2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 113 ultrasound_msg_cf3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 114 ultrasound_msg_cf4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 115 ultrasound_msg_cb1.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 116 ultrasound_msg_cb2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 117 ultrasound_msg_cb3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 127 ultrasound_msg_cf2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 128 ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
Luka_Danilovic 7:6c26fdb1d226 129 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 130 ultrasound_msg_cf3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 131 ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
Luka_Danilovic 7:6c26fdb1d226 132 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 133 ultrasound_msg_cf4.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 134 ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 139 ultrasound_msg_cb2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 140 ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
Luka_Danilovic 7:6c26fdb1d226 141 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 142 ultrasound_msg_cb3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 143 ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
Luka_Danilovic 7:6c26fdb1d226 144 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 145 ultrasound_msg_cb4.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 146 ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 156 nh.advertise(ultrasound_pub_cf2); // Adverstise Ultrasound topic
Luka_Danilovic 7:6c26fdb1d226 157 nh.advertise(ultrasound_pub_cf3); // Adverstise Ultrasound topic
Luka_Danilovic 7:6c26fdb1d226 158 nh.advertise(ultrasound_pub_cf4); // Adverstise Ultrasound topic
Luka_Danilovic 3:cd1f2bde7ac2 159 nh.advertise(ultrasound_pub_cb1); // Adverstise Ultrasound topic
Luka_Danilovic 7:6c26fdb1d226 160 nh.advertise(ultrasound_pub_cb2); // Adverstise Ultrasound topic
Luka_Danilovic 7:6c26fdb1d226 161 nh.advertise(ultrasound_pub_cb3); // Adverstise Ultrasound topic
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 174 USS_CF2.start();
Luka_Danilovic 7:6c26fdb1d226 175 USS_CF3.start();
Luka_Danilovic 7:6c26fdb1d226 176 USS_CF4.start();
Luka_Danilovic 2:7288dd12186e 177 USS_CB1.start();
Luka_Danilovic 7:6c26fdb1d226 178 USS_CB2.start();
Luka_Danilovic 7:6c26fdb1d226 179 USS_CB3.start();
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 184 cfDist2 = USS_CF2.get_dist_cm();
Luka_Danilovic 7:6c26fdb1d226 185 cfDist3 = USS_CF3.get_dist_cm();
Luka_Danilovic 7:6c26fdb1d226 186 cfDist4 = USS_CF4.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 187 cbDist1 = USS_CB1.get_dist_cm();
Luka_Danilovic 7:6c26fdb1d226 188 cbDist2 = USS_CB2.get_dist_cm();
Luka_Danilovic 7:6c26fdb1d226 189 cbDist3 = USS_CB3.get_dist_cm();
Luka_Danilovic 7:6c26fdb1d226 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 7:6c26fdb1d226 198 ultrasound_msg_cf2.range = cfDist2;
Luka_Danilovic 7:6c26fdb1d226 199 ultrasound_msg_cf3.range = cfDist3;
Luka_Danilovic 7:6c26fdb1d226 200 ultrasound_msg_cf4.range = cfDist4;
Luka_Danilovic 4:f74d88f5f629 201 ultrasound_msg_cb1.range = cbDist1;
Luka_Danilovic 7:6c26fdb1d226 202 ultrasound_msg_cb2.range = cbDist2;
Luka_Danilovic 7:6c26fdb1d226 203 ultrasound_msg_cb3.range = cbDist3;
Luka_Danilovic 7:6c26fdb1d226 204 ultrasound_msg_cb4.range = cbDist4;
Luka_Danilovic 7:6c26fdb1d226 205
Luka_Danilovic 3:cd1f2bde7ac2 206 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 6:40fc84f50432 207 //populateRosStamp(); // Put time into ROS messages
Luka_Danilovic 6:40fc84f50432 208 //publishRosMsg(); // Publish ROS messages
Luka_Danilovic 6:40fc84f50432 209
Luka_Danilovic 6:40fc84f50432 210 ultrasound_msg_cf1.header.stamp = nh.now();
Luka_Danilovic 6:40fc84f50432 211 ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
Luka_Danilovic 6:40fc84f50432 212 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 213
Luka_Danilovic 7:6c26fdb1d226 214 ultrasound_msg_cf2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 215 ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
Luka_Danilovic 7:6c26fdb1d226 216 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 217
Luka_Danilovic 7:6c26fdb1d226 218 ultrasound_msg_cf3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 219 ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
Luka_Danilovic 7:6c26fdb1d226 220 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 221
Luka_Danilovic 7:6c26fdb1d226 222 ultrasound_msg_cf4.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 223 ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
Luka_Danilovic 7:6c26fdb1d226 224 wait_ms(wPeriod);
Luka_Danilovic 6:40fc84f50432 225
Luka_Danilovic 7:6c26fdb1d226 226 ultrasound_msg_cb1.header.stamp = nh.now();
Luka_Danilovic 6:40fc84f50432 227 ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
Luka_Danilovic 6:40fc84f50432 228 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 229
Luka_Danilovic 7:6c26fdb1d226 230 ultrasound_msg_cb2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 231 ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
Luka_Danilovic 7:6c26fdb1d226 232 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 233
Luka_Danilovic 7:6c26fdb1d226 234 ultrasound_msg_cb3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 235 ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
Luka_Danilovic 7:6c26fdb1d226 236 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 237
Luka_Danilovic 7:6c26fdb1d226 238 ultrasound_msg_cb4.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 239 ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
Luka_Danilovic 7:6c26fdb1d226 240 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 241
Luka_Danilovic 7:6c26fdb1d226 242 wait_ms(15);
Luka_Danilovic 7:6c26fdb1d226 243
Luka_Danilovic 0:215bdd87b602 244 }
Luka_Danilovic 0:215bdd87b602 245 }