From 326a59af5ec543004c2f49ceb69ec7a74fcfb486 Mon Sep 17 00:00:00 2001 From: Juan Pablo Pino Bravo Date: Tue, 30 Apr 2024 13:43:48 +0200 Subject: [PATCH] Fix more cpplint issues --- CPPLINT.cfg | 4 --- include/oculus_driver/AsyncService.h | 5 +-- include/oculus_driver/Clock.h | 5 +-- include/oculus_driver/OculusMessage.h | 37 +++++++++----------- include/oculus_driver/helpers.h | 50 +++++++++++++-------------- include/oculus_driver/rtac_helpers.h | 14 ++++---- include/oculus_driver/utils.h | 38 ++++++++++---------- python/src/oculus_files.cpp | 21 ++++++----- python/src/oculus_message.cpp | 23 ++++++------ python/src/oculus_python.cpp | 32 ++++++++--------- src/AsyncService.cpp | 10 +++--- src/Recorder.cpp | 44 +++++++++++------------ src/SonarClient.cpp | 28 +++++++-------- src/SonarDriver.cpp | 22 ++++++------ src/StatusListener.cpp | 8 ++--- src/print_utils.cpp | 4 +-- tests/src/async_client_test.cpp | 8 ++--- tests/src/client_test.cpp | 6 ++-- tests/src/config_test.cpp | 14 ++++---- tests/src/filereader_test.cpp | 8 ++--- tests/src/helpers_test.cpp | 27 +++++++-------- tests/src/recorder_test.cpp | 18 +++++----- tests/src/status_display.cpp | 2 +- tests/src/statusraw_test.cpp | 8 ++--- 24 files changed, 211 insertions(+), 225 deletions(-) diff --git a/CPPLINT.cfg b/CPPLINT.cfg index 9fd871a..33e7ac2 100644 --- a/CPPLINT.cfg +++ b/CPPLINT.cfg @@ -7,7 +7,3 @@ filter=-build/namespaces filter=-whitespace/braces filter=-build/include_subdir filter=-readability/todo - - -# Should consider re-enabling -filter=-whitespace/parens diff --git a/include/oculus_driver/AsyncService.h b/include/oculus_driver/AsyncService.h index 82834a8..3cceaa2 100644 --- a/include/oculus_driver/AsyncService.h +++ b/include/oculus_driver/AsyncService.h @@ -16,8 +16,7 @@ * along with this program. If not, see . *****************************************************************************/ -#ifndef _DEF_OCULUS_DRIVER_ASYNC_SERVICE_H_ -#define _DEF_OCULUS_DRIVER_ASYNC_SERVICE_H_ +#pragma once #include #include @@ -53,5 +52,3 @@ class AsyncService }; } // namespace oculus - -#endif //_DEF_OCULUS_DRIVER_ASYNC_SERVICE_H_ diff --git a/include/oculus_driver/Clock.h b/include/oculus_driver/Clock.h index c16f1ac..90d8447 100644 --- a/include/oculus_driver/Clock.h +++ b/include/oculus_driver/Clock.h @@ -16,8 +16,7 @@ * along with this program. If not, see . *****************************************************************************/ -#ifndef _DEF_OCULUS_DRIVER_CLOCK_H_ -#define _DEF_OCULUS_DRIVER_CLOCK_H_ +#pragma once #include #include @@ -73,5 +72,3 @@ inline std::ostream& operator<<(std::ostream& os, const oculus::Clock& clock) os << clock.now() << "s"; return os; } - -#endif //_DEF_OCULUS_DRIVER_CLOCK_H_ diff --git a/include/oculus_driver/OculusMessage.h b/include/oculus_driver/OculusMessage.h index 1281b30..267b468 100644 --- a/include/oculus_driver/OculusMessage.h +++ b/include/oculus_driver/OculusMessage.h @@ -16,8 +16,7 @@ * along with this program. If not, see . *****************************************************************************/ -#ifndef _DEF_OCULUS_DRIVER_OCULUS_MESSAGE_H_ -#define _DEF_OCULUS_DRIVER_OCULUS_MESSAGE_H_ +#pragma once #include "oculus_driver/Oculus.h" #include "oculus_driver/utils.h" @@ -104,7 +103,7 @@ class Message { OculusMessageHeader header; std::memcpy(&header, data, sizeof(OculusMessageHeader)); - if(!header_valid(header) || size != header.payloadSize + sizeof(OculusMessageHeader)) { return nullptr; } + if (!header_valid(header) || size != header.payloadSize + sizeof(OculusMessageHeader)) { return nullptr; } auto res = Create(); res->header_ = header; res->timestamp_ = stamp; @@ -169,8 +168,8 @@ class PingWrapper PingWrapper(const Message::ConstPtr& msg) : msg_(msg) { - if(!msg_) { throw std::runtime_error("Trying to make a PingMessage out of empty data."); } - if(!msg_->is_ping_message()) { throw std::runtime_error("Trying to make a PingMessage out of non-ping data."); } + if (!msg_) { throw std::runtime_error("Trying to make a PingMessage out of empty data."); } + if (!msg_->is_ping_message()) { throw std::runtime_error("Trying to make a PingMessage out of non-ping data."); } } public: @@ -235,7 +234,7 @@ class PingWrapper1 : public PingWrapper PingWrapper1(const Message::ConstPtr& msg) : PingWrapper(msg) { - if(msg->message_version() == 2) + if (msg->message_version() == 2) { throw std::runtime_error("Tried to initialize a PingWrapper1 with data from a PingWrapper2"); } @@ -295,7 +294,7 @@ class PingWrapper1 : public PingWrapper virtual uint8_t sample_size() const { - switch(this->metadata().dataSize) + switch (this->metadata().dataSize) { case ImageData8Bit: return 1; @@ -308,11 +307,11 @@ class PingWrapper1 : public PingWrapper default: // invalid value in metadata.dataSize. Deducing from message size. auto lineStep = this->metadata().imageSize / this->metadata().nRanges; - if(lineStep * this->metadata().nRanges != this->metadata().imageSize) { return 0; } - if(this->has_gains()) lineStep -= 4; + if (lineStep * this->metadata().nRanges != this->metadata().imageSize) { return 0; } + if (this->has_gains()) lineStep -= 4; auto sampleSize = lineStep / this->metadata().nBeams; // Checking integrity - if(sampleSize * this->metadata().nBeams != lineStep) { return 0; } + if (sampleSize * this->metadata().nBeams != lineStep) { return 0; } return sampleSize; } } @@ -388,7 +387,7 @@ class PingWrapper2 : public PingWrapper PingWrapper2(const Message::ConstPtr& msg) : PingWrapper(msg) { - if(msg->message_version() != 2) + if (msg->message_version() != 2) { throw std::runtime_error("Tried to initialize a PingWrapper2 with data from a PingWrapper1"); } @@ -449,7 +448,7 @@ class PingWrapper2 : public PingWrapper virtual uint8_t sample_size() const { - switch(this->metadata().dataSize) + switch (this->metadata().dataSize) { case ImageData8Bit: return 1; @@ -462,11 +461,11 @@ class PingWrapper2 : public PingWrapper default: // invalid value in metadata.dataSize. Deducing from message size. auto lineStep = this->metadata().imageSize / this->metadata().nRanges; - if(lineStep * this->metadata().nRanges != this->metadata().imageSize) { return 0; } - if(this->has_gains()) lineStep -= 4; + if (lineStep * this->metadata().nRanges != this->metadata().imageSize) { return 0; } + if (this->has_gains()) lineStep -= 4; auto sampleSize = lineStep / this->metadata().nBeams; // Checking integrity - if(sampleSize * this->metadata().nBeams != lineStep) { return 0; } + if (sampleSize * this->metadata().nBeams != lineStep) { return 0; } return sampleSize; } } @@ -543,8 +542,8 @@ class PingMessage static PingWrapper::Ptr make_ping_wrapper(const Message::ConstPtr& msg) { - if(!msg || !msg->is_ping_message()) { return nullptr; } - if(msg->message_version() == 2) { return PingWrapper2::Create(msg); } + if (!msg || !msg->is_ping_message()) { return nullptr; } + if (msg->message_version() == 2) { return PingWrapper2::Create(msg); } else { return PingWrapper1::Create(msg); } } @@ -567,7 +566,7 @@ class PingMessage { OculusMessageHeader header; std::memcpy(&header, data, sizeof(OculusMessageHeader)); - if(!is_ping_message(header) || size != header.payloadSize + sizeof(OculusMessageHeader)) { return nullptr; } + if (!is_ping_message(header) || size != header.payloadSize + sizeof(OculusMessageHeader)) { return nullptr; } return Create(Message::Create(size, data, stamp)); } @@ -708,5 +707,3 @@ class PingMessage }; } // namespace oculus - -#endif //_DEF_OCULUS_DRIVER_OCULUS_MESSAGE_H_ diff --git a/include/oculus_driver/helpers.h b/include/oculus_driver/helpers.h index 2eaf4fa..8fec644 100644 --- a/include/oculus_driver/helpers.h +++ b/include/oculus_driver/helpers.h @@ -14,7 +14,7 @@ inline void write_pgm(const std::string& filename, const uint8_t* data) { std::ofstream f(filename, std::ios::out | std::ios::binary); - if(!f.is_open()) { + if (!f.is_open()) { std::ostringstream oss; oss << "Could not open file for writing : " << filename; throw std::runtime_error(oss.str()); @@ -33,7 +33,7 @@ inline void write_pgm(const std::string& filename, { float m = data[0]; float M = data[0]; - for(unsigned int i = 0; i < width*height; i++) { + for (unsigned int i = 0; i < width*height; i++) { m = std::min(m, data[i]); M = std::max(M, data[i]); } @@ -41,7 +41,7 @@ inline void write_pgm(const std::string& filename, float a = 255.0 / (M - m); float b = -m * a; std::vector imageData(width*height); - for(unsigned int i = 0; i < width*height; i++) { + for (unsigned int i = 0; i < width*height; i++) { imageData[i] = a*data[i] + b; } write_pgm(filename, width, height, imageData.data()); @@ -121,42 +121,42 @@ inline void ping_data_to_array(T* dst, const OculusPingResultType& metadata, const std::vector& pingData) { - if(has_16bits_data(metadata)) { + if (has_16bits_data(metadata)) { auto data = (const uint16_t*)(pingData.data() + metadata.imageOffset); - if(has_gains(metadata)) { + if (has_gains(metadata)) { // gain is sent - for(unsigned int h = 0; h < metadata.nRanges; h++) { + for (unsigned int h = 0; h < metadata.nRanges; h++) { float gain = 1.0f / sqrt((float)((const uint32_t*)data)[0]); data += 2; - for(int w = 0; w < metadata.nBeams; w++) { + for (int w = 0; w < metadata.nBeams; w++) { dst[metadata.nBeams*w + h] = gain * data[w]; } data += metadata.nBeams; } } else { - //gain was not sent - for(unsigned int i = 0; i < metadata.nBeams*metadata.nRanges; i++) { + // gain was not sent + for (unsigned int i = 0; i < metadata.nBeams*metadata.nRanges; i++) { dst[i] = data[i]; } } } else { auto data = (const uint8_t*)(pingData.data() + metadata.imageOffset); - if(has_gains(metadata)) { + if (has_gains(metadata)) { // gain is sent - for(unsigned int h = 0; h < metadata.nRanges; h++) { + for (unsigned int h = 0; h < metadata.nRanges; h++) { float gain = 1.0f / sqrt((float)((const uint32_t*)data)[0]); data += 4; - for(int w = 0; w < metadata.nBeams; w++) { + for (int w = 0; w < metadata.nBeams; w++) { dst[metadata.nBeams*w + h] = gain * data[w]; } data += metadata.nBeams; } } else { - //gain was not sent - for(unsigned int i = 0; i < metadata.nBeams*metadata.nRanges; i++) { + // gain was not sent + for (unsigned int i = 0; i < metadata.nBeams*metadata.nRanges; i++) { dst[i] = data[i]; } } @@ -165,10 +165,10 @@ inline void ping_data_to_array(T* dst, inline std::vector get_ping_acoustic_data(const std::vector& pingData) { auto header = *reinterpret_cast(pingData.data()); - if(header.msgId != MsgSimplePingResult) { + if (header.msgId != MsgSimplePingResult) { throw std::runtime_error("Not a ping result"); } - if(header.msgVersion != 2) { + if (header.msgVersion != 2) { auto metadata = *reinterpret_cast(pingData.data()); std::vector dst(metadata.nBeams * metadata.nRanges); ping_data_to_array(dst.data(), metadata, pingData); @@ -192,17 +192,17 @@ inline void get_ping_bearings(T* dst, { // copying bearing angles ( auto bearingData = (const int16_t*)(pingData.data() + sizeof(OculusPingResultType)); - for(unsigned int i = 0; i < metadata.nBeams; i++) { + for (unsigned int i = 0; i < metadata.nBeams; i++) { dst[i] = (0.01 * M_PI / 180.0) * bearingData[i]; } } inline std::vector get_ping_bearings(const std::vector& pingData) { auto header = *reinterpret_cast(pingData.data()); - if(header.msgId != MsgSimplePingResult) { + if (header.msgId != MsgSimplePingResult) { throw std::runtime_error("Not a ping result"); } - if(header.msgVersion != 2) { + if (header.msgVersion != 2) { auto metadata = *reinterpret_cast(pingData.data()); std::vector dst(metadata.nBeams); get_ping_bearings(dst.data(), metadata, pingData); @@ -249,14 +249,14 @@ inline std::pair image_from_ping_data( // Making a lookup table to rapidly get bearing index from a bearing angle. // This comes down to performing a nearest neighbour interpolation. std::vector bearingLut(metadata.nBeams); - for(unsigned int b = 0; b < metadata.nBeams; b++) { + for (unsigned int b = 0; b < metadata.nBeams; b++) { float bearingAngle = ((bearings.back() - bearings.front())*b) / (metadata.nBeams - 1) + bearings.front(); unsigned int idx = 0; float minDiff = std::abs(bearings[0] - bearingAngle); - for(int i = 1; i image_from_ping_data( // and y positive is right direction. // In the image coordinates, x points to the top and y points to the right. float imageResolution = get_range(metadata) / (height - 1); - for(unsigned int h = 0; h < height; h++) { + for (unsigned int h = 0; h < height; h++) { // inverting x dimension to have origin at the bottom a x up. float x = imageResolution * (height - 1 - h); - for(unsigned int w = 0; w < width; w++) { + for (unsigned int w = 0; w < width; w++) { float y = imageResolution * (w - 0.5f*width); float range = sqrt(x*x + y*y); float bearing = std::atan2(y, x); - if(range < 0.0f || range > get_range(metadata) || abs(bearing) > 0.5f*aperture) { + if (range < 0.0f || range > get_range(metadata) || abs(bearing) > 0.5f*aperture) { // we are outside of the sonar fan imageData[width*h + w] = 0.0f; } diff --git a/include/oculus_driver/rtac_helpers.h b/include/oculus_driver/rtac_helpers.h index a2bc0b9..9e52e8d 100644 --- a/include/oculus_driver/rtac_helpers.h +++ b/include/oculus_driver/rtac_helpers.h @@ -25,28 +25,28 @@ inline void oculus_to_rtac(SonarPing2D& dst, // using intermediary std::vector in case VectorT is not directly // writable (such as rtac::cuda::DeviceVector) std::vector bearings(metadata.nBeams); - for(unsigned int i = 0; i < bearings.size(); i++) { + for (unsigned int i = 0; i < bearings.size(); i++) { bearings[i] = (0.01 * M_PI / 180.0) * bearingData[i]; } dst.set_bearings(bearings); - //copying ping data + // copying ping data std::vector pingData(dst.size()); const uint8_t* data = data.data() + metadata.imageOffset; - if(metadata.fireMessage.flags & 0x04) { + if (metadata.fireMessage.flags & 0x04) { // gain is sent - for(unsigned int h = 0; h < dst.range_count(); h++) { + for (unsigned int h = 0; h < dst.range_count(); h++) { float gain = 1.0f * sqrt((float)((const uint32_t*)data)[0]); data += 4; - for(int w = 0; w < dst.bearing_count; w++) { + for (int w = 0; w < dst.bearing_count; w++) { pingData[dst.bearing_count()*w + h] = data[w]; } data += dst.bearing_count(); } } else { - //gain was not sent - for(int i = 0; i < pingData.size(); i++) { + // gain was not sent + for (int i = 0; i < pingData.size(); i++) { pingData[i] = data[i] / 255.0; } } diff --git a/include/oculus_driver/utils.h b/include/oculus_driver/utils.h index 76ef874..e7b780c 100644 --- a/include/oculus_driver/utils.h +++ b/include/oculus_driver/utils.h @@ -89,18 +89,18 @@ inline OculusSimpleFireMessage2 default_ping_config() inline bool check_config_feedback(const OculusSimpleFireMessage2& requested, const OculusSimpleFireMessage2& feedback) { // returns true if feedback coherent with requested config. - if(requested.pingRate == PingRateType::PingRateStandby) + if (requested.pingRate == PingRateType::PingRateStandby) { // If in standby, expecting a dummy message - if(feedback.head.msgId == OculusMessageType::MsgDummy) return true; + if (feedback.head.msgId == OculusMessageType::MsgDummy) return true; } else { // If got a simple ping result, checking relevant parameters - if(feedback.head.msgId == OculusMessageType::MsgSimplePingResult && + if (feedback.head.msgId == OculusMessageType::MsgSimplePingResult && requested.masterMode == feedback.masterMode // feedback is broken on pingRate field - //&& requested.pingRate == feedback.pingRate + // && requested.pingRate == feedback.pingRate && requested.gammaCorrection == feedback.gammaCorrection && requested.flags == feedback.flags && requested.range == feedback.range && std::abs(requested.gain - feedback.gain) < 1.0e-1) { @@ -110,13 +110,13 @@ inline bool check_config_feedback(const OculusSimpleFireMessage2& requested, con // For now simple ping is ok. Checking sound speed / salinity // parameters If speed of sound is 0.0, the sonar is using salinity // to calculate speed of sound. - if(requested.speedOfSound != 0.0) + if (requested.speedOfSound != 0.0) { - if(std::abs(requested.speedOfSound - feedback.speedOfSound) < 1.0e-1) return true; + if (std::abs(requested.speedOfSound - feedback.speedOfSound) < 1.0e-1) return true; } else { - if(std::abs(requested.salinity - feedback.salinity) < 1.0e-1) return true; + if (std::abs(requested.salinity - feedback.salinity) < 1.0e-1) return true; } } } @@ -126,16 +126,16 @@ inline bool check_config_feedback(const OculusSimpleFireMessage2& requested, con inline bool config_changed(const OculusSimpleFireMessage2& previous, const OculusSimpleFireMessage2& next) { - if(previous.masterMode != next.masterMode) return true; - if(previous.pingRate != next.pingRate) return true; - if(previous.networkSpeed != next.networkSpeed) return true; - if(previous.gammaCorrection != next.gammaCorrection) return true; - if(previous.flags != next.flags) return true; + if (previous.masterMode != next.masterMode) return true; + if (previous.pingRate != next.pingRate) return true; + if (previous.networkSpeed != next.networkSpeed) return true; + if (previous.gammaCorrection != next.gammaCorrection) return true; + if (previous.flags != next.flags) return true; - if(abs(previous.range - next.range) > 0.001) return true; - if(abs(previous.gain - next.gain) > 0.1) return true; - if(abs(previous.speedOfSound - next.speedOfSound) > 0.1) return true; - if(abs(previous.salinity - next.salinity) > 0.1) return true; + if (abs(previous.range - next.range) > 0.001) return true; + if (abs(previous.gain - next.gain) > 0.1) return true; + if (abs(previous.speedOfSound - next.speedOfSound) > 0.1) return true; + if (abs(previous.salinity - next.salinity) > 0.1) return true; return false; } @@ -147,16 +147,16 @@ bool timedCallback(eventpp::CallbackList& callbacks, auto start = std::chrono::steady_clock::now(); auto handle = eventpp::counterRemover(callbacks).append( [start, timeout_ms, &callback, &called](auto... args) { - if(std::chrono::steady_clock::now() - start < + if (std::chrono::steady_clock::now() - start < std::chrono::milliseconds(timeout_ms)) { callback(args...); called.test_and_set(); } }); - while(!called.test() || std::chrono::steady_clock::now() - start < + while (!called.test() || std::chrono::steady_clock::now() - start < std::chrono::milliseconds(timeout_ms)) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for (std::chrono::milliseconds(100)); } callbacks.remove(handle); return called.test(); diff --git a/python/src/oculus_files.cpp b/python/src/oculus_files.cpp index 089a1b7..b48ea75 100644 --- a/python/src/oculus_files.cpp +++ b/python/src/oculus_files.cpp @@ -1,9 +1,14 @@ +#include "oculus_files.h" + #include namespace py = pybind11; #include #include +#include +#include + #include "oculus_message.h" @@ -13,24 +18,25 @@ struct OculusFileReader oculus::blueprint::LogItem msgHeader_; std::vector data_; - OculusFileReader(const std::string& path) : file_(path) {} + explicit OculusFileReader(const std::string& path) : file_(path) {} const oculus::blueprint::LogHeader file_header() const { return file_.file_header(); } py::object read_next_message() const { auto msg = file_.read_next_message(); - if(msg) + if (msg) { return py::cast(msg.get()); - else + } else { return py::none(); + } } py::object read_next_ping() const { auto msg = file_.read_next_ping(); - if(msg) { + if (msg) { return py::cast(msg); - } - else + } else { return py::none(); + } } void rewind() { @@ -38,6 +44,7 @@ struct OculusFileReader } }; +// NOLINTNEXTLINE(runtime/references) void init_oculus_python_files(py::module& parentModule) { py::module m_ = parentModule.def_submodule("files", "Submodule to read .oculus files."); @@ -93,5 +100,3 @@ void init_oculus_python_files(py::module& parentModule) .def("read_next_ping", &OculusFileReader::read_next_ping) .def("rewind", &OculusFileReader::rewind); } - - diff --git a/python/src/oculus_message.cpp b/python/src/oculus_message.cpp index 64d166c..42ba7e5 100644 --- a/python/src/oculus_message.cpp +++ b/python/src/oculus_message.cpp @@ -1,3 +1,5 @@ +#include "oculus_message.h" + #include #include namespace py = pybind11; @@ -12,6 +14,7 @@ namespace py = pybind11; #include "message_utils.h" +// NOLINTNEXTLINE(runtime/references) void init_oculus_message(py::module& m_) { py::class_(m_, "OculusMessage") @@ -39,11 +42,12 @@ void init_oculus_message(py::module& m_) return make_memory_view(msg->data()); }) .def("metadata", [](const oculus::PingMessage::ConstPtr& msg) { - if(msg->message()->message_version() == 2) { - return py::cast(*reinterpret_cast(msg->data().data())); - } - else { - return py::cast(*reinterpret_cast(msg->data().data())); + if (msg->message()->message_version() == 2) { + auto data = reinterpret_cast(msg->data().data()); + return py::cast(*data); + } else { + auto data = reinterpret_cast(msg->data().data()); + return py::cast(*data); } }) @@ -66,7 +70,8 @@ void init_oculus_message(py::module& m_) return make_ping_data_view(*msg); }) .def("ping_index", &oculus::PingMessage::ping_index) - //.def("ping_firing_date", &oculus::PingMessage::ping_firing_date) // broken on hardware side ? + // ping_firing_date broken on hardware side ? + // .def("ping_firing_date", &oculus::PingMessage::ping_firing_date) .def("range", &oculus::PingMessage::range) .def("gain_percent", &oculus::PingMessage::gain_percent) .def("frequency", &oculus::PingMessage::frequency) @@ -79,7 +84,7 @@ void init_oculus_message(py::module& m_) msg->timestamp().time_since_epoch()).count(); }); - //m_.def("ping_message_from_bytes", [](py::bytes data) { + // m_.def("ping_message_from_bytes", [](py::bytes data) { m_.def("ping_message_from_bytes", [](py::bytes data, const oculus::Message::TimePoint& stamp) { @@ -88,7 +93,3 @@ void init_oculus_message(py::module& m_) (const uint8_t*)view.data()); }, py::arg("data"), py::arg("stamp") = oculus::Message::TimePoint()); } - - - - diff --git a/python/src/oculus_python.cpp b/python/src/oculus_python.cpp index 7d18078..143c1d2 100644 --- a/python/src/oculus_python.cpp +++ b/python/src/oculus_python.cpp @@ -14,43 +14,43 @@ inline void message_callback_wrapper(py::object callback, const oculus::Message: { PyGILState_STATE gstate; gstate = PyGILState_Ensure(); - + callback(py::cast(msg)); PyGILState_Release(gstate); } -inline void ping_callback_wrapper(py::object callback, +inline void ping_callback_wrapper(py::object callback, const oculus::PingMessage::ConstPtr& msg) { PyGILState_STATE gstate; gstate = PyGILState_Ensure(); - + callback(py::cast(msg)); - + PyGILState_Release(gstate); } -inline void status_callback_wrapper(py::object callback, +inline void status_callback_wrapper(py::object callback, const OculusStatusMsg& status) { PyGILState_STATE gstate; gstate = PyGILState_Ensure(); - + callback(py::cast(&status)); - + PyGILState_Release(gstate); } -inline void config_callback_wrapper(py::object callback, +inline void config_callback_wrapper(py::object callback, const OculusSimpleFireMessage& lastConfig, const OculusSimpleFireMessage& newConfig) { PyGILState_STATE gstate; gstate = PyGILState_Ensure(); - + callback(py::cast(&lastConfig), py::cast(&newConfig)); - + PyGILState_Release(gstate); } @@ -101,7 +101,7 @@ struct OculusPythonHandle } void recorder_start(const std::string& filename, bool overwrite) { - if(recorder_.is_open()) { + if (recorder_.is_open()) { return; } recorder_.open(filename, overwrite); @@ -111,7 +111,7 @@ struct OculusPythonHandle } void recorder_stop() { recorder_.close(); - if(recorderCallbackId_ > 0) { + if (recorderCallbackId_ > 0) { sonar_.remove_message_callback(recorderCallbackId_); } } @@ -239,11 +239,11 @@ PYBIND11_MODULE(_oculus_python, m_) .def_readonly("firmwareDate1", &OculusVersionInfo::firmwareDate1) .def_readonly("firmwareVersion2", &OculusVersionInfo::firmwareVersion2) .def_readonly("firmwareDate2", &OculusVersionInfo::firmwareDate2); - //.def("__str__", [](const OculusVersionInfo& version) { + // .def("__str__", [](const OculusVersionInfo& version) { // std::ostringstream oss; // oss << version; // return oss.str(); - //}); + // }); py::class_(m_, "OculusStatusMsg") .def(py::init<>()) @@ -298,7 +298,3 @@ PYBIND11_MODULE(_oculus_python, m_) init_oculus_message(m_); init_oculus_python_files(m_); } - - - - diff --git a/src/AsyncService.cpp b/src/AsyncService.cpp index ec691de..6f0d969 100644 --- a/src/AsyncService.cpp +++ b/src/AsyncService.cpp @@ -44,14 +44,14 @@ bool AsyncService::is_running() const void AsyncService::start() { - if(this->is_running()) return; + if (this->is_running()) return; std::cout << "starting" << std::endl; - if(service_->stopped()) + if (service_->stopped()) service_->reset(); thread_ = std::thread(boost::bind(&boost::asio::io_service::run, service_)); - if(!thread_.joinable()) + if (!thread_.joinable()) throw std::runtime_error("Failed to start AsyncService"); isRunning_ = true; @@ -59,13 +59,13 @@ void AsyncService::start() void AsyncService::stop() { - if(!this->is_running()) return; + if (!this->is_running()) return; std::cout << "stopping" << std::endl; service_->stop(); thread_.join(); - if(thread_.joinable()) + if (thread_.joinable()) throw std::runtime_error("Failed to stop AsyncService"); isRunning_ = false; diff --git a/src/Recorder.cpp b/src/Recorder.cpp index 227dc1f..72657a2 100644 --- a/src/Recorder.cpp +++ b/src/Recorder.cpp @@ -11,7 +11,7 @@ Recorder::Recorder() Recorder::~Recorder() { - if(this->is_open()) { + if (this->is_open()) { this->close(); } } @@ -19,7 +19,7 @@ Recorder::~Recorder() void Recorder::open(const std::string& filename, bool force) { file_.open(filename, std::ofstream::binary); - if(!file_.is_open()) { + if (!file_.is_open()) { std::ostringstream oss; oss << "Could not open file for writing : " << filename; throw std::runtime_error(oss.str()); @@ -48,7 +48,7 @@ void Recorder::close() std::size_t Recorder::write(const blueprint::LogItem& header, const uint8_t* data) const { - if(!this->is_open()) { + if (!this->is_open()) { return 0; } @@ -59,7 +59,7 @@ std::size_t Recorder::write(const blueprint::LogItem& header, std::size_t Recorder::write(const Message& message) const { - if(!this->is_open()) { + if (!this->is_open()) { return 0; } std::size_t writtenSize = 0; @@ -97,23 +97,23 @@ FileReader::FileReader(const std::string& filename) : FileReader::~FileReader() { - if(this->is_open()) { + if (this->is_open()) { this->close(); } } bool FileReader::check_file_header(const blueprint::LogHeader& header) { - if(header.fileHeader != FileMagicNumber) { + if (header.fileHeader != FileMagicNumber) { std::ostringstream oss; oss << "oculus::FileReader : invalid file header, is it a .oculus file ?.\n"; oss << " file : '" << filename_ << "'"; throw std::runtime_error(oss.str()); } - if(header.version != 1) { + if (header.version != 1) { std::cerr << "oculus::FileHeader version is != 1. Reaingd may fail" << std::endl; } - if(header.encryption != 0) { + if (header.encryption != 0) { std::ostringstream oss; oss << "oculus::FileReader : file is encrypted. Cannot decode.\n"; oss << " file : '" << filename_ << "'"; @@ -126,14 +126,14 @@ void FileReader::open(const std::string& filename) { filename_ = filename; file_.open(filename, std::ifstream::binary); - if(!file_.is_open()) { + if (!file_.is_open()) { std::ostringstream oss; oss << "Could not open file for reading : " << filename; throw std::runtime_error(oss.str()); } file_.read(reinterpret_cast(&fileHeader_), sizeof(fileHeader_)); - if(!file_) { + if (!file_) { std::ostringstream oss; oss << "oculus::FileReader : error reading file header.\n"; oss << " file : '" << filename_ << "'"; @@ -153,10 +153,10 @@ void FileReader::read_next_header() const { itemPosition_ = file_.tellg(); file_.read(reinterpret_cast(&nextItem_), sizeof(nextItem_)); - if(!file_) { + if (!file_) { std::memset(&nextItem_, 0, sizeof(nextItem_)); itemPosition_ = 0; - if(!file_.eof()) { + if (!file_.eof()) { std::ostringstream oss; oss << "oculus::FileReader : error reading next item header. "; oss << "The file may be corrupted.\n"; @@ -168,11 +168,11 @@ void FileReader::read_next_header() const std::size_t FileReader::jump_item() const { - if(nextItem_.type == 0) { + if (nextItem_.type == 0) { return 0; } - if(!file_.seekg(nextItem_.payloadSize, std::ios::cur)) { + if (!file_.seekg(nextItem_.payloadSize, std::ios::cur)) { std::memset(&nextItem_, 0, sizeof(nextItem_)); std::ostringstream oss; oss << "oculus::FileReader : error jumping to next item. File might be corrupted.\n"; @@ -187,11 +187,11 @@ std::size_t FileReader::jump_item() const std::size_t FileReader::read_next_item(uint8_t* dst) const { - if(nextItem_.type == 0) { + if (nextItem_.type == 0) { return 0; } - if(!file_.read(reinterpret_cast(dst), nextItem_.payloadSize)) { + if (!file_.read(reinterpret_cast(dst), nextItem_.payloadSize)) { std::memset(&nextItem_, 0, sizeof(nextItem_)); std::ostringstream oss; oss << "oculus::FileReader : error reading item data. File might be corrupted.\n"; @@ -207,7 +207,7 @@ std::size_t FileReader::read_next_item(uint8_t* dst) const std::size_t FileReader::read_next_item(std::vector& dst) const { - if(nextItem_.type == 0) { + if (nextItem_.type == 0) { return 0; } dst.resize(nextItem_.payloadSize); @@ -217,8 +217,8 @@ std::size_t FileReader::read_next_item(std::vector& dst) const Message::ConstPtr FileReader::read_next_message() const { // Reading file until we find a rt_oculusSonar message or enf of file - while(nextItem_.type != blueprint::rt_oculusSonar && this->jump_item()) {} - if(nextItem_.type == 0) { + while (nextItem_.type != blueprint::rt_oculusSonar && this->jump_item()) {} + if (nextItem_.type == 0) { return nullptr; } @@ -228,7 +228,7 @@ Message::ConstPtr FileReader::read_next_message() const // Reading TimeStamp from next message if it is there. Falling back to the // LogItem date if it is not there. - if(nextItem_.type == blueprint::rt_oculusSonarStamp) { + if (nextItem_.type == blueprint::rt_oculusSonarStamp) { // next message is timestamp associated with the message we just read. TimeStamp stamp; this->read_next_item(reinterpret_cast(&stamp)); @@ -244,10 +244,10 @@ Message::ConstPtr FileReader::read_next_message() const PingMessage::ConstPtr FileReader::read_next_ping() const { Message::ConstPtr msg = this->read_next_message(); - while(msg && !msg->is_ping_message()) { + while (msg && !msg->is_ping_message()) { msg = this->read_next_message(); } - if(!msg) + if (!msg) return nullptr; return PingMessage::Create(msg); } diff --git a/src/SonarClient.cpp b/src/SonarClient.cpp index cb71777..6a50366 100644 --- a/src/SonarClient.cpp +++ b/src/SonarClient.cpp @@ -53,7 +53,7 @@ size_t SonarClient::send(const boost::asio::streambuf& buffer) const { std::unique_lock lock(socketMutex_); // auto lock - if(!socket_ || !this->connected()) return 0; + if (!socket_ || !this->connected()) return 0; return socket_->send(buffer.data()); } @@ -66,13 +66,13 @@ size_t SonarClient::send(const boost::asio::streambuf& buffer) const */ void SonarClient::checker_callback(const boost::system::error_code& err) { - if(err) + if (err) { logger->error("Checker error: {}", err.message()); return; } - if(checkerTimer_.expires_at() == boost::posix_time::neg_infin) + if (checkerTimer_.expires_at() == boost::posix_time::neg_infin) { logger->error("Checker timer cancelled"); return; @@ -83,14 +83,14 @@ void SonarClient::checker_callback(const boost::system::error_code& err) this->checkerTimer_.async_wait( std::bind(&SonarClient::checker_callback, this, std::placeholders::_1)); - if(connectionState_ == Initializing || connectionState_ == Attempt) + if (connectionState_ == Initializing || connectionState_ == Attempt) { // Nothing more to be done. Waiting. return; } auto lastStatusTime = statusListener_.time_since_last_status(); - if(lastStatusTime > 5) + if (lastStatusTime > 5) { // The status is retrieved through broadcasted UDP packets. No status // means no sonar on the network -> no chance to connect. @@ -101,7 +101,7 @@ void SonarClient::checker_callback(const boost::system::error_code& err) return; } - if(this->time_since_last_message() > 10) + if (this->time_since_last_message() > 10) { // Here last status was received less than 5 seconds ago but the last // message is more than 10s old. The connection is probably broken and @@ -116,7 +116,7 @@ void SonarClient::checker_callback(const boost::system::error_code& err) void SonarClient::check_reception(const boost::system::error_code& err) { // no real handling for now - if(err) + if (err) { logger->error("Reception error : {}", err.message()); } @@ -141,14 +141,14 @@ void SonarClient::close_connection() std::unique_lock lock(socketMutex_); this->checkerTimer_.cancel(); checkerTimer_.expires_at(boost::posix_time::neg_infin); - if(socket_) + if (socket_) { logger->info("Socket open. Closing now"); try { boost::system::error_code err; socket_->shutdown(boost::asio::ip::tcp::socket::shutdown_both, err); - if(err) + if (err) { logger->error("Error closing socket : {} - {}", err.value(), err.message()); return; @@ -192,7 +192,7 @@ void SonarClient::on_first_status(const OculusStatusMsg& msg) void SonarClient::connect_callback(const boost::system::error_code& err) { - if(err) + if (err) { logger->error("Connection failure : {}. Remote: {}", err.message(), remote_.address().to_string()); @@ -219,7 +219,7 @@ void SonarClient::connect_callback(const boost::system::error_code& err) void SonarClient::initiate_receive() { std::unique_lock lock(socketMutex_); - if(!socket_) return; + if (!socket_) return; // asynchronously scan input until finding a valid header. // /!\ To be checked : This function and its callback handle the data // synchronization with the start of the ping message. It is relying on the @@ -248,7 +248,7 @@ void SonarClient::header_received_callback(const boost::system::error_code err, // TODO : check this last statement. Checked : wrong. // Other message types seem to be sent but are not documented by Oculus). this->check_reception(err); - if(receivedByteCount != sizeof(message_->header_) || !this->is_valid(message_->header_)) + if (receivedByteCount != sizeof(message_->header_) || !this->is_valid(message_->header_)) { // Either we got data in the middle of a ping or did not get enougth // bytes (end of message). Continue listening to get a valid header. @@ -263,7 +263,7 @@ void SonarClient::header_received_callback(const boost::system::error_code err, message_->update_from_header(); { std::unique_lock lock(socketMutex_); - if(!socket_) return; + if (!socket_) return; boost::asio::async_read( *socket_, boost::asio::buffer(message_->payload_handle(), @@ -276,7 +276,7 @@ void SonarClient::data_received_callback(const boost::system::error_code err, std::size_t receivedByteCount) { logger->trace("Data received callback"); - if(receivedByteCount != message_->header_.payloadSize) + if (receivedByteCount != message_->header_.payloadSize) { // We did not get enough bytes. Reinitiating reception. logger->error("Data reception error"); diff --git a/src/SonarDriver.cpp b/src/SonarDriver.cpp index 4788290..be144b2 100644 --- a/src/SonarDriver.cpp +++ b/src/SonarDriver.cpp @@ -46,7 +46,7 @@ bool SonarDriver::send_ping_config(PingConfig config) buf.sputn(reinterpret_cast(&config), sizeof(config)); auto bytesSent = this->send(buf); - if(bytesSent != sizeof(config)) + if (bytesSent != sizeof(config)) { logger->error("Could not send whole fire message({}/{})", bytesSent, sizeof(config)); @@ -62,7 +62,7 @@ bool SonarDriver::send_ping_config(PingConfig config) // Also saving the last pingRate which is not standby to be able to resume // the sonar to the last ping rate in the resume() method. - if(lastConfig_.pingRate != PingRateStandby) { + if (lastConfig_.pingRate != PingRateStandby) { lastPingRate_ = lastConfig_.pingRate; } return true; @@ -85,7 +85,7 @@ SonarDriver::PingConfig SonarDriver::current_ping_config() config.head = message->header(); }; - if(!timedCallback(messageCallbacks_, configSetter)) + if (!timedCallback(messageCallbacks_, configSetter)) { throw TimeoutReached(); } @@ -103,12 +103,12 @@ SonarDriver::PingConfig SonarDriver::request_ping_config(PingConfig request) const int maxCount = 100; // TODO: make a parameter out of this do { - if(this->send_ping_config(request)) + if (this->send_ping_config(request)) { try { feedback = this->current_ping_config(); - if(check_config_feedback(request, feedback)) break; + if (check_config_feedback(request, feedback)) break; } catch(const TimeoutReached& e) { @@ -117,9 +117,9 @@ SonarDriver::PingConfig SonarDriver::request_ping_config(PingConfig request) } } count++; - } while(count < maxCount); + } while (count < maxCount); - if(count >= maxCount) + if (count >= maxCount) { logger->error( "Could not get a proper feedback from the sonar. " @@ -169,7 +169,7 @@ void SonarDriver::handle_message(const Message::ConstPtr& message) const auto& header = message->header(); const auto& data = message->data(); auto newConfig = lastConfig_; - switch(header.msgId) + switch (header.msgId) { case MsgSimplePingResult: newConfig = reinterpret_cast(data.data())->fireMessage; @@ -181,7 +181,7 @@ void SonarDriver::handle_message(const Message::ConstPtr& message) // fireMessage in the ping results will be 40%). The gain is // rescaled here to ensure consistent parameter handling on client // side). - if(newConfig.masterMode == 2) { + if (newConfig.masterMode == 2) { newConfig.gain = (newConfig.gain - 40.0) * 100.0 / 60.0; } break; @@ -193,7 +193,7 @@ void SonarDriver::handle_message(const Message::ConstPtr& message) break; }; - if(config_changed(lastConfig_, newConfig)) { + if (config_changed(lastConfig_, newConfig)) { configCallbacks_(lastConfig_, newConfig); } lastConfig_ = newConfig; @@ -201,7 +201,7 @@ void SonarDriver::handle_message(const Message::ConstPtr& message) // Calling generic message callbacks first (in case we want to do something // before calling the specialized callbacks). messageCallbacks_(message); - switch(header.msgId) + switch (header.msgId) { case MsgSimplePingResult: pingCallbacks_(PingMessage::Create(message)); diff --git a/src/StatusListener.cpp b/src/StatusListener.cpp index bd55773..618678c 100644 --- a/src/StatusListener.cpp +++ b/src/StatusListener.cpp @@ -33,11 +33,11 @@ StatusListener::StatusListener(const IoServicePtr &service, socket_.open(boost::asio::ip::udp::v4(), err); // socket_.set_option(boost::asio::socket_base::broadcast(true)); - if(err) + if (err) throw std::runtime_error("Error opening socket"); socket_.bind(remote_); - if(err) + if (err) throw std::runtime_error("Socket remote error"); logger->info("listening to remote : {}", remote_.address().to_string()); @@ -56,13 +56,13 @@ void StatusListener::get_one_message() void StatusListener::message_callback(const boost::system::error_code& err, std::size_t bytesReceived) { - if(err) { + if (err) { logger->error("message_callback: Status reception error."); this->get_one_message(); return; } - if(bytesReceived != sizeof(OculusStatusMsg)) { + if (bytesReceived != sizeof(OculusStatusMsg)) { logger->error("message_callback: not enough bytes."); this->get_one_message(); return; diff --git a/src/print_utils.cpp b/src/print_utils.cpp index 7e739c8..53135eb 100644 --- a/src/print_utils.cpp +++ b/src/print_utils.cpp @@ -52,7 +52,7 @@ std::string to_string(DataSizeType dataType) std::string to_string(PingRateType pingRate) { - switch(pingRate) + switch (pingRate) { case PingRateNormal: return "normal (10Hz)"; @@ -168,7 +168,7 @@ std::string to_string(const OculusSimpleFireMessage2& msg, const std::string& pr // Reserved values have multiple fields. Don't need to show them. // << prefix // << "reserved :"; - // for(unsigned int i = 0; i < 8; i++) + // for (unsigned int i = 0; i < 8; i++) // { // oss << " " << msg.reserved[i]; // } diff --git a/tests/src/async_client_test.cpp b/tests/src/async_client_test.cpp index 39e37dc..fccc351 100644 --- a/tests/src/async_client_test.cpp +++ b/tests/src/async_client_test.cpp @@ -42,7 +42,7 @@ void print_dummy(const OculusMessageHeader& msg) void print_all(const Message::ConstPtr& msg) { - switch(msg->header().msgId) + switch (msg->header().msgId) { case OculusMessageType::MsgSimplePingResult: std::cout << "Got messageSimplePingResult" << endl; @@ -71,7 +71,7 @@ int main() SonarDriver sonar(ioService.io_service(), spdlog::get("console")); // sonar.add_ping_callback(&print_ping); - // sonar.add_dummy_callback(&print_dummy); + // sonar.add_dummy_callback(&print_dummy); sonar.message_callbacks().append(&print_all); ioService.start(); @@ -86,8 +86,8 @@ int main() auto config = default_ping_config(); config.pingRate = PingRateType::PingRateStandby; sonar.send_ping_config(config); - sonar.dummy_callbacks().append([](const OculusMessageHeader& header) { - std::cout << "Got awaited dummy !" << std::endl; + sonar.dummy_callbacks().append([](const OculusMessageHeader& header) { + std::cout << "Got awaited dummy !" << std::endl; }); std::cout << "After awaited dummy" << std::endl; diff --git a/tests/src/client_test.cpp b/tests/src/client_test.cpp index 8aa8890..cb80837 100644 --- a/tests/src/client_test.cpp +++ b/tests/src/client_test.cpp @@ -30,21 +30,21 @@ void print_ping(const PingMessage::ConstPtr& ping) { static unsigned int count = 0; cout << "=============== Got Ping : " << count++ << endl; - //cout << pingMetadata << endl; + // cout << pingMetadata << endl; } void print_dummy(const OculusMessageHeader& msg) { static unsigned int count = 0; cout << "=============== Got dummy : " << count++ << endl; - //cout << msg << endl; + // cout << msg << endl; } int main() { auto ioService = std::make_shared(); SonarDriver driver(ioService, spdlog::get("console")); - + driver.ping_callbacks().append(&print_ping); driver.dummy_callbacks().append(&print_dummy); diff --git a/tests/src/config_test.cpp b/tests/src/config_test.cpp index 79bbcdd..78d320f 100644 --- a/tests/src/config_test.cpp +++ b/tests/src/config_test.cpp @@ -31,30 +31,30 @@ using namespace oculus; void print_ping(const PingMessage::ConstPtr& ping) { cout << "=============== Got Ping :" << endl; - //cout << pingMetadata << endl; - //cout << pingMetadata.fireMessage.gainPercent << endl; + // cout << pingMetadata << endl; + // cout << pingMetadata.fireMessage.gainPercent << endl; } void print_dummy(const OculusMessageHeader& msg) { cout << "=============== Got dummy :" << endl; - //cout << msg << endl; + // cout << msg << endl; } int main() { - //Sonar sonar; + // Sonar sonar; AsyncService ioService; SonarDriver sonar(ioService.io_service(), spdlog::get("console")); - + sonar.ping_callbacks().append(&print_ping); sonar.dummy_callbacks().append(&print_dummy); ioService.start(); - //sonar.request_fire_config(default_fire_config()); - //sonar.request_fire_config(default_fire_config()); + // sonar.request_fire_config(default_fire_config()); + // sonar.request_fire_config(default_fire_config()); getchar(); diff --git a/tests/src/filereader_test.cpp b/tests/src/filereader_test.cpp index 5315bd5..19a9a75 100644 --- a/tests/src/filereader_test.cpp +++ b/tests/src/filereader_test.cpp @@ -7,23 +7,23 @@ using namespace oculus; int main(int argc, char** argv) { - if(argc < 2) { + if (argc < 2) { throw std::runtime_error("Must give a .oculus file as parameter"); } cout << "Opening file : " << argv[1] << endl; FileReader file(argv[1]); - //while(auto msg = file.read_next_ping()) { + // while (auto msg = file.read_next_ping()) { // cout << "Got ping item : " << msg->header() << endl; // cout << "size : " << msg->bearing_count() << 'x' << msg->range_count() << endl; - //} + // } auto msg = file.read_next_ping(); cout << "Got ping : " << msg->header() << endl; cout << "size : " << msg->bearing_count() << 'x' << msg->range_count() << endl; cout << "master mode : " << (unsigned int)msg->master_mode() << endl; cout << "bearings :\n"; auto bearingData = msg->bearing_data(); - for(int i = 0; i < msg->bearing_count(); i++) { + for (int i = 0; i < msg->bearing_count(); i++) { cout << ' ' << 0.01 * bearingData[i]; } cout << endl; diff --git a/tests/src/helpers_test.cpp b/tests/src/helpers_test.cpp index 8915e84..b6f819c 100644 --- a/tests/src/helpers_test.cpp +++ b/tests/src/helpers_test.cpp @@ -11,15 +11,13 @@ using namespace oculus; void handle_oculus_message(const std::vector& data) { auto header = *reinterpret_cast(data.data()); - if(header.msgId != MsgSimplePingResult) { + if (header.msgId != MsgSimplePingResult) { cout << "Got non-ping message" << endl; - } - else { - if(header.msgVersion != 2) { + } else { + if (header.msgVersion != 2) { cout << *reinterpret_cast(data.data()) << endl; cout << "PingResultV1" << endl; - } - else { + } else { cout << *reinterpret_cast(data.data()) << endl; cout << "PingResultV2" << endl; } @@ -28,7 +26,7 @@ void handle_oculus_message(const std::vector& data) int main(int argc, char** argv) { - if(argc < 2) { + if (argc < 2) { throw std::runtime_error("Must give a .oculus file as parameter"); } cout << "Opening file : " << argv[1] << endl; @@ -38,26 +36,26 @@ int main(int argc, char** argv) std::shared_ptr msg; do { msg = file.read_next_message(); - if(!msg) break; - if(!msg->is_ping_message()) continue; + if (!msg) break; + if (!msg->is_ping_message()) continue; count++; - } while(count < 100); + } while (count < 100); - if(!msg || !msg->is_ping_message()) { + if (!msg || !msg->is_ping_message()) { std::cerr << "Could not read an oculus ping message from " << argv[1] << std::endl; return -1; } auto bearings = get_ping_bearings(msg->data()); cout << "bearings :\n"; - for(auto b : bearings) { + for (auto b : bearings) { cout << " " << 180.0*b / M_PI; } cout << endl; auto pingData = get_ping_acoustic_data(msg->data()); auto msgHeader = msg->header(); - if(msgHeader.msgVersion != 2) { + if (msgHeader.msgVersion != 2) { auto metadata = *reinterpret_cast(msg->data().data()); write_pgm("output_polar.pgm", metadata.nBeams, metadata.nRanges, pingData.data()); cout << metadata << endl; @@ -66,8 +64,7 @@ int main(int argc, char** argv) write_pgm("output_cartesian.pgm", imageShape.first, imageShape.second, cartesianImage.data()); - } - else { + } else { auto metadata = *reinterpret_cast(msg->data().data()); cout << metadata << endl; write_pgm("output_polar.pgm", metadata.nBeams, metadata.nRanges, pingData.data()); diff --git a/tests/src/recorder_test.cpp b/tests/src/recorder_test.cpp index 24d7189..734ad96 100644 --- a/tests/src/recorder_test.cpp +++ b/tests/src/recorder_test.cpp @@ -32,18 +32,18 @@ void print_ping(const OculusSimplePingResult2& pingMetadata, const std::vector& pingData) { cout << "=============== Got Ping :" << endl; - //cout << pingMetadata << endl; + // cout << pingMetadata << endl; } void print_dummy(const OculusMessageHeader& msg) { cout << "=============== Got dummy :" << endl; - //cout << msg << endl; + // cout << msg << endl; } void print_all(const Message::ConstPtr& msg) { - switch(msg->header().msgId) { + switch (msg->header().msgId) { case MsgSimplePingResult: std::cout << "Got messageSimplePingResult" << endl; break; @@ -62,7 +62,6 @@ void print_all(const Message::ConstPtr& msg) default: break; } - } void recorder_callback(const Recorder* recorder, @@ -73,12 +72,12 @@ void recorder_callback(const Recorder* recorder, int main() { - //Sonar sonar; + // Sonar sonar; AsyncService ioService; SonarDriver sonar(ioService.io_service(), spdlog::get("console")); - - //sonar.add_ping_callback(&print_ping); - //sonar.add_dummy_callback(&print_dummy); + + // sonar.add_ping_callback(&print_ping); + // sonar.add_dummy_callback(&print_dummy); sonar.message_callbacks().append(&print_all); ioService.start(); @@ -86,7 +85,8 @@ int main() Recorder recorder; recorder.open("output.oculus", true); - sonar.message_callbacks().append(std::bind(recorder_callback, &recorder, std::placeholders::_1)); + sonar.message_callbacks().append( + std::bind(recorder_callback, &recorder, std::placeholders::_1)); getchar(); diff --git a/tests/src/status_display.cpp b/tests/src/status_display.cpp index 899e494..b524b8a 100644 --- a/tests/src/status_display.cpp +++ b/tests/src/status_display.cpp @@ -39,7 +39,7 @@ int main() StatusListener listener(ioService, spdlog::get("console")); listener.callbacks().append(&print_callback); - + ioService->run(); // is blocking return 0; diff --git a/tests/src/statusraw_test.cpp b/tests/src/statusraw_test.cpp index 3919ce9..8b29839 100644 --- a/tests/src/statusraw_test.cpp +++ b/tests/src/statusraw_test.cpp @@ -46,13 +46,13 @@ struct CallbackTest { cout << "callback2" << endl; } - + // name of function must be unique (otherwise fails at overload resolution) void callback3(int value, const OculusStatusMsg& msg) { cout << "callback3, value : " << value << endl; } - + // cannot bind this one. msg must be the last parameter void callback4(const OculusStatusMsg& msg, int value) { @@ -75,8 +75,8 @@ int main() listener.callbacks().append(std::bind(&CallbackTest::callback4, &test0, _1, 16)); // this fail at compile time - //listener.callbacks().append(&CallbackTest::callback4, &test0, 14); - + // listener.callbacks().append(&CallbackTest::callback4, &test0, 14); + ioService->run(); // is blocking return 0;