Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.cpp@15:d23cd07d24c0, 2019-05-14 (annotated)

Committer:
Luka_Danilovic
Date:
Tue May 14 15:44:25 2019 +0000
Revision:
15:d23cd07d24c0
Parent:
12:1a3272d67500
Child:
14:b972d0d0f63d
4 curb sensors ; new max range at 0.4m; cb1 and cb2 swaped to match urdf

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 9:2d9a0c9e5456 15 char gps_c = 0; // GPS serial character
Luka_Danilovic 12:1a3272d67500 16 int sats_n = 0; // GPS Satelite count
Luka_Danilovic 11:955c2ed70de2 17
Luka_Danilovic 9:2d9a0c9e5456 18 int main()
Luka_Danilovic 9:2d9a0c9e5456 19 {
Luka_Danilovic 9:2d9a0c9e5456 20 nh.getHardware()->setBaud(ROS_Baud); // Set Baud Rate for ROS Serial
Luka_Danilovic 9:2d9a0c9e5456 21 nh.initNode(); // Initialise ROS Node Handler
Luka_Danilovic 9:2d9a0c9e5456 22
Luka_Danilovic 9:2d9a0c9e5456 23 advRosPub(); // Adverstise Ultrasound Topic
Luka_Danilovic 9:2d9a0c9e5456 24 nh.advertise(gps_odom_pub); // Adverstise Odometry topic
Luka_Danilovic 9:2d9a0c9e5456 25
Luka_Danilovic 9:2d9a0c9e5456 26 setupRosMsg(); // Setup the message constant values
Luka_Danilovic 9:2d9a0c9e5456 27 gps_odom_msg.header.frame_id = frameID_g; // Pack ROS frame ID
Luka_Danilovic 9:2d9a0c9e5456 28 gps_odom_msg.status.status = UNAGUMENTED; // Location fix method
Luka_Danilovic 9:2d9a0c9e5456 29 gps_odom_msg.status.service = GPS_CONSTELL; // Satelite constellation used
Luka_Danilovic 9:2d9a0c9e5456 30 gps_odom_msg.altitude = 0; // Altitude (Always fixed)
Luka_Danilovic 9:2d9a0c9e5456 31 gps_odom_msg.position_covariance[ 0] = CVX; // X pos covariance
Luka_Danilovic 9:2d9a0c9e5456 32 gps_odom_msg.position_covariance[ 1] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 33 gps_odom_msg.position_covariance[ 2] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 34 gps_odom_msg.position_covariance[ 3] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 35 gps_odom_msg.position_covariance[ 4] = CVY; // Y pos covariance
Luka_Danilovic 9:2d9a0c9e5456 36 gps_odom_msg.position_covariance[ 5] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 37 gps_odom_msg.position_covariance[ 6] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 38 gps_odom_msg.position_covariance[ 7] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 39 gps_odom_msg.position_covariance[ 8] = CVZ; // Z pos covariance
Luka_Danilovic 9:2d9a0c9e5456 40 gps_odom_msg.position_covariance_type = UNNWN; // Covariance type
Luka_Danilovic 9:2d9a0c9e5456 41 gpsSer.baud(GPS_Baud); // Set Baud rate for GPS Serial
Luka_Danilovic 9:2d9a0c9e5456 42
Luka_Danilovic 9:2d9a0c9e5456 43
Luka_Danilovic 9:2d9a0c9e5456 44 while(!nh.connected()) { // While node handler is !connected
Luka_Danilovic 9:2d9a0c9e5456 45 nh.spinOnce(); // Attempt to connect & synchronise
Luka_Danilovic 9:2d9a0c9e5456 46 }
Luka_Danilovic 9:2d9a0c9e5456 47
Luka_Danilovic 9:2d9a0c9e5456 48 while(true) {
Luka_Danilovic 9:2d9a0c9e5456 49 startSensors(); // Start the Sensors
Luka_Danilovic 9:2d9a0c9e5456 50 recordDist(); // Record the Distances (meters)
Luka_Danilovic 9:2d9a0c9e5456 51 checkCliff(); // Check the Cliff Sensors Readings
Luka_Danilovic 9:2d9a0c9e5456 52 setRosDist(); // Put distance in ROS messages
Luka_Danilovic 9:2d9a0c9e5456 53 setRosStamp(); // Put time into ROS messages
Luka_Danilovic 9:2d9a0c9e5456 54 publishRosMsg(); // Publish ROS messages
Luka_Danilovic 9:2d9a0c9e5456 55
Luka_Danilovic 9:2d9a0c9e5456 56 if(gpsSer.readable()) { // If serial buffer has character
Luka_Danilovic 9:2d9a0c9e5456 57 gps_c = gpsSer.getc(); // Read serial buffer and store character
Luka_Danilovic 9:2d9a0c9e5456 58 gpsModule.encode(gps_c); // Encode character from GPS
Luka_Danilovic 9:2d9a0c9e5456 59
Luka_Danilovic 9:2d9a0c9e5456 60 } else {
Luka_Danilovic 9:2d9a0c9e5456 61 leds = leds & 0b100; // Flash LED[0,1] bus OFF. Leave LED[2] on if its on to indicate that the signal was present
Luka_Danilovic 9:2d9a0c9e5456 62 }
Luka_Danilovic 9:2d9a0c9e5456 63 if (gpsModule.location.isValid()) { // If GPS location is Valid
Luka_Danilovic 9:2d9a0c9e5456 64 gps_odom_msg.latitude = gpsModule.location.lng(); // Get Longtitude
Luka_Danilovic 9:2d9a0c9e5456 65 gps_odom_msg.longitude = gpsModule.location.lat(); // Get Latitude
Luka_Danilovic 9:2d9a0c9e5456 66 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 9:2d9a0c9e5456 67 gps_odom_msg.header.stamp = nh.now(); // Get current time
Luka_Danilovic 9:2d9a0c9e5456 68 gps_odom_pub.publish(&gps_odom_msg); // Publish the Odometry message
Luka_Danilovic 9:2d9a0c9e5456 69 }
Luka_Danilovic 9:2d9a0c9e5456 70 sats_n = gpsModule.satellites.value(); // Aquire satelite number
Luka_Danilovic 9:2d9a0c9e5456 71 if (sats_n > 0){
Luka_Danilovic 9:2d9a0c9e5456 72 leds = LEDS_ON;
Luka_Danilovic 9:2d9a0c9e5456 73 }
Luka_Danilovic 9:2d9a0c9e5456 74 }
Luka_Danilovic 9:2d9a0c9e5456 75 }
Luka_Danilovic 9:2d9a0c9e5456 76
Luka_Danilovic 9:2d9a0c9e5456 77 /*============================================================================*/
Luka_Danilovic 9:2d9a0c9e5456 78 /* FUNCTIONS */
Luka_Danilovic 9:2d9a0c9e5456 79 /*============================================================================*/
Luka_Danilovic 9:2d9a0c9e5456 80
Luka_Danilovic 9:2d9a0c9e5456 81
Luka_Danilovic 8:49306a01c52a 82 void advRosPub()
Luka_Danilovic 8:49306a01c52a 83 {
Luka_Danilovic 8:49306a01c52a 84 // Adverstise Ultrasound topic
Luka_Danilovic 8:49306a01c52a 85 nh.advertise(ultrasound_pub_cf1);
Luka_Danilovic 8:49306a01c52a 86 nh.advertise(ultrasound_pub_cf2);
Luka_Danilovic 8:49306a01c52a 87 nh.advertise(ultrasound_pub_cf3);
Luka_Danilovic 8:49306a01c52a 88 nh.advertise(ultrasound_pub_cf4);
Luka_Danilovic 8:49306a01c52a 89 nh.advertise(ultrasound_pub_cb1);
Luka_Danilovic 8:49306a01c52a 90 nh.advertise(ultrasound_pub_cb2);
Luka_Danilovic 8:49306a01c52a 91 nh.advertise(ultrasound_pub_cb3);
Luka_Danilovic 8:49306a01c52a 92 nh.advertise(ultrasound_pub_cb4);
Luka_Danilovic 8:49306a01c52a 93 }
Luka_Danilovic 0:215bdd87b602 94
Luka_Danilovic 0:215bdd87b602 95 void setupRosMsg()
Luka_Danilovic 3:cd1f2bde7ac2 96 {
Luka_Danilovic 8:49306a01c52a 97 // Setup the message constant values
Luka_Danilovic 8:49306a01c52a 98
Luka_Danilovic 3:cd1f2bde7ac2 99 // Frame ID
Luka_Danilovic 3:cd1f2bde7ac2 100 ultrasound_msg_cf1.header.frame_id = "uss_cf1";
Luka_Danilovic 7:6c26fdb1d226 101 ultrasound_msg_cf2.header.frame_id = "uss_cf2";
Luka_Danilovic 7:6c26fdb1d226 102 ultrasound_msg_cf3.header.frame_id = "uss_cf3";
Luka_Danilovic 7:6c26fdb1d226 103 ultrasound_msg_cf4.header.frame_id = "uss_cf4";
Luka_Danilovic 3:cd1f2bde7ac2 104 ultrasound_msg_cb1.header.frame_id = "uss_cb1";
Luka_Danilovic 7:6c26fdb1d226 105 ultrasound_msg_cb2.header.frame_id = "uss_cb2";
Luka_Danilovic 7:6c26fdb1d226 106 ultrasound_msg_cb3.header.frame_id = "uss_cb3";
Luka_Danilovic 7:6c26fdb1d226 107 ultrasound_msg_cb4.header.frame_id = "uss_cb4";
Luka_Danilovic 3:cd1f2bde7ac2 108
Luka_Danilovic 3:cd1f2bde7ac2 109 // Radiation Type
Luka_Danilovic 3:cd1f2bde7ac2 110 ultrasound_msg_cf1.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 111 ultrasound_msg_cf2.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 112 ultrasound_msg_cf3.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 113 ultrasound_msg_cf4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 114 ultrasound_msg_cb1.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 115 ultrasound_msg_cb2.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 116 ultrasound_msg_cb3.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 117 ultrasound_msg_cb4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 118
Luka_Danilovic 3:cd1f2bde7ac2 119 // Field of View
Luka_Danilovic 3:cd1f2bde7ac2 120 ultrasound_msg_cf1.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 121 ultrasound_msg_cf2.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 122 ultrasound_msg_cf3.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 123 ultrasound_msg_cf4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 124 ultrasound_msg_cb1.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 125 ultrasound_msg_cb2.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 126 ultrasound_msg_cb3.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 127 ultrasound_msg_cb4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 128
Luka_Danilovic 3:cd1f2bde7ac2 129 // Minumum Range
Luka_Danilovic 3:cd1f2bde7ac2 130 ultrasound_msg_cf1.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 131 ultrasound_msg_cf2.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 132 ultrasound_msg_cf3.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 133 ultrasound_msg_cf4.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 134 ultrasound_msg_cb1.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 135 ultrasound_msg_cb2.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 136 ultrasound_msg_cb3.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 137 ultrasound_msg_cb4.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 138
Luka_Danilovic 3:cd1f2bde7ac2 139 // Maximum Range
Luka_Danilovic 3:cd1f2bde7ac2 140 ultrasound_msg_cf1.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 141 ultrasound_msg_cf2.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 142 ultrasound_msg_cf3.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 143 ultrasound_msg_cf4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 144 ultrasound_msg_cb1.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 145 ultrasound_msg_cb2.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 146 ultrasound_msg_cb3.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 147 ultrasound_msg_cb4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 148 }
Luka_Danilovic 3:cd1f2bde7ac2 149
Luka_Danilovic 8:49306a01c52a 150 void startSensors()
Luka_Danilovic 8:49306a01c52a 151 {
Luka_Danilovic 8:49306a01c52a 152 // Start the Sensors
Luka_Danilovic 8:49306a01c52a 153 USS_CF1.start();
Luka_Danilovic 8:49306a01c52a 154 USS_CF2.start();
Luka_Danilovic 8:49306a01c52a 155 USS_CF3.start();
Luka_Danilovic 8:49306a01c52a 156 USS_CF4.start();
Luka_Danilovic 8:49306a01c52a 157 USS_CB1.start();
Luka_Danilovic 8:49306a01c52a 158 USS_CB2.start();
Luka_Danilovic 8:49306a01c52a 159 USS_CB3.start();
Luka_Danilovic 8:49306a01c52a 160 USS_CB4.start();
Luka_Danilovic 8:49306a01c52a 161 }
Luka_Danilovic 8:49306a01c52a 162
Luka_Danilovic 8:49306a01c52a 163 void recordDist()
Luka_Danilovic 8:49306a01c52a 164 {
Luka_Danilovic 8:49306a01c52a 165 // Record the Distances (meters)
Luka_Danilovic 8:49306a01c52a 166 cfDist1 = USS_CF1.get_dist();
Luka_Danilovic 8:49306a01c52a 167 cfDist2 = USS_CF2.get_dist();
Luka_Danilovic 8:49306a01c52a 168 cfDist3 = USS_CF3.get_dist();
Luka_Danilovic 8:49306a01c52a 169 cfDist4 = USS_CF4.get_dist();
Luka_Danilovic 8:49306a01c52a 170 cbDist1 = USS_CB1.get_dist();
Luka_Danilovic 8:49306a01c52a 171 cbDist2 = USS_CB2.get_dist();
Luka_Danilovic 8:49306a01c52a 172 cbDist3 = USS_CB3.get_dist();
Luka_Danilovic 8:49306a01c52a 173 cbDist4 = USS_CB4.get_dist();
Luka_Danilovic 8:49306a01c52a 174 }
Luka_Danilovic 8:49306a01c52a 175
Luka_Danilovic 3:cd1f2bde7ac2 176 void checkCliff()
Luka_Danilovic 3:cd1f2bde7ac2 177 {
Luka_Danilovic 3:cd1f2bde7ac2 178 /* Check the Cliff Sensors (If distance more than clif treshold,
Luka_Danilovic 3:cd1f2bde7ac2 179 hardcode the cliff value in, else no obstacle)*/
Luka_Danilovic 3:cd1f2bde7ac2 180 if (static_cast<double>(cfDist1) <= CLIFF_TRH) {
Luka_Danilovic 6:40fc84f50432 181 cfDist1 = CLIFF_ZRO;
Luka_Danilovic 3:cd1f2bde7ac2 182 } else {
Luka_Danilovic 6:40fc84f50432 183 cfDist1 = EXCL_ZONE;
Luka_Danilovic 3:cd1f2bde7ac2 184 }
Luka_Danilovic 7:6c26fdb1d226 185 if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 186 cfDist2 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 187 } else {
Luka_Danilovic 7:6c26fdb1d226 188 cfDist2 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 189 }
Luka_Danilovic 7:6c26fdb1d226 190 if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 191 cfDist3 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 192 } else {
Luka_Danilovic 7:6c26fdb1d226 193 cfDist3 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 194 }
Luka_Danilovic 7:6c26fdb1d226 195 if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 196 cfDist4 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 197 } else {
Luka_Danilovic 7:6c26fdb1d226 198 cfDist4 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 199 }
Luka_Danilovic 3:cd1f2bde7ac2 200 }
Luka_Danilovic 3:cd1f2bde7ac2 201
Luka_Danilovic 8:49306a01c52a 202 void setRosDist()
Luka_Danilovic 3:cd1f2bde7ac2 203 {
Luka_Danilovic 3:cd1f2bde7ac2 204 // Put distance in ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 205 ultrasound_msg_cf1.range = cfDist1;
Luka_Danilovic 7:6c26fdb1d226 206 ultrasound_msg_cf2.range = cfDist2;
Luka_Danilovic 7:6c26fdb1d226 207 ultrasound_msg_cf3.range = cfDist3;
Luka_Danilovic 7:6c26fdb1d226 208 ultrasound_msg_cf4.range = cfDist4;
Luka_Danilovic 3:cd1f2bde7ac2 209 ultrasound_msg_cb1.range = cbDist1;
Luka_Danilovic 7:6c26fdb1d226 210 ultrasound_msg_cb2.range = cbDist2;
Luka_Danilovic 7:6c26fdb1d226 211 ultrasound_msg_cb3.range = cbDist3;
Luka_Danilovic 7:6c26fdb1d226 212 ultrasound_msg_cb4.range = cbDist4;
Luka_Danilovic 3:cd1f2bde7ac2 213 }
Luka_Danilovic 3:cd1f2bde7ac2 214
Luka_Danilovic 8:49306a01c52a 215 void setRosStamp()
Luka_Danilovic 3:cd1f2bde7ac2 216 {
Luka_Danilovic 3:cd1f2bde7ac2 217 // Get current time and put into ROS messages
Luka_Danilovic 8:49306a01c52a 218 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 3:cd1f2bde7ac2 219 ultrasound_msg_cf1.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 220 ultrasound_msg_cf2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 221 ultrasound_msg_cf3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 222 ultrasound_msg_cf4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 223 ultrasound_msg_cb1.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 224 ultrasound_msg_cb2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 225 ultrasound_msg_cb3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 226 ultrasound_msg_cb4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 227 }
Luka_Danilovic 3:cd1f2bde7ac2 228
Luka_Danilovic 3:cd1f2bde7ac2 229 void publishRosMsg()
Luka_Danilovic 3:cd1f2bde7ac2 230 {
Luka_Danilovic 3:cd1f2bde7ac2 231 // Publish ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 232 ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
Luka_Danilovic 4:f74d88f5f629 233 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 234 ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
Luka_Danilovic 7:6c26fdb1d226 235 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 236 ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
Luka_Danilovic 7:6c26fdb1d226 237 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 238 ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
Luka_Danilovic 7:6c26fdb1d226 239 wait_ms(wPeriod);
Luka_Danilovic 3:cd1f2bde7ac2 240 ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
Luka_Danilovic 3:cd1f2bde7ac2 241 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 242 ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
Luka_Danilovic 7:6c26fdb1d226 243 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 244 ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
Luka_Danilovic 7:6c26fdb1d226 245 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 246 ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
Luka_Danilovic 7:6c26fdb1d226 247 wait_ms(wPeriod);
Luka_Danilovic 3:cd1f2bde7ac2 248 }