Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

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

Committer:
Luka_Danilovic
Date:
Tue May 14 15:34:59 2019 +0000
Revision:
14:b972d0d0f63d
Parent:
12:1a3272d67500 
Added two extra ultrasound sensors

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