Prepare USS for usage of new Lib

modules/catkin_ws/src/car/CMakeLists.txt

@@ -56,7 +56,12 @@ set(MAIN_NODE_SOURCE_FILES
         src/mainNode/NotifiableThread.cpp
         src/mainNode/PlatoonController.cpp
         src/mainNode/CruiseController.cpp
-   )
+)
+
+set(USS_SOURCE_FILES
+        src/ultrasonic/Ultrasonic.cpp
+        src/ultrasonic/USS_SRF02.cpp
+)
 
 set(LOCAL_INSTALL_DIR ${CMAKE_CURRENT_LIST_DIR}/../../install)
 
@@ -87,9 +92,11 @@ add_library(logging src/logging/Logging.cpp)
 add_dependencies(logging ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
 target_link_libraries(logging ${catkin_LIBRARIES})
 
-add_library(ultrasonic src/ultrasonic/Ultrasonic.cpp)
+add_library(ultrasonic ${USS_SOURCE_FILES})
 add_dependencies(ultrasonic ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
-target_link_libraries(ultrasonic ${catkin_LIBRARIES})
+target_link_libraries(ultrasonic
+    ${catkin_LIBRARIES}
+)
 
 add_library(camera src/camera/Camera.cpp)
 add_dependencies(camera ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)

modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h

@@ -4,10 +4,12 @@
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
 #include "boost/thread.hpp"
+#include "USS_SRF02.h"
 
 namespace car
 {
-	class Ultrasonic : public nodelet::Nodelet
+	class Ultrasonic : public USS_SRF02, 
+                       public nodelet::Nodelet
 	{
 		public:
 			virtual void onInit();

modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp

-#include "../include/USS_SRF02.h"
-#include "../lib/wiringPi/wiringPi/wiringPiI2C.h"
+#include "ultrasonic/USS_SRF02.h"
+#include <wiringPiI2C.h>
 
 #include <iostream>
 
@@ -40,22 +40,24 @@ USS_SRF02::USS_SRF02(int devId) {
 }
 
 int USS_SRF02::getDistance() {
-	int distance;
+    return 0;
+	// int distance;
 
-	if(fd == -1){
-		USS_SRF02::setup();
-	}
+	// if(fd == -1){
+	// 	USS_SRF02::setup();
+	// }
 
-	wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
-	usleep(DELAY * 1000);
-	distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+	// wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
+	// usleep(DELAY * 1000);
+	// distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
 
-	return distance;
+	// return distance;
 }
 
 void USS_SRF02::setup() {
 
-	USS_SRF02::fd = wiringPiI2CSetupInterface(DEVICE, this->devId);
+	// USS_SRF02::fd = wiringPiI2CSetupInterface(DEVICE, this->devId);
+    
 	//todo error handling,
 	//however wiringPiI2CSetupInterface() calls exit() if it fails
 

modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp

@@ -10,10 +10,10 @@ PLUGINLIB_EXPORT_CLASS(car::Ultrasonic, nodelet::Nodelet
 
 namespace car
 {
-Ultrasonic::Ultrasonic(ros::NodeHandle & nh, std::string & name) : nh_(nh), name_(name)
+Ultrasonic::Ultrasonic(ros::NodeHandle & nh, std::string & name) : USS_SRF02(0x76), nh_(nh), name_(name)
 {}
 
-Ultrasonic::Ultrasonic()
+Ultrasonic::Ultrasonic()  : USS_SRF02(0x76)
 {}
 
 Ultrasonic::~Ultrasonic()
@@ -21,6 +21,8 @@ Ultrasonic::~Ultrasonic()
 
 void Ultrasonic::onInit()
 {
+    int dist = getDistance();
+    
     NODELET_INFO("Ultrasonic::onInit -- START");
     ussData = nh_.advertise<ussDataMsg>("ussData", 1);
     main = boost::thread(