Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.cpp@10:7d954fba5e7a, 2019-05-08 (annotated)

Committer:
Luka_Danilovic
Date:
Wed May 08 13:55:17 2019 +0000
Revision:
10:7d954fba5e7a
Parent:
9:2d9a0c9e5456
Child:
11:955c2ed70de2
ROS - Working; USS - Working; GPS - Working; LED - Not supported by the board

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 10:7d954fba5e7a 16 int sats_n = 0; // GPS Satelite count
Luka_Danilovic 9:2d9a0c9e5456 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 10:7d954fba5e7a 26 nh.subscribe(AudioStatus);
Luka_Danilovic 10:7d954fba5e7a 27 nh.subscribe(Velocity);
Luka_Danilovic 10:7d954fba5e7a 28
Luka_Danilovic 9:2d9a0c9e5456 29 setupRosMsg(); // Setup the message constant values
Luka_Danilovic 9:2d9a0c9e5456 30 gps_odom_msg.header.frame_id = frameID_g; // Pack ROS frame ID
Luka_Danilovic 9:2d9a0c9e5456 31 gps_odom_msg.status.status = UNAGUMENTED; // Location fix method
Luka_Danilovic 9:2d9a0c9e5456 32 gps_odom_msg.status.service = GPS_CONSTELL; // Satelite constellation used
Luka_Danilovic 9:2d9a0c9e5456 33 gps_odom_msg.altitude = 0; // Altitude (Always fixed)
Luka_Danilovic 9:2d9a0c9e5456 34 gps_odom_msg.position_covariance[ 0] = CVX; // X pos covariance
Luka_Danilovic 9:2d9a0c9e5456 35 gps_odom_msg.position_covariance[ 1] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 36 gps_odom_msg.position_covariance[ 2] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 37 gps_odom_msg.position_covariance[ 3] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 38 gps_odom_msg.position_covariance[ 4] = CVY; // Y pos covariance
Luka_Danilovic 9:2d9a0c9e5456 39 gps_odom_msg.position_covariance[ 5] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 40 gps_odom_msg.position_covariance[ 6] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 41 gps_odom_msg.position_covariance[ 7] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 42 gps_odom_msg.position_covariance[ 8] = CVZ; // Z pos covariance
Luka_Danilovic 9:2d9a0c9e5456 43 gps_odom_msg.position_covariance_type = UNNWN; // Covariance type
Luka_Danilovic 9:2d9a0c9e5456 44 gpsSer.baud(GPS_Baud); // Set Baud rate for GPS Serial
Luka_Danilovic 9:2d9a0c9e5456 45
Luka_Danilovic 10:7d954fba5e7a 46 int LEDBrightness = 128;
Luka_Danilovic 10:7d954fba5e7a 47 float led_tmp = LED_COUNT/4;
Luka_Danilovic 10:7d954fba5e7a 48 int led_num = floor(led_tmp);
Luka_Danilovic 10:7d954fba5e7a 49 int k = 0;
Luka_Danilovic 10:7d954fba5e7a 50 rgb_color led_colour = (rgb_color){0, 0, 0};
Luka_Danilovic 9:2d9a0c9e5456 51
Luka_Danilovic 9:2d9a0c9e5456 52 while(!nh.connected()) { // While node handler is !connected
Luka_Danilovic 9:2d9a0c9e5456 53 nh.spinOnce(); // Attempt to connect & synchronise
Luka_Danilovic 9:2d9a0c9e5456 54 }
Luka_Danilovic 9:2d9a0c9e5456 55
Luka_Danilovic 9:2d9a0c9e5456 56 while(true) {
Luka_Danilovic 9:2d9a0c9e5456 57 startSensors(); // Start the Sensors
Luka_Danilovic 9:2d9a0c9e5456 58 recordDist(); // Record the Distances (meters)
Luka_Danilovic 9:2d9a0c9e5456 59 checkCliff(); // Check the Cliff Sensors Readings
Luka_Danilovic 9:2d9a0c9e5456 60 setRosDist(); // Put distance in ROS messages
Luka_Danilovic 9:2d9a0c9e5456 61 setRosStamp(); // Put time into ROS messages
Luka_Danilovic 9:2d9a0c9e5456 62 publishRosMsg(); // Publish ROS messages
Luka_Danilovic 9:2d9a0c9e5456 63
Luka_Danilovic 9:2d9a0c9e5456 64 if(gpsSer.readable()) { // If serial buffer has character
Luka_Danilovic 9:2d9a0c9e5456 65 gps_c = gpsSer.getc(); // Read serial buffer and store character
Luka_Danilovic 9:2d9a0c9e5456 66 gpsModule.encode(gps_c); // Encode character from GPS
Luka_Danilovic 9:2d9a0c9e5456 67
Luka_Danilovic 9:2d9a0c9e5456 68 } else {
Luka_Danilovic 9:2d9a0c9e5456 69 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 70 }
Luka_Danilovic 9:2d9a0c9e5456 71 if (gpsModule.location.isValid()) { // If GPS location is Valid
Luka_Danilovic 9:2d9a0c9e5456 72 gps_odom_msg.latitude = gpsModule.location.lng(); // Get Longtitude
Luka_Danilovic 9:2d9a0c9e5456 73 gps_odom_msg.longitude = gpsModule.location.lat(); // Get Latitude
Luka_Danilovic 9:2d9a0c9e5456 74 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 9:2d9a0c9e5456 75 gps_odom_msg.header.stamp = nh.now(); // Get current time
Luka_Danilovic 9:2d9a0c9e5456 76 gps_odom_pub.publish(&gps_odom_msg); // Publish the Odometry message
Luka_Danilovic 9:2d9a0c9e5456 77 }
Luka_Danilovic 9:2d9a0c9e5456 78 sats_n = gpsModule.satellites.value(); // Aquire satelite number
Luka_Danilovic 9:2d9a0c9e5456 79 if (sats_n > 0){
Luka_Danilovic 9:2d9a0c9e5456 80 leds = LEDS_ON;
Luka_Danilovic 9:2d9a0c9e5456 81 }
Luka_Danilovic 10:7d954fba5e7a 82
Luka_Danilovic 10:7d954fba5e7a 83 nh.spinOnce();
Luka_Danilovic 10:7d954fba5e7a 84 if (((abs(vel) || abs(ang)) > 0.1f) && (audio_state == "Playing")){
Luka_Danilovic 10:7d954fba5e7a 85 if(abs(vel) >= abs(ang)) {
Luka_Danilovic 10:7d954fba5e7a 86 LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
Luka_Danilovic 10:7d954fba5e7a 87 } else {
Luka_Danilovic 10:7d954fba5e7a 88 LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
Luka_Danilovic 10:7d954fba5e7a 89 }
Luka_Danilovic 10:7d954fba5e7a 90 led_colour = (rgb_color) {0, 0, 255};
Luka_Danilovic 10:7d954fba5e7a 91 for(int i = 0; i < led_num; i++){
Luka_Danilovic 10:7d954fba5e7a 92 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 93 }
Luka_Danilovic 10:7d954fba5e7a 94 led_colour = (rgb_color) {LEDBrightness, 0, 0};
Luka_Danilovic 10:7d954fba5e7a 95 for(int i = led_num; i < ((3*led_num)+1); i++){
Luka_Danilovic 10:7d954fba5e7a 96 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 97 }
Luka_Danilovic 10:7d954fba5e7a 98 led_colour = (rgb_color) {0, 0, 255};
Luka_Danilovic 10:7d954fba5e7a 99 for(int i = ((3*led_num)+1); i < LED_COUNT; i++){
Luka_Danilovic 10:7d954fba5e7a 100 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 101 }
Luka_Danilovic 10:7d954fba5e7a 102 } else if((abs(vel) || abs(ang)) > 0.1f) { //If Not In DeadZone (Robot Is Moving)
Luka_Danilovic 10:7d954fba5e7a 103 if(abs(vel) >= abs(ang)) {
Luka_Danilovic 10:7d954fba5e7a 104 LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
Luka_Danilovic 10:7d954fba5e7a 105 } else {
Luka_Danilovic 10:7d954fba5e7a 106 LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
Luka_Danilovic 10:7d954fba5e7a 107 }
Luka_Danilovic 10:7d954fba5e7a 108 led_colour = (rgb_color) {0, LEDBrightness, 0};
Luka_Danilovic 10:7d954fba5e7a 109 for(int i = 0; i < LED_COUNT; i++){
Luka_Danilovic 10:7d954fba5e7a 110 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 111 }
Luka_Danilovic 10:7d954fba5e7a 112 } else if (audio_state == "Playing") { //Else If Audio Is Playing
Luka_Danilovic 10:7d954fba5e7a 113 led_colour = (rgb_color) {0, 0, 255};
Luka_Danilovic 10:7d954fba5e7a 114 for(int i = 0; i < LED_COUNT; i++){
Luka_Danilovic 10:7d954fba5e7a 115 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 116 }
Luka_Danilovic 10:7d954fba5e7a 117 } else { //Robot Is Neither Moving Nor Playing Audio
Luka_Danilovic 10:7d954fba5e7a 118 led_colour = (rgb_color) {255, 0, 0};
Luka_Danilovic 10:7d954fba5e7a 119 for(int i = 0; i < LED_COUNT; i++){
Luka_Danilovic 10:7d954fba5e7a 120 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 121 }
Luka_Danilovic 10:7d954fba5e7a 122 }
Luka_Danilovic 10:7d954fba5e7a 123
Luka_Danilovic 10:7d954fba5e7a 124 ledStripFront.write(colors, LED_COUNT);
Luka_Danilovic 10:7d954fba5e7a 125 ledStripBack.write(colors, LED_COUNT);
Luka_Danilovic 10:7d954fba5e7a 126 wait_ms(5);
Luka_Danilovic 9:2d9a0c9e5456 127 }
Luka_Danilovic 9:2d9a0c9e5456 128 }
Luka_Danilovic 9:2d9a0c9e5456 129
Luka_Danilovic 9:2d9a0c9e5456 130 /*============================================================================*/
Luka_Danilovic 9:2d9a0c9e5456 131 /* FUNCTIONS */
Luka_Danilovic 9:2d9a0c9e5456 132 /*============================================================================*/
Luka_Danilovic 9:2d9a0c9e5456 133
Luka_Danilovic 9:2d9a0c9e5456 134
Luka_Danilovic 8:49306a01c52a 135 void advRosPub()
Luka_Danilovic 8:49306a01c52a 136 {
Luka_Danilovic 8:49306a01c52a 137 // Adverstise Ultrasound topic
Luka_Danilovic 8:49306a01c52a 138 nh.advertise(ultrasound_pub_cf1);
Luka_Danilovic 8:49306a01c52a 139 nh.advertise(ultrasound_pub_cf2);
Luka_Danilovic 8:49306a01c52a 140 nh.advertise(ultrasound_pub_cf3);
Luka_Danilovic 8:49306a01c52a 141 nh.advertise(ultrasound_pub_cf4);
Luka_Danilovic 8:49306a01c52a 142 nh.advertise(ultrasound_pub_cb1);
Luka_Danilovic 8:49306a01c52a 143 nh.advertise(ultrasound_pub_cb2);
Luka_Danilovic 8:49306a01c52a 144 nh.advertise(ultrasound_pub_cb3);
Luka_Danilovic 8:49306a01c52a 145 nh.advertise(ultrasound_pub_cb4);
Luka_Danilovic 8:49306a01c52a 146 }
Luka_Danilovic 0:215bdd87b602 147
Luka_Danilovic 0:215bdd87b602 148 void setupRosMsg()
Luka_Danilovic 3:cd1f2bde7ac2 149 {
Luka_Danilovic 8:49306a01c52a 150 // Setup the message constant values
Luka_Danilovic 8:49306a01c52a 151
Luka_Danilovic 3:cd1f2bde7ac2 152 // Frame ID
Luka_Danilovic 3:cd1f2bde7ac2 153 ultrasound_msg_cf1.header.frame_id = "uss_cf1";
Luka_Danilovic 7:6c26fdb1d226 154 ultrasound_msg_cf2.header.frame_id = "uss_cf2";
Luka_Danilovic 7:6c26fdb1d226 155 ultrasound_msg_cf3.header.frame_id = "uss_cf3";
Luka_Danilovic 7:6c26fdb1d226 156 ultrasound_msg_cf4.header.frame_id = "uss_cf4";
Luka_Danilovic 3:cd1f2bde7ac2 157 ultrasound_msg_cb1.header.frame_id = "uss_cb1";
Luka_Danilovic 7:6c26fdb1d226 158 ultrasound_msg_cb2.header.frame_id = "uss_cb2";
Luka_Danilovic 7:6c26fdb1d226 159 ultrasound_msg_cb3.header.frame_id = "uss_cb3";
Luka_Danilovic 7:6c26fdb1d226 160 ultrasound_msg_cb4.header.frame_id = "uss_cb4";
Luka_Danilovic 3:cd1f2bde7ac2 161
Luka_Danilovic 3:cd1f2bde7ac2 162 // Radiation Type
Luka_Danilovic 3:cd1f2bde7ac2 163 ultrasound_msg_cf1.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 164 ultrasound_msg_cf2.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 165 ultrasound_msg_cf3.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 166 ultrasound_msg_cf4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 167 ultrasound_msg_cb1.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 168 ultrasound_msg_cb2.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 169 ultrasound_msg_cb3.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 170 ultrasound_msg_cb4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 171
Luka_Danilovic 3:cd1f2bde7ac2 172 // Field of View
Luka_Danilovic 3:cd1f2bde7ac2 173 ultrasound_msg_cf1.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 174 ultrasound_msg_cf2.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 175 ultrasound_msg_cf3.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 176 ultrasound_msg_cf4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 177 ultrasound_msg_cb1.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 178 ultrasound_msg_cb2.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 179 ultrasound_msg_cb3.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 180 ultrasound_msg_cb4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 181
Luka_Danilovic 3:cd1f2bde7ac2 182 // Minumum Range
Luka_Danilovic 3:cd1f2bde7ac2 183 ultrasound_msg_cf1.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 184 ultrasound_msg_cf2.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 185 ultrasound_msg_cf3.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 186 ultrasound_msg_cf4.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 187 ultrasound_msg_cb1.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 188 ultrasound_msg_cb2.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 189 ultrasound_msg_cb3.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 190 ultrasound_msg_cb4.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 191
Luka_Danilovic 3:cd1f2bde7ac2 192 // Maximum Range
Luka_Danilovic 3:cd1f2bde7ac2 193 ultrasound_msg_cf1.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 194 ultrasound_msg_cf2.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 195 ultrasound_msg_cf3.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 196 ultrasound_msg_cf4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 197 ultrasound_msg_cb1.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 198 ultrasound_msg_cb2.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 199 ultrasound_msg_cb3.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 200 ultrasound_msg_cb4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 201 }
Luka_Danilovic 3:cd1f2bde7ac2 202
Luka_Danilovic 8:49306a01c52a 203 void startSensors()
Luka_Danilovic 8:49306a01c52a 204 {
Luka_Danilovic 8:49306a01c52a 205 // Start the Sensors
Luka_Danilovic 8:49306a01c52a 206 USS_CF1.start();
Luka_Danilovic 8:49306a01c52a 207 USS_CF2.start();
Luka_Danilovic 8:49306a01c52a 208 USS_CF3.start();
Luka_Danilovic 8:49306a01c52a 209 USS_CF4.start();
Luka_Danilovic 8:49306a01c52a 210 USS_CB1.start();
Luka_Danilovic 8:49306a01c52a 211 USS_CB2.start();
Luka_Danilovic 8:49306a01c52a 212 USS_CB3.start();
Luka_Danilovic 8:49306a01c52a 213 USS_CB4.start();
Luka_Danilovic 8:49306a01c52a 214 }
Luka_Danilovic 8:49306a01c52a 215
Luka_Danilovic 8:49306a01c52a 216 void recordDist()
Luka_Danilovic 8:49306a01c52a 217 {
Luka_Danilovic 8:49306a01c52a 218 // Record the Distances (meters)
Luka_Danilovic 8:49306a01c52a 219 cfDist1 = USS_CF1.get_dist();
Luka_Danilovic 8:49306a01c52a 220 cfDist2 = USS_CF2.get_dist();
Luka_Danilovic 8:49306a01c52a 221 cfDist3 = USS_CF3.get_dist();
Luka_Danilovic 8:49306a01c52a 222 cfDist4 = USS_CF4.get_dist();
Luka_Danilovic 8:49306a01c52a 223 cbDist1 = USS_CB1.get_dist();
Luka_Danilovic 8:49306a01c52a 224 cbDist2 = USS_CB2.get_dist();
Luka_Danilovic 8:49306a01c52a 225 cbDist3 = USS_CB3.get_dist();
Luka_Danilovic 8:49306a01c52a 226 cbDist4 = USS_CB4.get_dist();
Luka_Danilovic 8:49306a01c52a 227 }
Luka_Danilovic 8:49306a01c52a 228
Luka_Danilovic 3:cd1f2bde7ac2 229 void checkCliff()
Luka_Danilovic 3:cd1f2bde7ac2 230 {
Luka_Danilovic 3:cd1f2bde7ac2 231 /* Check the Cliff Sensors (If distance more than clif treshold,
Luka_Danilovic 3:cd1f2bde7ac2 232 hardcode the cliff value in, else no obstacle)*/
Luka_Danilovic 3:cd1f2bde7ac2 233 if (static_cast<double>(cfDist1) <= CLIFF_TRH) {
Luka_Danilovic 6:40fc84f50432 234 cfDist1 = CLIFF_ZRO;
Luka_Danilovic 3:cd1f2bde7ac2 235 } else {
Luka_Danilovic 6:40fc84f50432 236 cfDist1 = EXCL_ZONE;
Luka_Danilovic 3:cd1f2bde7ac2 237 }
Luka_Danilovic 7:6c26fdb1d226 238 if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 239 cfDist2 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 240 } else {
Luka_Danilovic 7:6c26fdb1d226 241 cfDist2 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 242 }
Luka_Danilovic 7:6c26fdb1d226 243 if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 244 cfDist3 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 245 } else {
Luka_Danilovic 7:6c26fdb1d226 246 cfDist3 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 247 }
Luka_Danilovic 7:6c26fdb1d226 248 if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 249 cfDist4 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 250 } else {
Luka_Danilovic 7:6c26fdb1d226 251 cfDist4 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 252 }
Luka_Danilovic 3:cd1f2bde7ac2 253 }
Luka_Danilovic 3:cd1f2bde7ac2 254
Luka_Danilovic 8:49306a01c52a 255 void setRosDist()
Luka_Danilovic 3:cd1f2bde7ac2 256 {
Luka_Danilovic 3:cd1f2bde7ac2 257 // Put distance in ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 258 ultrasound_msg_cf1.range = cfDist1;
Luka_Danilovic 7:6c26fdb1d226 259 ultrasound_msg_cf2.range = cfDist2;
Luka_Danilovic 7:6c26fdb1d226 260 ultrasound_msg_cf3.range = cfDist3;
Luka_Danilovic 7:6c26fdb1d226 261 ultrasound_msg_cf4.range = cfDist4;
Luka_Danilovic 3:cd1f2bde7ac2 262 ultrasound_msg_cb1.range = cbDist1;
Luka_Danilovic 7:6c26fdb1d226 263 ultrasound_msg_cb2.range = cbDist2;
Luka_Danilovic 7:6c26fdb1d226 264 ultrasound_msg_cb3.range = cbDist3;
Luka_Danilovic 7:6c26fdb1d226 265 ultrasound_msg_cb4.range = cbDist4;
Luka_Danilovic 3:cd1f2bde7ac2 266 }
Luka_Danilovic 3:cd1f2bde7ac2 267
Luka_Danilovic 8:49306a01c52a 268 void setRosStamp()
Luka_Danilovic 3:cd1f2bde7ac2 269 {
Luka_Danilovic 3:cd1f2bde7ac2 270 // Get current time and put into ROS messages
Luka_Danilovic 8:49306a01c52a 271 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 3:cd1f2bde7ac2 272 ultrasound_msg_cf1.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 273 ultrasound_msg_cf2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 274 ultrasound_msg_cf3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 275 ultrasound_msg_cf4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 276 ultrasound_msg_cb1.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 277 ultrasound_msg_cb2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 278 ultrasound_msg_cb3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 279 ultrasound_msg_cb4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 280 }
Luka_Danilovic 3:cd1f2bde7ac2 281
Luka_Danilovic 3:cd1f2bde7ac2 282 void publishRosMsg()
Luka_Danilovic 3:cd1f2bde7ac2 283 {
Luka_Danilovic 3:cd1f2bde7ac2 284 // Publish ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 285 ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
Luka_Danilovic 4:f74d88f5f629 286 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 287 ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
Luka_Danilovic 7:6c26fdb1d226 288 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 289 ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
Luka_Danilovic 7:6c26fdb1d226 290 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 291 ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
Luka_Danilovic 7:6c26fdb1d226 292 wait_ms(wPeriod);
Luka_Danilovic 3:cd1f2bde7ac2 293 ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
Luka_Danilovic 3:cd1f2bde7ac2 294 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 295 ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
Luka_Danilovic 7:6c26fdb1d226 296 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 297 ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
Luka_Danilovic 7:6c26fdb1d226 298 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 299 ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
Luka_Danilovic 7:6c26fdb1d226 300 wait_ms(wPeriod);
Luka_Danilovic 3:cd1f2bde7ac2 301 }
Luka_Danilovic 10:7d954fba5e7a 302
Luka_Danilovic 10:7d954fba5e7a 303 void AudioStatusCB(const std_msgs::String &status) {
Luka_Danilovic 10:7d954fba5e7a 304 audio_state = status.data;
Luka_Danilovic 10:7d954fba5e7a 305 }
Luka_Danilovic 10:7d954fba5e7a 306
Luka_Danilovic 10:7d954fba5e7a 307 oid VelocityCB(const nav_msgs::Odometry &odom) {
Luka_Danilovic 10:7d954fba5e7a 308 vel = odom.twist.twist.linear.x;
Luka_Danilovic 10:7d954fba5e7a 309 ang = odom.twist.twist.angular.z;
Luka_Danilovic 10:7d954fba5e7a 310 }
Luka_Danilovic 10:7d954fba5e7a 311
Luka_Danilovic 10:7d954fba5e7a 312 float Map(float x, float in_min, float in_max, float out_min, float out_max){
Luka_Danilovic 10:7d954fba5e7a 313 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
Luka_Danilovic 10:7d954fba5e7a 314 }