From 8a00a45488e1d5a7dd3d51351ba9c18be52c56f2 Mon Sep 17 00:00:00 2001
From: Franz Bethke <bethke@math.hu-berlin.de>
Date: Fri, 20 Apr 2018 18:33:22 +0200
Subject: [PATCH] Prepare USS for usage of new Lib
---
modules/catkin_ws/src/car/CMakeLists.txt | 13 +++++++---
.../src/car/include/ultrasonic/Ultrasonic.h | 4 +++-
.../src/car/src/ultrasonic/USS_SRF02.cpp | 24 ++++++++++---------
.../src/car/src/ultrasonic/Ultrasonic.cpp | 6 +++--
4 files changed, 30 insertions(+), 17 deletions(-)
diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index ea019a94..7c109908 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/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)
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h b/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h
index d31c30d3..885116c1 100644
--- a/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h
+++ b/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();
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
index 85243aad..7a5aa89d 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
@@ -1,5 +1,5 @@
-#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
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp b/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
index 987a684f..0f4ed519 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
+++ b/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(
--
GitLab