Luka Danilovic
Q
Dependencies: SRF05 ros_lib_kinetic TinyGPSPlus
Diff: PROJ515.cpp
- Revision:
- 8:49306a01c52a
- Parent:
- 7:6c26fdb1d226
- Child:
- 9:2d9a0c9e5456
diff -r 6c26fdb1d226 -r 49306a01c52a PROJ515.cpp
--- a/PROJ515.cpp Tue May 07 11:49:14 2019 +0000
+++ b/PROJ515.cpp Tue May 07 15:57:01 2019 +0000
@@ -1,4 +1,4 @@
-#include "PROJ515.hpp" // Contains all Libraries, Definitions & Function Prototypes
+#include "PROJ515.hpp" // Contains Libraries, Definitions & Function Prototypes
float cfDist1 = 0; // Distance returned by cliff u_sensor1 (m)
float cfDist2 = 0; // Distance returned by cliff u_sensor2 (m)
@@ -12,9 +12,23 @@
float wPeriod = 10; // Wait period(ms) for ROS publisher to finish publishing
+void advRosPub()
+{
+ // Adverstise Ultrasound topic
+ nh.advertise(ultrasound_pub_cf1);
+ nh.advertise(ultrasound_pub_cf2);
+ nh.advertise(ultrasound_pub_cf3);
+ nh.advertise(ultrasound_pub_cf4);
+ nh.advertise(ultrasound_pub_cb1);
+ nh.advertise(ultrasound_pub_cb2);
+ nh.advertise(ultrasound_pub_cb3);
+ nh.advertise(ultrasound_pub_cb4);
+}
void setupRosMsg()
{
+ // Setup the message constant values
+
// Frame ID
ultrasound_msg_cf1.header.frame_id = "uss_cf1";
ultrasound_msg_cf2.header.frame_id = "uss_cf2";
@@ -66,6 +80,32 @@
ultrasound_msg_cb4.max_range = MAX_RANGE;
}
+void startSensors()
+{
+ // Start the Sensors
+ USS_CF1.start();
+ USS_CF2.start();
+ USS_CF3.start();
+ USS_CF4.start();
+ USS_CB1.start();
+ USS_CB2.start();
+ USS_CB3.start();
+ USS_CB4.start();
+}
+
+void recordDist()
+{
+ // Record the Distances (meters)
+ cfDist1 = USS_CF1.get_dist();
+ cfDist2 = USS_CF2.get_dist();
+ cfDist3 = USS_CF3.get_dist();
+ cfDist4 = USS_CF4.get_dist();
+ cbDist1 = USS_CB1.get_dist();
+ cbDist2 = USS_CB2.get_dist();
+ cbDist3 = USS_CB3.get_dist();
+ cbDist4 = USS_CB4.get_dist();
+}
+
void checkCliff()
{
/* Check the Cliff Sensors (If distance more than clif treshold,
@@ -92,7 +132,7 @@
}
}
-void populateRosDist()
+void setRosDist()
{
// Put distance in ROS messages
ultrasound_msg_cf1.range = cfDist1;
@@ -105,9 +145,10 @@
ultrasound_msg_cb4.range = cbDist4;
}
-void populateRosStamp()
+void setRosStamp()
{
// Get current time and put into ROS messages
+ nh.spinOnce(); // Reccuring connect and synchronise
ultrasound_msg_cf1.header.stamp = nh.now();
ultrasound_msg_cf2.header.stamp = nh.now();
ultrasound_msg_cf3.header.stamp = nh.now();
@@ -121,125 +162,41 @@
void publishRosMsg()
{
// Publish ROS messages
- ultrasound_msg_cf1.header.stamp = nh.now();
ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
wait_ms(wPeriod);
- ultrasound_msg_cf2.header.stamp = nh.now();
ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
wait_ms(wPeriod);
- ultrasound_msg_cf3.header.stamp = nh.now();
ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
wait_ms(wPeriod);
- ultrasound_msg_cf4.header.stamp = nh.now();
ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
wait_ms(wPeriod);
- ultrasound_msg_cb1.header.stamp = nh.now();
ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
wait_ms(wPeriod);
- ultrasound_msg_cb2.header.stamp = nh.now();
ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
wait_ms(wPeriod);
- ultrasound_msg_cb3.header.stamp = nh.now();
ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
wait_ms(wPeriod);
- ultrasound_msg_cb4.header.stamp = nh.now();
ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
wait_ms(wPeriod);
}
int main()
{
- nh.getHardware()->setBaud(ROS_Baud); // Set Baud Rate for ROS Serial
- nh.initNode(); // Initialise ROS Node Handler
+ nh.getHardware()->setBaud(ROS_Baud); // Set Baud Rate for ROS Serial
+ nh.initNode(); // Initialise ROS Node Handler
+ advRosPub(); // Adverstise Ultrasound Topic
+ setupRosMsg(); // Setup the message constant values
- nh.advertise(ultrasound_pub_cf1); // Adverstise Ultrasound topic
- nh.advertise(ultrasound_pub_cf2); // Adverstise Ultrasound topic
- nh.advertise(ultrasound_pub_cf3); // Adverstise Ultrasound topic
- nh.advertise(ultrasound_pub_cf4); // Adverstise Ultrasound topic
- nh.advertise(ultrasound_pub_cb1); // Adverstise Ultrasound topic
- nh.advertise(ultrasound_pub_cb2); // Adverstise Ultrasound topic
- nh.advertise(ultrasound_pub_cb3); // Adverstise Ultrasound topic
- nh.advertise(ultrasound_pub_cb4); // Adverstise Ultrasound topic
-
- setupRosMsg();
-
- while(!nh.connected()) { // While node handler is not connected
- nh.spinOnce(); // Attempt to connect and synchronise
+ while(!nh.connected()) { // While node handler is !connected
+ nh.spinOnce(); // Attempt to connect & synchronise
}
while(true) {
-
- // Start the Sensors
- USS_CF1.start();
- USS_CF2.start();
- USS_CF3.start();
- USS_CF4.start();
- USS_CB1.start();
- USS_CB2.start();
- USS_CB3.start();
- USS_CB4.start();
-
- // Record the Distances (meters)
- cfDist1 = USS_CF1.get_dist_cm();
- cfDist2 = USS_CF2.get_dist_cm();
- cfDist3 = USS_CF3.get_dist_cm();
- cfDist4 = USS_CF4.get_dist_cm();
- cbDist1 = USS_CB1.get_dist_cm();
- cbDist2 = USS_CB2.get_dist_cm();
- cbDist3 = USS_CB3.get_dist_cm();
- cbDist4 = USS_CB4.get_dist_cm();
-
- /* Check the Cliff Sensors (If distance more than cliff treshold,
- hardcode the cliff value in, else no obstacle)*/
- checkCliff();
-
- //populateRosDist(); // Put distance in ROS messages
- ultrasound_msg_cf1.range = cfDist1;
- ultrasound_msg_cf2.range = cfDist2;
- ultrasound_msg_cf3.range = cfDist3;
- ultrasound_msg_cf4.range = cfDist4;
- ultrasound_msg_cb1.range = cbDist1;
- ultrasound_msg_cb2.range = cbDist2;
- ultrasound_msg_cb3.range = cbDist3;
- ultrasound_msg_cb4.range = cbDist4;
-
- nh.spinOnce(); // Reccuring connect and synchronise
- //populateRosStamp(); // Put time into ROS messages
- //publishRosMsg(); // Publish ROS messages
-
- ultrasound_msg_cf1.header.stamp = nh.now();
- ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
- wait_ms(wPeriod);
-
- ultrasound_msg_cf2.header.stamp = nh.now();
- ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
- wait_ms(wPeriod);
-
- ultrasound_msg_cf3.header.stamp = nh.now();
- ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
- wait_ms(wPeriod);
-
- ultrasound_msg_cf4.header.stamp = nh.now();
- ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
- wait_ms(wPeriod);
-
- ultrasound_msg_cb1.header.stamp = nh.now();
- ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
- wait_ms(wPeriod);
-
- ultrasound_msg_cb2.header.stamp = nh.now();
- ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
- wait_ms(wPeriod);
-
- ultrasound_msg_cb3.header.stamp = nh.now();
- ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
- wait_ms(wPeriod);
-
- ultrasound_msg_cb4.header.stamp = nh.now();
- ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
- wait_ms(wPeriod);
-
- wait_ms(15);
-
+ startSensors(); // Start the Sensors
+ recordDist(); // Record the Distances (meters)
+ checkCliff(); // Check the Cliff Sensors Readings
+ setRosDist(); // Put distance in ROS messages
+ setRosStamp(); // Put time into ROS messages
+ publishRosMsg(); // Publish ROS messages
}
}
\ No newline at end of file