From e8f339bf030b6cbceb4bbc124664beaad9ac879e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Samuel=20Thor=C3=A9n?= Date: Mon, 20 Apr 2026 13:50:04 +0200 Subject: [PATCH] Apply formatting --- atos/common/CRSTransformation.cpp | 138 +- atos/common/CRSTransformation.hpp | 27 +- atos/common/journal.cpp | 129 +- atos/common/journal.hpp | 6 +- atos/common/loggable.hpp | 14 +- atos/common/module.cpp | 20 +- atos/common/module.hpp | 128 +- atos/common/objectconfig.cpp | 202 +- atos/common/objectconfig.hpp | 82 +- atos/common/osihandler/osi_handler.cpp | 143 +- atos/common/osihandler/osi_handler.hpp | 70 +- atos/common/regexpatterns.hpp | 9 +- .../cartesiantrajectorychannel.hpp | 40 +- atos/common/roschannels/commandchannels.hpp | 685 ++-- .../roschannels/controlsignalchannel.hpp | 34 +- .../roschannels/customcommandaction.hpp | 18 +- atos/common/roschannels/gnsspathchannel.hpp | 45 +- atos/common/roschannels/navsatfixchannel.hpp | 79 +- .../roschannels/objstatechangechannel.hpp | 34 +- atos/common/roschannels/pathchannel.hpp | 45 +- atos/common/roschannels/pointcloudchannel.hpp | 39 +- .../roschannels/remotecontrolchannels.hpp | 183 +- atos/common/roschannels/roschannel.hpp | 33 +- atos/common/roschannels/scenariochannel.hpp | 17 +- atos/common/roschannels/statechange.hpp | 34 +- atos/common/roschannels/test_channels.hpp | 33 +- atos/common/shmem/shmem.cpp | 294 +- atos/common/sockets/canhandler.cpp | 31 +- atos/common/sockets/canhandler.hpp | 24 +- atos/common/sockets/client.cpp | 41 +- atos/common/sockets/server.cpp | 91 +- atos/common/sockets/server.hpp | 7 +- atos/common/sockets/socket.cpp | 139 +- atos/common/sockets/socket.hpp | 70 +- atos/common/sockets/socketexceptions.hpp | 130 +- atos/common/sockets/tcphandler.cpp | 399 ++- atos/common/sockets/tcphandler.hpp | 158 +- atos/common/sockets/test_can.cpp | 5 +- atos/common/sockets/test_socket.cpp | 88 +- atos/common/sockets/udphandler.cpp | 332 +- atos/common/sockets/udphandler.hpp | 139 +- atos/common/testUtils/testUtils.hpp | 112 +- atos/common/tests/test_relativetrajectory.cpp | 415 ++- atos/common/trajectory.cpp | 474 ++- atos/common/trajectory.hpp | 181 +- atos/common/type.cpp | 13 +- atos/launch/launch_basic.py | 14 +- atos/launch/launch_full.py | 24 +- atos/launch/launch_graphical.py | 25 +- atos/launch/launch_integration_testing.py | 30 +- atos/launch/launch_utils/generate_cert.py | 15 +- atos/launch/launch_utils/launch_base.py | 12 +- atos/launch/launch_utils/validate_files.py | 49 +- atos/modules/BackToStart/inc/backtostart.hpp | 9 +- atos/modules/BackToStart/src/backtostart.cpp | 81 +- atos/modules/BackToStart/src/main.cpp | 3 +- atos/modules/BackToStart/tests/main.cpp | 9 +- .../BackToStart/tests/test_backtostart.cpp | 222 +- .../DirectControl/inc/directcontrol.hpp | 6 +- .../DirectControl/src/directcontrol.cpp | 188 +- atos/modules/DirectControl/src/main.cpp | 6 +- .../EsminiAdapter/inc/esminiadapter.hpp | 30 +- .../EsminiAdapter/inc/string_utility.hpp | 16 +- .../EsminiAdapter/src/esminiadapter.cpp | 353 +- atos/modules/EsminiAdapter/src/main.cpp | 5 +- .../inc/integrationtesting.hpp | 45 +- .../inc/integrationtestingfactory.hpp | 11 +- .../inc/integrationtestinghandler.hpp | 17 +- .../inc/scenarioexecution.hpp | 27 +- .../src/integrationtesting.cpp | 50 +- .../src/integrationtestingfactory.cpp | 16 +- .../src/integrationtestinghandler.cpp | 22 +- atos/modules/IntegrationTesting/src/main.cpp | 4 +- .../src/scenarioexecution.cpp | 68 +- .../JournalControl/inc/journalcontrol.hpp | 21 +- .../JournalControl/inc/journalmodel.hpp | 41 +- .../inc/journalmodelcollection.hpp | 35 +- .../JournalControl/src/journalcontrol.cpp | 54 +- .../JournalControl/src/journalmodel.cpp | 44 +- .../src/journalmodelcollection.cpp | 128 +- atos/modules/JournalControl/src/main.cpp | 10 +- atos/modules/MQTTBridge/inc/Imqtt2ros.hpp | 24 +- atos/modules/MQTTBridge/inc/Iros2mqtt.hpp | 25 +- atos/modules/MQTTBridge/inc/mqttbridge.hpp | 265 +- atos/modules/MQTTBridge/src/main.cpp | 4 +- atos/modules/MQTTBridge/src/mqttbridge.cpp | 769 +++-- atos/modules/OSIAdapter/inc/osiadapter.hpp | 70 +- atos/modules/OSIAdapter/inc/serverfactory.hpp | 36 +- atos/modules/OSIAdapter/src/main.cpp | 15 +- atos/modules/OSIAdapter/src/osiadapter.cpp | 198 +- atos/modules/OSIAdapter/src/serverfactory.cpp | 86 +- .../ObjectControl/inc/objectconnection.hpp | 26 +- .../ObjectControl/inc/objectlistener.hpp | 13 +- .../ObjectControl/inc/relativeanchor.hpp | 6 +- .../ObjectControl/inc/relativetestobject.hpp | 22 +- .../ObjectControl/src/dump_state_machine.cpp | 534 ++-- atos/modules/ObjectControl/src/main.cpp | 6 +- .../ObjectControl/src/objectconnection.cpp | 32 +- .../ObjectControl/src/objectlistener.cpp | 12 +- .../openscenariogateway.py | 16 +- .../OpenScenarioGateway/storyboard_handler.py | 4 +- .../tests/test_openscenariogateway.py | 5 +- .../tests/test_storyboard_handler.py | 2 +- .../inc/pointcloudpublisher.hpp | 30 +- atos/modules/PointcloudPublisher/src/main.cpp | 16 +- .../src/pointcloudpublisher.cpp | 39 +- atos/modules/RESTBridge/inc/restbridge.hpp | 18 +- atos/modules/RESTBridge/src/main.cpp | 12 +- .../modules/SampleModule/inc/samplemodule.hpp | 18 +- atos/modules/SampleModule/src/main.cpp | 3 +- .../modules/SampleModule/src/samplemodule.cpp | 28 +- atos/modules/SampleModule/test/main.cpp | 9 +- .../SampleModule/test/test_samplemodule.cpp | 188 +- .../SystemControl/inc/systemcontrol.hpp | 332 +- atos/modules/SystemControl/src/main.cpp | 22 +- .../SystemControl/src/systemcontrol.cpp | 2826 ++++++++--------- .../inc/trajectoryletstreamer.hpp | 18 +- .../inc/trajectorypublisher.hpp | 28 +- .../TrajectoryletStreamer/src/main.cpp | 6 +- .../src/trajectoryletstreamer.cpp | 55 +- .../src/trajectorypublisher.cpp | 96 +- atos/modules/conftest.py | 1 + atos_gui/atos_gui/configpanel/configpanel.py | 416 ++- .../atos_gui/configpanel/local_file_picker.py | 97 +- .../atos_gui/controlpanel/controlpanel.py | 150 +- atos_gui/atos_gui/main.py | 43 +- atos_gui/atos_gui/objectpanel/objectpanel.py | 142 +- atos_gui/launch/gui.py | 50 +- atos_gui/setup.py | 28 +- .../rviz2/include/object_monitor_display.hpp | 14 +- plugins/rviz2/src/object_monitor_display.cpp | 234 +- .../integration_testing/run_scenario_test.py | 37 +- .../openscenario/garage_plan_test_scenario.py | 2 - 133 files changed, 7213 insertions(+), 7035 deletions(-) diff --git a/atos/common/CRSTransformation.cpp b/atos/common/CRSTransformation.cpp index fd66e111f..68ab6688c 100644 --- a/atos/common/CRSTransformation.cpp +++ b/atos/common/CRSTransformation.cpp @@ -13,19 +13,14 @@ * @param toCRS Coordinate reference system to */ -CRSTransformation::CRSTransformation(const std::string &fromCRS, - const std::string &toCRS) - : ctxt(proj_context_create(), - [](PJ_CONTEXT *ctxt) { proj_context_destroy(ctxt); }), - projection(proj_create_crs_to_crs(ctxt.get(), fromCRS.c_str(), - toCRS.c_str(), nullptr), - [](PJ *proj) { proj_destroy(proj); }) { - if (projection == nullptr) { - throw std::logic_error( - "Failed to create CRS conversion from " + fromCRS + " to " + toCRS + - ": " + - proj_context_errno_string(ctxt.get(), proj_context_errno(ctxt.get()))); - } +CRSTransformation::CRSTransformation(const std::string& fromCRS, const std::string& toCRS) : + ctxt(proj_context_create(), [](PJ_CONTEXT* ctxt) { proj_context_destroy(ctxt); }), + projection(proj_create_crs_to_crs(ctxt.get(), fromCRS.c_str(), toCRS.c_str(), nullptr), + [](PJ* proj) { proj_destroy(proj); }) { + if (projection == nullptr) { + throw std::logic_error("Failed to create CRS conversion from " + fromCRS + " to " + toCRS + ": " + + proj_context_errno_string(ctxt.get(), proj_context_errno(ctxt.get()))); + } } /** @@ -33,42 +28,41 @@ CRSTransformation::CRSTransformation(const std::string &fromCRS, * * @param traj reference to trajectory to transform */ -void CRSTransformation::apply( - std::vector &trajPoints) { +void CRSTransformation::apply(std::vector& trajPoints) { - // Put TrajPoints into array of PJ_POINTS - PJ_COORD in[trajPoints.size()]; - auto arraySize = sizeof(in) / sizeof(in[0]); - for (int i = 0; i < arraySize; i++) { - in[i].xyz.x = trajPoints[i].getXCoord(); - in[i].xyz.y = trajPoints[i].getYCoord(); - in[i].xyz.z = trajPoints[i].getZCoord(); - } + // Put TrajPoints into array of PJ_POINTS + PJ_COORD in[trajPoints.size()]; + auto arraySize = sizeof(in) / sizeof(in[0]); + for (int i = 0; i < arraySize; i++) { + in[i].xyz.x = trajPoints[i].getXCoord(); + in[i].xyz.y = trajPoints[i].getYCoord(); + in[i].xyz.z = trajPoints[i].getZCoord(); + } - RCLCPP_DEBUG(logger, "Converting trajectory with %ld points", arraySize); - if (0 != proj_trans_array(projection.get(), PJ_FWD, arraySize, in)) { - throw std::runtime_error("Failed to convert trajectory"); - } + RCLCPP_DEBUG(logger, "Converting trajectory with %ld points", arraySize); + if (0 != proj_trans_array(projection.get(), PJ_FWD, arraySize, in)) { + throw std::runtime_error("Failed to convert trajectory"); + } - // Put transformed PJ_POINTS back into TrajPoints - for (int i = 0; i < arraySize; i++) { - if (isnan(in[i].xyz.x) || isnan(in[i].xyz.y) || isnan(in[i].xyz.z)) { - // Apply transformation to the point again. quick fix. - // TODO: Need to understand why some points are not transformed - auto point = geometry_msgs::msg::Point(); - point.x = trajPoints[i].getXCoord(); - point.y = trajPoints[i].getYCoord(); - point.z = trajPoints[i].getZCoord(); - apply(point, PJ_FWD); - in[i].xyz.x = point.x; - in[i].xyz.y = point.y; - in[i].xyz.z = point.z; - } + // Put transformed PJ_POINTS back into TrajPoints + for (int i = 0; i < arraySize; i++) { + if (isnan(in[i].xyz.x) || isnan(in[i].xyz.y) || isnan(in[i].xyz.z)) { + // Apply transformation to the point again. quick fix. + // TODO: Need to understand why some points are not transformed + auto point = geometry_msgs::msg::Point(); + point.x = trajPoints[i].getXCoord(); + point.y = trajPoints[i].getYCoord(); + point.z = trajPoints[i].getZCoord(); + apply(point, PJ_FWD); + in[i].xyz.x = point.x; + in[i].xyz.y = point.y; + in[i].xyz.z = point.z; + } - trajPoints[i].setXCoord(in[i].xyz.x); - trajPoints[i].setYCoord(in[i].xyz.y); - trajPoints[i].setZCoord(in[i].xyz.z); - } + trajPoints[i].setXCoord(in[i].xyz.x); + trajPoints[i].setYCoord(in[i].xyz.y); + trajPoints[i].setZCoord(in[i].xyz.z); + } } /** @@ -77,13 +71,12 @@ void CRSTransformation::apply( * @param point The point to transform * @param direction Which direction to transform, e.g. PJ_FWD or PJ_INV */ -void CRSTransformation::apply(geometry_msgs::msg::Point &point, - PJ_DIRECTION direction) { - PJ_COORD in = proj_coord(point.x, point.y, point.z, 0); - PJ_COORD out = proj_trans(projection.get(), direction, in); - point.x = out.xyz.x; - point.y = out.xyz.y; - point.z = out.xyz.z; +void CRSTransformation::apply(geometry_msgs::msg::Point& point, PJ_DIRECTION direction) { + PJ_COORD in = proj_coord(point.x, point.y, point.z, 0); + PJ_COORD out = proj_trans(projection.get(), direction, in); + point.x = out.xyz.x; + point.y = out.xyz.y; + point.z = out.xyz.z; } /** @@ -93,21 +86,19 @@ void CRSTransformation::apply(geometry_msgs::msg::Point &point, * @parameter: datum datum to transform to * @return std::vector Vector with lat, lon, height */ -std::vector CRSTransformation::projToLLH(const std::string &projString, - const std::string &datum) { - auto *pjSrc = - proj_create_crs_to_crs(NULL, projString.c_str(), datum.c_str(), NULL); - if (pjSrc == NULL) { - throw std::runtime_error("Failed to create projPJ object"); - } +std::vector CRSTransformation::projToLLH(const std::string& projString, const std::string& datum) { + auto* pjSrc = proj_create_crs_to_crs(NULL, projString.c_str(), datum.c_str(), NULL); + if (pjSrc == NULL) { + throw std::runtime_error("Failed to create projPJ object"); + } - PJ_COORD src = proj_coord(0, 0, 0, 0); - PJ_COORD dst = proj_trans(pjSrc, PJ_FWD, src); + PJ_COORD src = proj_coord(0, 0, 0, 0); + PJ_COORD dst = proj_trans(pjSrc, PJ_FWD, src); - std::vector result = {dst.xyz.x, dst.xyz.y, dst.xyz.z}; - proj_destroy(pjSrc); + std::vector result = {dst.xyz.x, dst.xyz.y, dst.xyz.z}; + proj_destroy(pjSrc); - return result; + return result; } /** @@ -117,15 +108,12 @@ std::vector CRSTransformation::projToLLH(const std::string &projString, * @param llh The latitude, longitude, height [degrees, degrees, meters] * @param xyzOffset Meters offset from llh [meters, meters, meters] */ -void CRSTransformation::llhOffsetMeters(double *llh, const double *xyzOffset) { - constexpr double EARTH_EQUATOR_RADIUS_M = - 6378137.0; // earth semimajor axis (WGS84) (m) - const auto [lat, lon, hgt] = std::make_tuple(llh[0], llh[1], llh[2]); - const auto [dx, dy, dz] = - std::make_tuple(xyzOffset[0], xyzOffset[1], xyzOffset[2]); +void CRSTransformation::llhOffsetMeters(double* llh, const double* xyzOffset) { + constexpr double EARTH_EQUATOR_RADIUS_M = 6378137.0; // earth semimajor axis (WGS84) (m) + const auto [lat, lon, hgt] = std::make_tuple(llh[0], llh[1], llh[2]); + const auto [dx, dy, dz] = std::make_tuple(xyzOffset[0], xyzOffset[1], xyzOffset[2]); - llh[0] = lat + (dy / EARTH_EQUATOR_RADIUS_M) * (180 / M_PI); - llh[1] = lon + (dx / EARTH_EQUATOR_RADIUS_M) * (180 / M_PI) / - std::cos(lat * M_PI / 180.0); - llh[2] = hgt + dz; -} \ No newline at end of file + llh[0] = lat + (dy / EARTH_EQUATOR_RADIUS_M) * (180 / M_PI); + llh[1] = lon + (dx / EARTH_EQUATOR_RADIUS_M) * (180 / M_PI) / std::cos(lat * M_PI / 180.0); + llh[2] = hgt + dz; +} diff --git a/atos/common/CRSTransformation.hpp b/atos/common/CRSTransformation.hpp index 8b8d842bc..c9d8feacd 100644 --- a/atos/common/CRSTransformation.hpp +++ b/atos/common/CRSTransformation.hpp @@ -5,25 +5,22 @@ */ #pragma once +#include "geometry_msgs/msg/point.hpp" #include "proj.h" -#include "trajectory.hpp" #include "rclcpp/logger.hpp" -#include "geometry_msgs/msg/point.hpp" +#include "trajectory.hpp" class CRSTransformation { - public: - - CRSTransformation(const std::string &fromCRS, const std::string &toCRS); - void apply(std::vector &traj); - void apply(geometry_msgs::msg::Point &point, PJ_DIRECTION direction); - static std::vector projToLLH(const std::string &projString, const std::string &datum); - static void llhOffsetMeters(double *llh, const double *xyzOffset); - - private: - - std::unique_ptr> ctxt; - std::unique_ptr> projection; - rclcpp::Logger logger = rclcpp::get_logger("CRSTransformation"); +public: + CRSTransformation(const std::string& fromCRS, const std::string& toCRS); + void apply(std::vector& traj); + void apply(geometry_msgs::msg::Point& point, PJ_DIRECTION direction); + static std::vector projToLLH(const std::string& projString, const std::string& datum); + static void llhOffsetMeters(double* llh, const double* xyzOffset); +private: + std::unique_ptr> ctxt; + std::unique_ptr> projection; + rclcpp::Logger logger = rclcpp::get_logger("CRSTransformation"); }; diff --git a/atos/common/journal.cpp b/atos/common/journal.cpp index b22012d45..73181a220 100644 --- a/atos/common/journal.cpp +++ b/atos/common/journal.cpp @@ -6,13 +6,13 @@ #include "journal.hpp" #include "util.h" -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include #define JOURNAL_LABEL_MAX_LENGTH 100 #define FILENAME_DATESTR_MAX_LENGTH 100 @@ -21,10 +21,10 @@ #define DELIMITER ";" #define EVENT_FLAG " " -#define DAY_LENGTH_S (24*60*60) +#define DAY_LENGTH_S (24 * 60 * 60) -using Clock = std::chrono::system_clock; -using Days = std::chrono::duration >::type>; +using Clock = std::chrono::system_clock; +using Days = std::chrono::duration>::type>; using Seconds = std::chrono::duration; template using TimePoint = std::chrono::time_point; @@ -41,7 +41,7 @@ static int endJournalEntry(FILE*); static std::string logName; int JournalInit(const char* journalName, rclcpp::Logger log) { - logName = log.get_name(); + logName = log.get_name(); journalLabel = journalName; return reinitializeJournal(); } @@ -54,28 +54,26 @@ int JournalRecordData(const JournalRecordType type, const char* format, ...) { if (fp != nullptr) { va_start(args, format); switch (type) { - case JOURNAL_RECORD_EVENT: - fprintf(fp, EVENT_FLAG); - break; - case JOURNAL_RECORD_STRING: - // TODO: print some specifier - break; - case JOURNAL_RECORD_MONITOR_DATA: - // TODO: print some specifier - // TODO: call recordMonitorData with args somehow - break; + case JOURNAL_RECORD_EVENT: + fprintf(fp, EVENT_FLAG); + break; + case JOURNAL_RECORD_STRING: + // TODO: print some specifier + break; + case JOURNAL_RECORD_MONITOR_DATA: + // TODO: print some specifier + // TODO: call recordMonitorData with args somehow + break; } vfprintf(fp, format, args); va_end(args); return endJournalEntry(fp); - } - else { + } else { RCLCPP_ERROR(rclcpp::get_logger(logName), "Unable to open journal file %s for writing", journalPath.c_str()); return -1; } } - int JournalRecordMonitorData(const ObjectDataType* objectData) { FILE* fp; char errorString[1024]; @@ -84,13 +82,17 @@ int JournalRecordMonitorData(const ObjectDataType* objectData) { checkDate(); fp = beginJournalEntry(); if (fp != nullptr) { - data->isTimestampValid ? fprintf(fp, "%.6f" DELIMITER, data->timestamp.tv_sec + data->timestamp.tv_usec / 1000000.0) - : fprintf(fp, "NaN" DELIMITER); + data->isTimestampValid + ? fprintf(fp, "%.6f" DELIMITER, data->timestamp.tv_sec + data->timestamp.tv_usec / 1000000.0) + : fprintf(fp, "NaN" DELIMITER); fprintf(fp, "%u" DELIMITER, objectData->ClientID); - fprintf(fp, "%s" DELIMITER, inet_ntop(AF_INET, &objectData->ClientIP, ipString, sizeof (ipString))); - data->position.isPositionValid ? fprintf(fp, "%.3f" DELIMITER "%.3f" DELIMITER "%.3f" DELIMITER, - data->position.xCoord_m, data->position.yCoord_m, data->position.zCoord_m) - : fprintf(fp, "NaN" DELIMITER "NaN" DELIMITER "NaN" DELIMITER); + fprintf(fp, "%s" DELIMITER, inet_ntop(AF_INET, &objectData->ClientIP, ipString, sizeof(ipString))); + data->position.isPositionValid ? fprintf(fp, + "%.3f" DELIMITER "%.3f" DELIMITER "%.3f" DELIMITER, + data->position.xCoord_m, + data->position.yCoord_m, + data->position.zCoord_m) + : fprintf(fp, "NaN" DELIMITER "NaN" DELIMITER "NaN" DELIMITER); data->position.isHeadingValid ? fprintf(fp, "%.2f" DELIMITER, data->position.heading_rad) : fprintf(fp, "NaN" DELIMITER); data->speed.isLongitudinalValid ? fprintf(fp, "%.2f" DELIMITER, data->speed.longitudinal_m_s) @@ -100,37 +102,36 @@ int JournalRecordMonitorData(const ObjectDataType* objectData) { data->acceleration.isLongitudinalValid ? fprintf(fp, "%.3f" DELIMITER, data->acceleration.longitudinal_m_s2) : fprintf(fp, "NaN" DELIMITER); data->acceleration.isLateralValid ? fprintf(fp, "%.3f" DELIMITER, data->acceleration.lateral_m_s2) - : fprintf(fp, "NaN" DELIMITER); + : fprintf(fp, "NaN" DELIMITER); switch (data->drivingDirection) { - case DriveDirectionType::OBJECT_DRIVE_DIRECTION_FORWARD: - fprintf(fp, "FWD" DELIMITER); - break; - case DriveDirectionType::OBJECT_DRIVE_DIRECTION_BACKWARD: - fprintf(fp, "REV" DELIMITER); - break; - case DriveDirectionType::OBJECT_DRIVE_DIRECTION_UNAVAILABLE: - default: - fprintf(fp, "NaN" DELIMITER); - break; + case DriveDirectionType::OBJECT_DRIVE_DIRECTION_FORWARD: + fprintf(fp, "FWD" DELIMITER); + break; + case DriveDirectionType::OBJECT_DRIVE_DIRECTION_BACKWARD: + fprintf(fp, "REV" DELIMITER); + break; + case DriveDirectionType::OBJECT_DRIVE_DIRECTION_UNAVAILABLE: + default: + fprintf(fp, "NaN" DELIMITER); + break; } fprintf(fp, "%s" DELIMITER, objectStateToASCII(data->state)); switch (data->armReadiness) { - case ObjectArmReadinessType::OBJECT_READY_TO_ARM: - fprintf(fp, "READY_TO_ARM" DELIMITER); - break; - case ObjectArmReadinessType::OBJECT_NOT_READY_TO_ARM: - fprintf(fp, "NOT_READY_TO_ARM" DELIMITER); - break; - case ObjectArmReadinessType::OBJECT_READY_TO_ARM_UNAVAILABLE: - default: - fprintf(fp, "NaN" DELIMITER); - break; + case ObjectArmReadinessType::OBJECT_READY_TO_ARM: + fprintf(fp, "READY_TO_ARM" DELIMITER); + break; + case ObjectArmReadinessType::OBJECT_NOT_READY_TO_ARM: + fprintf(fp, "NOT_READY_TO_ARM" DELIMITER); + break; + case ObjectArmReadinessType::OBJECT_READY_TO_ARM_UNAVAILABLE: + default: + fprintf(fp, "NaN" DELIMITER); + break; } - errorStatusToASCII(data->error, errorString, sizeof (errorString)); + errorStatusToASCII(data->error, errorString, sizeof(errorString)); fprintf(fp, "%s" DELIMITER, errorString); return endJournalEntry(fp); - } - else { + } else { RCLCPP_ERROR(rclcpp::get_logger(logName), "Unable to open journal file %s for writing", journalPath.c_str()); return -1; } @@ -140,27 +141,26 @@ int reinitializeJournal() { char dateStr[FILENAME_DATESTR_MAX_LENGTH]; char journalDirPath[PATH_MAX]; std::string printout; - char * cptr; + char* cptr; struct stat sb; // Get current time - auto now = Clock::now(); + auto now = Clock::now(); creationDate = std::chrono::time_point_cast(now); - auto now_t = Clock::to_time_t(now); - auto now_tm = *std::localtime(&now_t); + auto now_t = Clock::to_time_t(now); + auto now_tm = *std::localtime(&now_t); std::strftime(dateStr, FILENAME_DATESTR_MAX_LENGTH, FILENAME_DATESTR_FORMAT, &now_tm); // Form base filename from path and label - UtilGetJournalDirectoryPath(journalDirPath, sizeof (journalDirPath)); + UtilGetJournalDirectoryPath(journalDirPath, sizeof(journalDirPath)); journalPath = journalDirPath + journalLabel + "-" + dateStr + JOURNAL_FILE_ENDING; // Check if log directory exists - if(stat(journalDirPath, &sb)) { + if (stat(journalDirPath, &sb)) { // Create directory if not if (mkdir(journalDirPath, 0775) != -1) { RCLCPP_INFO(rclcpp::get_logger(logName), "Created directory %s", journalDirPath); - } - else { + } else { RCLCPP_ERROR(rclcpp::get_logger(logName), "Unable to create directory <%s>", journalDirPath); return -1; } @@ -168,7 +168,7 @@ int reinitializeJournal() { // Remove the newline at end of date string strcpy(dateStr, std::asctime(&now_tm)); - cptr = strchrnul(dateStr, '\n'); + cptr = strchrnul(dateStr, '\n'); *cptr = '\0'; // Check if journal already exists @@ -176,15 +176,14 @@ int reinitializeJournal() { RCLCPP_DEBUG(rclcpp::get_logger(logName), "Found existing journal %s", journalPath.c_str()); printout = "Opened on "; printout.append(dateStr); - } - else { + } else { RCLCPP_DEBUG(rclcpp::get_logger(logName), "No journal found, creating %s", journalPath.c_str()); printout = "Created on "; printout.append(dateStr); } // Print initialization message to journal to catch errors - if (JournalRecordData(JOURNAL_RECORD_STRING,printout.c_str()) == -1) { + if (JournalRecordData(JOURNAL_RECORD_STRING, printout.c_str()) == -1) { RCLCPP_ERROR(rclcpp::get_logger(logName), "Unable to write to file %s", journalPath.c_str()); return -1; } @@ -207,7 +206,7 @@ int checkDate() { } FILE* beginJournalEntry() { - FILE *fp = fopen(journalPath.c_str(), JOURNAL_FILE_WRITE_MODE); + FILE* fp = fopen(journalPath.c_str(), JOURNAL_FILE_WRITE_MODE); if (fp != nullptr) { auto now = std::chrono::time_point_cast(Clock::now()); fprintf(fp, "%f: ", now.time_since_epoch().count()); diff --git a/atos/common/journal.hpp b/atos/common/journal.hpp index 505bdedbb..6abc2e760 100644 --- a/atos/common/journal.hpp +++ b/atos/common/journal.hpp @@ -11,11 +11,7 @@ #include "util.h" #include -typedef enum { - JOURNAL_RECORD_MONITOR_DATA, - JOURNAL_RECORD_EVENT, - JOURNAL_RECORD_STRING -} JournalRecordType; +typedef enum { JOURNAL_RECORD_MONITOR_DATA, JOURNAL_RECORD_EVENT, JOURNAL_RECORD_STRING } JournalRecordType; int JournalInit(const char* name, rclcpp::Logger logger); int JournalRecordData(JournalRecordType type, const char* format, ...); diff --git a/atos/common/loggable.hpp b/atos/common/loggable.hpp index 3676a8322..e46a997af 100644 --- a/atos/common/loggable.hpp +++ b/atos/common/loggable.hpp @@ -8,10 +8,12 @@ class Loggable { public: - Loggable(rclcpp::Logger lg) : logger(lg) {} - rclcpp::Logger get_logger() const { - return logger; - } + Loggable(rclcpp::Logger lg) : + logger(lg) {} + rclcpp::Logger get_logger() const { + return logger; + } + protected: - rclcpp::Logger logger; -}; \ No newline at end of file + rclcpp::Logger logger; +}; diff --git a/atos/common/module.cpp b/atos/common/module.cpp index 2db7ef35c..f92d2326a 100644 --- a/atos/common/module.cpp +++ b/atos/common/module.cpp @@ -9,28 +9,26 @@ using namespace std_msgs::msg; using namespace std_srvs::srv; -bool Module::shouldExit(){ - return this->quit; +bool Module::shouldExit() { + return this->quit; } -void Module::onExitMessage(const Empty::SharedPtr){ - this->quit=true; +void Module::onExitMessage(const Empty::SharedPtr) { + this->quit = true; } /*! - * \brief A try/catch wrapper that logs messages + * \brief A try/catch wrapper that logs messages * \param tryExecute function to execute * \param executeIfFail if executing tryExecute fails, this function is executed * \param topic The topic to print. * \param logger The logger to use. * \return true if the initialization was successful, false otherwise */ -void Module::tryHandleMessage( - std::function tryExecute, - std::function executeIfFail, - const std::string& topic, - const rclcpp::Logger& logger) -{ +void Module::tryHandleMessage(std::function tryExecute, + std::function executeIfFail, + const std::string& topic, + const rclcpp::Logger& logger) { try { RCLCPP_DEBUG(logger, "Handling command on %s", topic.c_str()); tryExecute(); diff --git a/atos/common/module.hpp b/atos/common/module.hpp index 0a648ebba..840d964f1 100644 --- a/atos/common/module.hpp +++ b/atos/common/module.hpp @@ -8,27 +8,27 @@ #include #include -#include #include "roschannels/commandchannels.hpp" +#include namespace ServiceNames { -const std::string initDataDict = "init_data_dictionary"; -const std::string getObjectIds = "get_object_ids"; -const std::string getObjectIp = "get_object_ip"; -const std::string getTestOrigin = "get_test_origin"; -const std::string getObjectTrajectory = "get_object_trajectory"; -const std::string getObjectTriggerStart = "get_object_trigger_start"; -const std::string getObjectControlState = "get_object_control_state"; +const std::string initDataDict = "init_data_dictionary"; +const std::string getObjectIds = "get_object_ids"; +const std::string getObjectIp = "get_object_ip"; +const std::string getTestOrigin = "get_test_origin"; +const std::string getObjectTrajectory = "get_object_trajectory"; +const std::string getObjectTriggerStart = "get_object_trigger_start"; +const std::string getObjectControlState = "get_object_control_state"; const std::string getObjectReturnTrajectory = "get_object_return_trajectory"; -const std::string getOpenScenarioFilePath = "get_open_scenario_file_path"; -} +const std::string getOpenScenarioFilePath = "get_open_scenario_file_path"; +} // namespace ServiceNames // TODO move somewhere else? also make generic to allow more args (variadic template)? /*! * \brief Facilitates one-line intialization * of a ros message with one argument. */ -template +template Msg_T msgCtr1(MsgData_T data) { Msg_T ret; ret.data = data; @@ -43,51 +43,51 @@ Msg_T msgCtr1(MsgData_T data) { * \param name name of the module. Is passed to the ROS node. */ class Module : public rclcpp::Node { - public: - Module(const std::string name) : Node(name), getStatusResponsePub(*this) {}; +public: + Module(const std::string name) : + Node(name), + getStatusResponsePub(*this) {}; Module() = default; bool shouldExit(); - protected: - bool quit=false; +protected: + bool quit = false; ROSChannels::GetStatusResponse::Pub getStatusResponsePub; - virtual void onFailureMessage(const ROSChannels::Failure::message_type::SharedPtr){}; - virtual void onGetStatusResponse(const ROSChannels::GetStatusResponse::message_type::SharedPtr){}; + virtual void onFailureMessage(const ROSChannels::Failure::message_type::SharedPtr) {}; + virtual void onGetStatusResponse(const ROSChannels::GetStatusResponse::message_type::SharedPtr) {}; virtual void onGetStatusMessage(const ROSChannels::GetStatus::message_type::SharedPtr) { auto msg = std_msgs::msg::String(); msg.data = this->get_name(); getStatusResponsePub.publish(msg); }; - virtual void onInitMessage(const ROSChannels::Init::message_type::SharedPtr){}; - virtual void onConnectMessage(const ROSChannels::Connect::message_type::SharedPtr){}; - virtual void onDisconnectMessage(const ROSChannels::Disconnect::message_type::SharedPtr){}; - virtual void onArmMessage(const ROSChannels::Arm::message_type::SharedPtr){}; - virtual void onDisarmMessage(const ROSChannels::Disarm::message_type::SharedPtr){}; - virtual void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr){}; - virtual void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr){}; - virtual void onStartMessage(const ROSChannels::Start::message_type::SharedPtr){}; - virtual void onStartObjectMessage(const ROSChannels::StartObject::message_type::SharedPtr){}; - virtual void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr){}; - virtual void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr){}; - virtual void onReplayMessage(const ROSChannels::Replay::message_type::SharedPtr){}; + virtual void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) {}; + virtual void onConnectMessage(const ROSChannels::Connect::message_type::SharedPtr) {}; + virtual void onDisconnectMessage(const ROSChannels::Disconnect::message_type::SharedPtr) {}; + virtual void onArmMessage(const ROSChannels::Arm::message_type::SharedPtr) {}; + virtual void onDisarmMessage(const ROSChannels::Disarm::message_type::SharedPtr) {}; + virtual void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr) {}; + virtual void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr) {}; + virtual void onStartMessage(const ROSChannels::Start::message_type::SharedPtr) {}; + virtual void onStartObjectMessage(const ROSChannels::StartObject::message_type::SharedPtr) {}; + virtual void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr) {}; + virtual void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr) {}; + virtual void onReplayMessage(const ROSChannels::Replay::message_type::SharedPtr) {}; virtual void onExitMessage(const ROSChannels::Exit::message_type::SharedPtr); - virtual void onResetTestObjectsMessage(const ROSChannels::ResetTestObjects::message_type::SharedPtr){}; - virtual void onReloadObjectSettingsMessage(const ROSChannels::ReloadObjectSettings::message_type::SharedPtr){}; - + virtual void onResetTestObjectsMessage(const ROSChannels::ResetTestObjects::message_type::SharedPtr) {}; + virtual void onReloadObjectSettingsMessage(const ROSChannels::ReloadObjectSettings::message_type::SharedPtr) {}; static void tryHandleMessage(std::function tryExecute, std::function executeIfFail, const std::string& topic, const rclcpp::Logger& logger); - /** * @brief This helper function performs a service call given a client and yields a response. This * function is used when you want to specify the request instead of sending an empty request. - * + * * @tparam Srv Srv The name of the service to request. * @param timeout The timeout for the service call. * @param client The client to use to request the service. @@ -95,12 +95,11 @@ class Module : public rclcpp::Node { * @param request The request of the service, with the data to be sent. Defaults to an empty request. * @return The response of the service. */ - template - bool callService( const std::chrono::duration< double > &timeout, - std::shared_ptr> &client, - std::shared_ptr &response, - std::shared_ptr request = std::make_shared()) -{ + template + bool callService(const std::chrono::duration& timeout, + std::shared_ptr>& client, + std::shared_ptr& response, + std::shared_ptr request = std::make_shared()) { auto promise = client->async_send_request(request); if (rclcpp::spin_until_future_complete(get_node_base_interface(), promise, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -113,17 +112,16 @@ class Module : public rclcpp::Node { } /*! \brief This helper function waits for a service to become available and returns a client. - * \tparam Srv The name of the service to request. - * \param n The number of times to retry. - * \param timeout The timeout to wait for the service. - * \param serviceName The name of the service to request. - * \return The response of the service. - */ - template + * \tparam Srv The name of the service to request. + * \param n The number of times to retry. + * \param timeout The timeout to wait for the service. + * \param serviceName The name of the service to request. + * \return The response of the service. + */ + template typename std::shared_ptr> nTimesWaitForService(int n, - const std::chrono::duration< double > &timeout, - const std::string &serviceName) -{ + const std::chrono::duration& timeout, + const std::string& serviceName) { int retries = 0; auto client = create_client(serviceName); @@ -140,25 +138,23 @@ class Module : public rclcpp::Node { throw std::runtime_error("Failed to initialize service " + serviceName); } - /*! \brief This helper function is used by rosnodes to request a service. - * \tparam Srv The name of the service to request. - * \param n Number of times to retry until accepting the service is not available. - * \param timeout The timeout to wait for the service. - * \param serviceName The name of the service to request. - * \param response Response of the service, to be returned. - * \return The response of the service. - */ - template - bool nShotServiceRequest(int n, - const std::chrono::duration< double > &timeout, - const std::string &serviceName, - std::shared_ptr &response) { + * \tparam Srv The name of the service to request. + * \param n Number of times to retry until accepting the service is not available. + * \param timeout The timeout to wait for the service. + * \param serviceName The name of the service to request. + * \param response Response of the service, to be returned. + * \return The response of the service. + */ + template + bool nShotServiceRequest(int n, + const std::chrono::duration& timeout, + const std::string& serviceName, + std::shared_ptr& response) { std::shared_ptr> client; - try{ + try { client = nTimesWaitForService(n, timeout, serviceName); - } - catch (std::runtime_error &e){ + } catch (std::runtime_error& e) { RCLCPP_ERROR(get_logger(), "Failed to initialize service %s", serviceName.c_str()); return false; } diff --git a/atos/common/objectconfig.cpp b/atos/common/objectconfig.cpp index 50cdcca2c..4346d632b 100644 --- a/atos/common/objectconfig.cpp +++ b/atos/common/objectconfig.cpp @@ -7,7 +7,9 @@ #include "util.h" #include -ObjectConfig::ObjectConfig(rclcpp::Logger lg) : Loggable(lg), trajectory(lg) { +ObjectConfig::ObjectConfig(rclcpp::Logger lg) : + Loggable(lg), + trajectory(lg) { origin.latitude_deg = origin.longitude_deg = origin.altitude_m = 0.0; origin.isLongitudeValid = origin.isLatitudeValid = origin.isAltitudeValid = false; } @@ -27,48 +29,42 @@ ObjectConfig::ObjectConfig(const ObjectConfig&& other) : */ std::string ObjectConfig::toString() const { char ipAddr[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &remoteIP, ipAddr, sizeof (ipAddr)); + inet_ntop(AF_INET, &remoteIP, ipAddr, sizeof(ipAddr)); std::string retval = ""; std::string idsString; - for(auto id: injectionMap.sourceIDs) { + for (auto id : injectionMap.sourceIDs) { idsString += std::to_string(id) + " "; } - retval += "\n Object ID: " + std::to_string(transmitterID) - + "\n IP: " + ipAddr + "\n Trajectory: " + trajectory.name.c_str() - + "\n OpenDRIVE: " + opendriveFile.filename().string().c_str() - + "\n OpenSCENARIO: " + openscenarioFile.filename().string().c_str() - + "\n Turning diameter: " + std::to_string(turningDiameter) + "\n Max speed: " + std::to_string(maximumSpeed) - + "\n Object file: " + objectFile.filename().string() + "\n Anchor: " + (isAnchorObject? "Yes":"No") - + "\n OSI compatible: " + (isOSICompatible? "Yes":"No") - + "\n Injection IDs: " + idsString; + retval += + "\n Object ID: " + std::to_string(transmitterID) + "\n IP: " + ipAddr + + "\n Trajectory: " + trajectory.name.c_str() + "\n OpenDRIVE: " + opendriveFile.filename().string().c_str() + + "\n OpenSCENARIO: " + openscenarioFile.filename().string().c_str() + + "\n Turning diameter: " + std::to_string(turningDiameter) + "\n Max speed: " + std::to_string(maximumSpeed) + + "\n Object file: " + objectFile.filename().string() + "\n Anchor: " + (isAnchorObject ? "Yes" : "No") + + "\n OSI compatible: " + (isOSICompatible ? "Yes" : "No") + "\n Injection IDs: " + idsString; return retval; } -void ObjectConfig::parseObjectIdFromConfigurationFile(const fs::path& objectFile){ +void ObjectConfig::parseObjectIdFromConfigurationFile(const fs::path& objectFile) { char setting[100]; // Get ID setting - if (UtilGetObjectFileSetting(OBJECT_SETTING_ID, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof (setting)) == -1) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_ID, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) == -1) { throw std::invalid_argument("Cannot find ID setting in file " + objectFile.string()); } - char *endptr; + char* endptr; uint32_t id = static_cast(strtoul(setting, &endptr, 10)); if (endptr == setting) { - throw std::invalid_argument("ID " + std::string(setting) + " in file " - + objectFile.string() + " is invalid"); + throw std::invalid_argument("ID " + std::string(setting) + " in file " + objectFile.string() + " is invalid"); } this->transmitterID = id; } - - -void ObjectConfig::parseConfigurationFile( - const fs::path &objectFile) { +void ObjectConfig::parseConfigurationFile(const fs::path& objectFile) { char setting[100]; int result; @@ -76,187 +72,178 @@ void ObjectConfig::parseConfigurationFile( char odrDirPath[MAX_FILE_PATH]; char oscDirPath[MAX_FILE_PATH]; - UtilGetTrajDirectoryPath(trajDirPath, sizeof (trajDirPath)); - UtilGetOdrDirectoryPath(odrDirPath, sizeof (odrDirPath)); - UtilGetOscDirectoryPath(oscDirPath, sizeof (oscDirPath)); - + UtilGetTrajDirectoryPath(trajDirPath, sizeof(trajDirPath)); + UtilGetOdrDirectoryPath(odrDirPath, sizeof(odrDirPath)); + UtilGetOscDirectoryPath(oscDirPath, sizeof(oscDirPath)); // Get IP setting - if (UtilGetObjectFileSetting(OBJECT_SETTING_IP, objectFile.c_str(), - objectFile.string().length()+1, setting, - sizeof (setting)) == -1) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_IP, objectFile.c_str(), objectFile.string().length() + 1, setting, sizeof(setting)) == -1) { throw std::invalid_argument("Cannot find IP setting in file " + objectFile.string()); } result = inet_pton(AF_INET, setting, &this->remoteIP); if (result == -1) { using namespace std; throw system_error(make_error_code(static_cast(errno))); - } - else if (result == 0) { - throw std::invalid_argument("Address " + std::string(setting) - + " in object file " + objectFile.string() - + " is not a valid IPv4 address"); + } else if (result == 0) { + throw std::invalid_argument("Address " + std::string(setting) + " in object file " + objectFile.string() + + " is not a valid IPv4 address"); } // Get ID setting - if (UtilGetObjectFileSetting(OBJECT_SETTING_ID, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof (setting)) == -1) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_ID, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) == -1) { throw std::invalid_argument("Cannot find ID setting in file " + objectFile.string()); } - char *endptr; + char* endptr; uint32_t id = static_cast(strtoul(setting, &endptr, 10)); if (endptr == setting) { - throw std::invalid_argument("ID " + std::string(setting) + " in file " - + objectFile.string() + " is invalid"); + throw std::invalid_argument("ID " + std::string(setting) + " in file " + objectFile.string() + " is invalid"); } this->transmitterID = id; // Get anchor setting - if (UtilGetObjectFileSetting(OBJECT_SETTING_IS_ANCHOR, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof (setting)) == -1) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_IS_ANCHOR, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) == -1) { this->isAnchorObject = false; - } - else { + } else { this->isAnchorObject = this->isSettingTrue(setting); } // Get trajectory file setting - if (UtilGetObjectFileSetting(OBJECT_SETTING_TRAJ, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof (setting)) == 0) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_TRAJ, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) == 0) { fs::path trajFile(std::string(trajDirPath) + std::string(setting)); if (!fs::exists(trajFile.string())) { - throw std::invalid_argument("Configured trajectory file " + std::string(setting) - + " in file " + objectFile.string() + " not found"); + throw std::invalid_argument("Configured trajectory file " + std::string(setting) + " in file " + + objectFile.string() + " not found"); } this->trajectoryFile = trajFile; this->trajectory.initializeFromFile(setting); RCLCPP_DEBUG(get_logger(), "Loaded trajectory with %lu points", trajectory.points.size()); } - + // Get opendrive file setting - if (UtilGetObjectFileSetting(OBJECT_SETTING_OPENDRIVE, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof(setting)) == 0) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_OPENDRIVE, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) == 0) { fs::path odrFile(std::string(odrDirPath) + std::string(setting)); if (!fs::exists(odrFile.string())) { - throw std::invalid_argument("Configured OpenDRIVE file " + std::string(setting) - + " in file " + objectFile.string() + " not found"); + throw std::invalid_argument("Configured OpenDRIVE file " + std::string(setting) + " in file " + + objectFile.string() + " not found"); } this->opendriveFile = odrFile; } - + // Get openscenario file setting - if (UtilGetObjectFileSetting(OBJECT_SETTING_OPENSCENARIO, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof(setting)) == 0) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_OPENSCENARIO, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) == + 0) { fs::path oscFile(std::string(oscDirPath) + std::string(setting)); if (!fs::exists(oscFile.string())) { - throw std::invalid_argument("Configured OpenSCENARIO file " + std::string(setting) - + " in file " + objectFile.string() + " not found"); + throw std::invalid_argument("Configured OpenSCENARIO file " + std::string(setting) + " in file " + + objectFile.string() + " not found"); } this->openscenarioFile = oscFile; } // Get origin settings this->origin = {}; - if (UtilGetObjectFileSetting(OBJECT_SETTING_ORIGIN_LATITUDE, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof (setting)) != -1) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_ORIGIN_LATITUDE, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) != + -1) { origin.latitude_deg = strtod(setting, &endptr); if (setting != endptr) { origin.isLatitudeValid = true; } } - if (UtilGetObjectFileSetting(OBJECT_SETTING_ORIGIN_LONGITUDE, objectFile.c_str(), + if (UtilGetObjectFileSetting(OBJECT_SETTING_ORIGIN_LONGITUDE, + objectFile.c_str(), objectFile.string().length(), - setting, sizeof (setting)) != -1) { + setting, + sizeof(setting)) != -1) { origin.longitude_deg = strtod(setting, &endptr); if (setting != endptr) { origin.isLongitudeValid = true; } } - if (UtilGetObjectFileSetting(OBJECT_SETTING_ORIGIN_ALTITUDE, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof (setting)) != -1) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_ORIGIN_ALTITUDE, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) != + -1) { origin.altitude_m = strtod(setting, &endptr); if (setting != endptr) { origin.isAltitudeValid = true; } } - if (origin.isAltitudeValid == origin.isLatitudeValid - && origin.isLatitudeValid == origin.isLongitudeValid) { + if (origin.isAltitudeValid == origin.isLatitudeValid && origin.isLatitudeValid == origin.isLongitudeValid) { if (!origin.isAltitudeValid) { GeoPositionType orig; if (UtilReadOriginConfiguration(&orig) != -1) { - this->origin.latitude_deg = orig.Latitude; - this->origin.longitude_deg = orig.Longitude; - this->origin.altitude_m = orig.Altitude; - this->origin.isLatitudeValid = true; + this->origin.latitude_deg = orig.Latitude; + this->origin.longitude_deg = orig.Longitude; + this->origin.altitude_m = orig.Altitude; + this->origin.isLatitudeValid = true; this->origin.isLongitudeValid = true; - this->origin.isAltitudeValid = true; - } - else { + this->origin.isAltitudeValid = true; + } else { throw std::invalid_argument("No origin setting found"); } } - } - else { + } else { throw std::invalid_argument("Partial origin setting in file " + objectFile.string()); } // Get Turning diameter - if (UtilGetObjectFileSetting(OBJECT_SETTING_TURNING_DIAMETER, objectFile.c_str(), + if (UtilGetObjectFileSetting(OBJECT_SETTING_TURNING_DIAMETER, + objectFile.c_str(), objectFile.string().length(), - setting, sizeof (setting)) != -1) { - char *endptr; + setting, + sizeof(setting)) != -1) { + char* endptr; double val = strtod(setting, &endptr); if (endptr == setting) { - throw std::invalid_argument("Turning diameter " + std::string(setting) + " in file " - + objectFile.string() + " is invalid"); + throw std::invalid_argument("Turning diameter " + std::string(setting) + " in file " + objectFile.string() + + " is invalid"); } this->turningDiameterKnown = true; - this->turningDiameter = val; + this->turningDiameter = val; } // Get Maximum speed - if (UtilGetObjectFileSetting(OBJECT_SETTING_MAX_SPEED, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof (setting)) != -1) { - char *endptr; + if (UtilGetObjectFileSetting( + OBJECT_SETTING_MAX_SPEED, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) != -1) { + char* endptr; double val = strtod(setting, &endptr); if (endptr == setting) { - throw std::invalid_argument("Max speed " + std::string(setting) + " in file " - + objectFile.string() + " is invalid"); + throw std::invalid_argument("Max speed " + std::string(setting) + " in file " + objectFile.string() + + " is invalid"); } this->hasMaximumSpeed = true; - this->maximumSpeed = val; + this->maximumSpeed = val; } // Get OSI compatibility - if (UtilGetObjectFileSetting(OBJECT_SETTING_IS_OSI_COMPATIBLE, objectFile.c_str(), + if (UtilGetObjectFileSetting(OBJECT_SETTING_IS_OSI_COMPATIBLE, + objectFile.c_str(), objectFile.string().length(), - setting, sizeof (setting)) == -1) { + setting, + sizeof(setting)) == -1) { this->isOSICompatible = false; - } - else { + } else { this->isOSICompatible = this->isSettingTrue(setting); } // Get Injector IDs - if (UtilGetObjectFileSetting(OBJECT_SETTING_INJECTOR_IDS, objectFile.c_str(), - objectFile.string().length(), - setting, sizeof (setting)) != -1) { + if (UtilGetObjectFileSetting( + OBJECT_SETTING_INJECTOR_IDS, objectFile.c_str(), objectFile.string().length(), setting, sizeof(setting)) != + -1) { std::vector ids; std::string settingString(setting); this->split(settingString, ',', ids); @@ -276,25 +263,22 @@ template bool ObjectConfig::isSettingTrue(char (&setting)[N]) { if (setting[0] == '1' || setting[0] == '0') { return setting[0] == '1'; - } - else { + } else { std::string settingString(setting); - for (char &c : settingString) { + for (char& c : settingString) { c = std::tolower(c, std::locale()); } if (settingString.compare("true") == 0) { return true; - } - else if (settingString.compare("false") == 0) { + } else if (settingString.compare("false") == 0) { return false; - } - else { + } else { throw std::invalid_argument("Setting " + settingString + " is invalid"); } } } -void ObjectConfig::split(std::string &str, char delim, std::vector &out) { +void ObjectConfig::split(std::string& str, char delim, std::vector& out) { size_t start; size_t end = 0; diff --git a/atos/common/objectconfig.hpp b/atos/common/objectconfig.hpp index edacde458..90867b096 100644 --- a/atos/common/objectconfig.hpp +++ b/atos/common/objectconfig.hpp @@ -5,10 +5,10 @@ */ #pragma once +#include "loggable.hpp" +#include "trajectory.hpp" #include #include -#include "trajectory.hpp" -#include "loggable.hpp" // GCC version 8.1 brings non-experimental support for std::filesystem #if __GNUC__ > 8 || (__GNUC__ == 8 && __GNUC_MINOR__ >= 1) @@ -28,28 +28,60 @@ struct DataInjectionMap { class ObjectConfig : public Loggable { public: ObjectConfig(rclcpp::Logger); - //ObjectConfig(const ObjectConfig&&); + // ObjectConfig(const ObjectConfig&&); void parseObjectIdFromConfigurationFile(const fs::path& file); void parseConfigurationFile(const fs::path& file); - bool isAnchor() const { return isAnchorObject; } - bool isOSI() const { return isOSICompatible; } - DataInjectionMap getInjectionMap() const { return injectionMap; } - in_addr_t getIP(void) const { return remoteIP; } - double getMaximumSpeed() const { return maximumSpeed; } - GeographicPositionType getOrigin() const { return origin; } + bool isAnchor() const { + return isAnchorObject; + } + bool isOSI() const { + return isOSICompatible; + } + DataInjectionMap getInjectionMap() const { + return injectionMap; + } + in_addr_t getIP(void) const { + return remoteIP; + } + double getMaximumSpeed() const { + return maximumSpeed; + } + GeographicPositionType getOrigin() const { + return origin; + } std::string getProjString() const; - ATOS::Trajectory getTrajectory() const { return trajectory; } - void setTrajectory(const ATOS::Trajectory& newTraj) { trajectory = newTraj; } - uint32_t getTransmitterID() const { return transmitterID; } - void setTransmitterID(const uint32_t id) { transmitterID = id; } - double getTurningDiameter() const { return turningDiameter; } - std::string getObjectFileName() const { return objectFile.filename().string(); } - std::string getTrajectoryFileName() const { return trajectoryFile.filename().string(); } - void addInjectionTarget(const uint32_t target) { this->injectionMap.targetIDs.insert(target); } - void clearInjectionSources() { this->injectionMap.sourceIDs.clear(); } - void setOrigin(const GeographicPositionType& origin) { this->origin = origin; } + ATOS::Trajectory getTrajectory() const { + return trajectory; + } + void setTrajectory(const ATOS::Trajectory& newTraj) { + trajectory = newTraj; + } + uint32_t getTransmitterID() const { + return transmitterID; + } + void setTransmitterID(const uint32_t id) { + transmitterID = id; + } + double getTurningDiameter() const { + return turningDiameter; + } + std::string getObjectFileName() const { + return objectFile.filename().string(); + } + std::string getTrajectoryFileName() const { + return trajectoryFile.filename().string(); + } + void addInjectionTarget(const uint32_t target) { + this->injectionMap.targetIDs.insert(target); + } + void clearInjectionSources() { + this->injectionMap.sourceIDs.clear(); + } + void setOrigin(const GeographicPositionType& origin) { + this->origin = origin; + } std::string toString() const; @@ -58,19 +90,19 @@ class ObjectConfig : public Loggable { fs::path trajectoryFile; fs::path opendriveFile; fs::path openscenarioFile; - bool isAnchorObject = false; + bool isAnchorObject = false; bool isOSICompatible = false; - in_addr_t remoteIP = 0; - double maximumSpeed = 0; + in_addr_t remoteIP = 0; + double maximumSpeed = 0; bool hasMaximumSpeed = false; GeographicPositionType origin; ATOS::Trajectory trajectory; - uint32_t transmitterID = 0; + uint32_t transmitterID = 0; bool turningDiameterKnown = false; - double turningDiameter = 0; + double turningDiameter = 0; DataInjectionMap injectionMap; template bool isSettingTrue(char (&setting)[N]); - void split(std::string &str, char delim, std::vector& out); + void split(std::string& str, char delim, std::vector& out); }; diff --git a/atos/common/osihandler/osi_handler.cpp b/atos/common/osihandler/osi_handler.cpp index 2e46f929c..20137b501 100644 --- a/atos/common/osihandler/osi_handler.cpp +++ b/atos/common/osihandler/osi_handler.cpp @@ -12,72 +12,65 @@ */ #include "osi_handler.hpp" +#include +#include #include -#include #include -#include -#include +#include -void OsiHandler::decodeSdMessage( - const std::vector& msg, - const int msgSize, - const bool debug) { +void OsiHandler::decodeSdMessage(const std::vector& msg, const int msgSize, const bool debug) { - std::string data(msg.begin(),msg.begin()+msgSize); + std::string data(msg.begin(), msg.begin() + msgSize); if (Sd.ParseFromString(data)) { - + if (debug) { using std::cout, std::endl; cout << "Decoder debug:" << endl; cout << Sd.DebugString() << endl; } - } - else { + } else { throw std::invalid_argument("Couldn't decode message!"); } } +void OsiHandler::decodeSvGtMessage(const std::vector& msg, + const int msgSize, + std::vector& retval, + std::string& projStr, + const bool debug) { -void OsiHandler::decodeSvGtMessage( - const std::vector& msg, - const int msgSize, - std::vector& retval, - std::string& projStr, - const bool debug) { - - std::string data(msg.begin(),msg.begin()+msgSize); + std::string data(msg.begin(), msg.begin() + msgSize); if (Sv.ParseFromString(data)) { if (Sv.has_global_ground_truth()) { - int seconds = Sv.global_ground_truth().timestamp().seconds(); - int nanos = Sv.global_ground_truth().timestamp().nanos(); - float timestamp = seconds + nanos / pow(10,9); + int seconds = Sv.global_ground_truth().timestamp().seconds(); + int nanos = Sv.global_ground_truth().timestamp().nanos(); + float timestamp = seconds + nanos / pow(10, 9); std::cout << "Simulation timestamp: " << timestamp << std::endl; int no_of_mov_obj = Sv.global_ground_truth().moving_object_size(); - for (int i=0; i< no_of_mov_obj; i++) { + for (int i = 0; i < no_of_mov_obj; i++) { osi3::MovingObject obj = Sv.global_ground_truth().moving_object(i); OsiHandler::GlobalObjectGroundTruth_t ret; - ret.id = obj.id().value(); - ret.pos_m.x = obj.base().position().x(); - ret.pos_m.y = obj.base().position().y(); - ret.pos_m.z = obj.base().position().z(); - ret.vel_m_s.x = obj.base().velocity().x(); - ret.vel_m_s.y = obj.base().velocity().y(); - ret.vel_m_s.z = obj.base().velocity().z(); - ret.acc_m_s2.x = obj.base().acceleration().x(); - ret.acc_m_s2.y = obj.base().acceleration().y(); - ret.acc_m_s2.z = obj.base().acceleration().z(); - ret.orientation_rad.yaw = obj.base().orientation().yaw(); + ret.id = obj.id().value(); + ret.pos_m.x = obj.base().position().x(); + ret.pos_m.y = obj.base().position().y(); + ret.pos_m.z = obj.base().position().z(); + ret.vel_m_s.x = obj.base().velocity().x(); + ret.vel_m_s.y = obj.base().velocity().y(); + ret.vel_m_s.z = obj.base().velocity().z(); + ret.acc_m_s2.x = obj.base().acceleration().x(); + ret.acc_m_s2.y = obj.base().acceleration().y(); + ret.acc_m_s2.z = obj.base().acceleration().z(); + ret.orientation_rad.yaw = obj.base().orientation().yaw(); ret.orientation_rad.pitch = obj.base().orientation().pitch(); - ret.orientation_rad.roll = obj.base().orientation().roll(); + ret.orientation_rad.roll = obj.base().orientation().roll(); - //Check if ID exists in objects ##TEMP - if(std::any_of(retval.begin(),retval.end(), [&](const OsiHandler::GlobalObjectGroundTruth_t& elem){ - return elem.id == ret.id; - })) - { + // Check if ID exists in objects ##TEMP + if (std::any_of(retval.begin(), retval.end(), [&](const OsiHandler::GlobalObjectGroundTruth_t& elem) { + return elem.id == ret.id; + })) { continue; } @@ -89,38 +82,36 @@ void OsiHandler::decodeSvGtMessage( cout << "Decoder debug:" << endl; cout << Sv.DebugString() << endl; } - } - else { + } else { std::cout << "Message has no groundtruth content" << std::endl; } - } - else { + } else { throw std::invalid_argument("Couldn't decode message!"); } } -std::string OsiHandler::encodeSvGtMessage( - const std::vector& data, - const std::chrono::system_clock::time_point& timestamp, - const std::string& projectionString, const bool debug) { +std::string OsiHandler::encodeSvGtMessage(const std::vector& data, + const std::chrono::system_clock::time_point& timestamp, + const std::string& projectionString, + const bool debug) { auto nanos = std::chrono::time_point_cast(timestamp).time_since_epoch().count(); - auto secs = nanos/1000000000; - nanos = nanos - secs*1000000000; + auto secs = nanos / 1000000000; + nanos = nanos - secs * 1000000000; Sv.Clear(); - osi3::GroundTruth *groundTruth = Sv.mutable_global_ground_truth(); + osi3::GroundTruth* groundTruth = Sv.mutable_global_ground_truth(); groundTruth->set_proj_string(projectionString); groundTruth->mutable_timestamp()->set_seconds(secs); groundTruth->mutable_timestamp()->set_nanos(static_cast(nanos)); for (const auto& elem : data) { using Eigen::AngleAxisd, Eigen::Vector3d; - osi3::MovingObject *movingObject = groundTruth->add_moving_object(); + osi3::MovingObject* movingObject = groundTruth->add_moving_object(); movingObject->mutable_id()->set_value(elem.id); - auto objOrientation = movingObject->mutable_base()->mutable_orientation(); - auto objPosition = movingObject->mutable_base()->mutable_position(); - auto objVelocity = movingObject->mutable_base()->mutable_velocity(); + auto objOrientation = movingObject->mutable_base()->mutable_orientation(); + auto objPosition = movingObject->mutable_base()->mutable_position(); + auto objVelocity = movingObject->mutable_base()->mutable_velocity(); auto objAcceleration = movingObject->mutable_base()->mutable_acceleration(); objPosition->set_x(elem.pos_m.x); @@ -130,18 +121,18 @@ std::string OsiHandler::encodeSvGtMessage( objOrientation->set_pitch(elem.orientation_rad.pitch); objOrientation->set_roll(elem.orientation_rad.roll); - auto rot = AngleAxisd(elem.orientation_rad.roll, Vector3d::UnitX()) - * AngleAxisd(elem.orientation_rad.pitch, Vector3d::UnitY()) - * AngleAxisd(elem.orientation_rad.yaw, Vector3d::UnitZ()); + auto rot = AngleAxisd(elem.orientation_rad.roll, Vector3d::UnitX()) * + AngleAxisd(elem.orientation_rad.pitch, Vector3d::UnitY()) * + AngleAxisd(elem.orientation_rad.yaw, Vector3d::UnitZ()); - Vector3d velObjFrame(elem.vel_m_s.lon,elem.vel_m_s.lat,elem.vel_m_s.up); + Vector3d velObjFrame(elem.vel_m_s.lon, elem.vel_m_s.lat, elem.vel_m_s.up); Vector3d velWorldFrame = rot.inverse() * velObjFrame; objVelocity->set_x(velWorldFrame.x()); objVelocity->set_y(velWorldFrame.y()); objVelocity->set_z(velWorldFrame.z()); - Vector3d accObjFrame(elem.acc_m_s2.lon,elem.acc_m_s2.lat,elem.acc_m_s2.up); + Vector3d accObjFrame(elem.acc_m_s2.lon, elem.acc_m_s2.lat, elem.acc_m_s2.up); Vector3d accWorldFrame = rot.inverse() * accObjFrame; objAcceleration->set_x(accWorldFrame.x()); @@ -157,24 +148,24 @@ std::string OsiHandler::encodeSvGtMessage( return Sv.SerializeAsString(); } -std::string OsiHandler::encodeSvGtMessage( - const std::vector &data, - const std::chrono::system_clock::time_point ×tamp, - const std::string &projectionString, const bool debug) { +std::string OsiHandler::encodeSvGtMessage(const std::vector& data, + const std::chrono::system_clock::time_point& timestamp, + const std::string& projectionString, + const bool debug) { auto nanos = std::chrono::time_point_cast(timestamp).time_since_epoch().count(); - auto secs = nanos/1000000000; - nanos = nanos - secs*1000000000; + auto secs = nanos / 1000000000; + nanos = nanos - secs * 1000000000; Sv.Clear(); - osi3::GroundTruth *groundTruth = Sv.mutable_global_ground_truth(); + osi3::GroundTruth* groundTruth = Sv.mutable_global_ground_truth(); groundTruth->set_proj_string(projectionString); groundTruth->mutable_timestamp()->set_seconds(secs); groundTruth->mutable_timestamp()->set_nanos(static_cast(nanos)); for (const auto& elem : data) { - osi3::MovingObject *movingObject = groundTruth->add_moving_object(); + osi3::MovingObject* movingObject = groundTruth->add_moving_object(); movingObject->mutable_id()->set_value(elem.id); movingObject->mutable_base()->mutable_position()->set_x(elem.pos_m.x); movingObject->mutable_base()->mutable_position()->set_y(elem.pos_m.y); @@ -198,16 +189,16 @@ std::string OsiHandler::encodeSvGtMessage( return Sv.SerializeAsString(); } -std::string OsiHandler::encodeSvGtMessage( - const LocalObjectGroundTruth_t& data, - const std::chrono::system_clock::time_point& timestamp, - const std::string& projectionString, const bool debug) { +std::string OsiHandler::encodeSvGtMessage(const LocalObjectGroundTruth_t& data, + const std::chrono::system_clock::time_point& timestamp, + const std::string& projectionString, + const bool debug) { return this->encodeSvGtMessage(std::vector({data}), timestamp, projectionString, debug); } -std::string OsiHandler::encodeSvGtMessage( - const GlobalObjectGroundTruth_t& data, - const std::chrono::system_clock::time_point& timestamp, - const std::string& projectionString, const bool debug) { +std::string OsiHandler::encodeSvGtMessage(const GlobalObjectGroundTruth_t& data, + const std::chrono::system_clock::time_point& timestamp, + const std::string& projectionString, + const bool debug) { return this->encodeSvGtMessage(std::vector({data}), timestamp, projectionString, debug); } diff --git a/atos/common/osihandler/osi_handler.hpp b/atos/common/osihandler/osi_handler.hpp index c47fb9968..3a732666c 100644 --- a/atos/common/osihandler/osi_handler.hpp +++ b/atos/common/osihandler/osi_handler.hpp @@ -6,18 +6,17 @@ /** * @file osi_handler.h - * @author Albin Nykvist - * @brief Osi handler class for encoding/decoding osi messages + * @author Albin Nykvist + * @brief Osi handler class for encoding/decoding osi messages * @date 2021-05-25 */ #pragma once -#include -#include "osi3/osi_sensorview.pb.h" #include "osi3/osi_sensordata.pb.h" +#include "osi3/osi_sensorview.pb.h" +#include -class OsiHandler -{ +class OsiHandler { public: /*! * \struct GlobalObjectGroundTruth_t @@ -152,40 +151,49 @@ class OsiHandler } orientation_rad; } LocalObjectGroundTruth_t; - /*! - * \brief Decode groundtruth content of Sensorview osi message. - * \param msg Recieved serialized OSI message. - * \param msgSize Size of recieved OSI message. - * \param debug Debug flag for printing message content. - */ - void decodeSvGtMessage(const std::vector& msg, const int msgSize, std::vector& retval, std::string& projStr, const bool debug); + /*! + * \brief Decode groundtruth content of Sensorview osi message. + * \param msg Recieved serialized OSI message. + * \param msgSize Size of recieved OSI message. + * \param debug Debug flag for printing message content. + */ + void decodeSvGtMessage(const std::vector& msg, + const int msgSize, + std::vector& retval, + std::string& projStr, + const bool debug); std::string encodeSvGtMessage(const std::vector& data, - const std::chrono::system_clock::time_point& timestamp, - const std::string& projectionString, const bool debug); + const std::chrono::system_clock::time_point& timestamp, + const std::string& projectionString, + const bool debug); std::string encodeSvGtMessage(const GlobalObjectGroundTruth_t& data, - const std::chrono::system_clock::time_point& timestamp, - const std::string& projectionString, const bool debug); + const std::chrono::system_clock::time_point& timestamp, + const std::string& projectionString, + const bool debug); std::string encodeSvGtMessage(const std::vector& data, - const std::chrono::system_clock::time_point& timestamp, - const std::string& projectionString, const bool debug); + const std::chrono::system_clock::time_point& timestamp, + const std::string& projectionString, + const bool debug); std::string encodeSvGtMessage(const LocalObjectGroundTruth_t& data, - const std::chrono::system_clock::time_point& timestamp, - const std::string& projectionString, const bool debug); + const std::chrono::system_clock::time_point& timestamp, + const std::string& projectionString, + const bool debug); /*! - * \brief Decode content of SensorData osi message. - * \param msg Recieved serialized OSI message. - * \param msgSize Size of recieved OSI message. - * \param debug Debug flag for printing message content. - */ + * \brief Decode content of SensorData osi message. + * \param msg Recieved serialized OSI message. + * \param msgSize Size of recieved OSI message. + * \param debug Debug flag for printing message content. + */ void decodeSdMessage(const std::vector& msg, const int msgSize, const bool debug); - // Public Sensorview message object. Calling decodeSvGtMessage will update the content. - osi3::SensorView Sv; // Use the internal methods to access the data. For example see the debug section of decodeSvGtMessage - // Public Sensordata message object. Calling decodeSdMessage will update the content. - osi3::SensorData Sd; // Use the internal methods to access the data. For example see the debug section of decodeSdMessage - + // Public Sensorview message object. Calling decodeSvGtMessage will update the content. + osi3::SensorView + Sv; // Use the internal methods to access the data. For example see the debug section of decodeSvGtMessage + // Public Sensordata message object. Calling decodeSdMessage will update the content. + osi3::SensorData + Sd; // Use the internal methods to access the data. For example see the debug section of decodeSdMessage }; diff --git a/atos/common/regexpatterns.hpp b/atos/common/regexpatterns.hpp index 89cb73656..98cedaa38 100644 --- a/atos/common/regexpatterns.hpp +++ b/atos/common/regexpatterns.hpp @@ -8,11 +8,10 @@ #include namespace RegexPatterns { -const std::string intPattern = "[0-9]+"; -const std::string floatPattern = "[-+]?[0-9]*\\.?[0-9]+"; -const std::string namePattern = "[a-zA-Z0-9\\._-]+"; +const std::string intPattern = "[0-9]+"; +const std::string floatPattern = "[-+]?[0-9]*\\.?[0-9]+"; +const std::string namePattern = "[a-zA-Z0-9\\._-]+"; const std::string versionPattern = "v?(" + intPattern + ")\\.?(" + intPattern + ")?\\.?(" + intPattern + ")?"; -} - +} // namespace RegexPatterns #endif diff --git a/atos/common/roschannels/cartesiantrajectorychannel.hpp b/atos/common/roschannels/cartesiantrajectorychannel.hpp index 41916afea..9d2f566a3 100644 --- a/atos/common/roschannels/cartesiantrajectorychannel.hpp +++ b/atos/common/roschannels/cartesiantrajectorychannel.hpp @@ -5,28 +5,28 @@ */ #pragma once -#include "roschannel.hpp" #include "atos_interfaces/msg/cartesian_trajectory.hpp" +#include "roschannel.hpp" namespace ROSChannels { - namespace CartesianTrajectory { - const std::string topicName = "cartesian_trajectory"; - using message_type = atos_interfaces::msg::CartesianTrajectory; +namespace CartesianTrajectory { +const std::string topicName = "cartesian_trajectory"; +using message_type = atos_interfaces::msg::CartesianTrajectory; - class Pub : public BasePub { - public: - const uint32_t objectId; - Pub(rclcpp::Node& node, const uint32_t id) : - BasePub(node, "object_" + std::to_string(id) + "/" + topicName), - objectId(id) {} - }; +class Pub : public BasePub { +public: + const uint32_t objectId; + Pub(rclcpp::Node& node, const uint32_t id) : + BasePub(node, "object_" + std::to_string(id) + "/" + topicName), + objectId(id) {} +}; - class Sub : public BaseSub { - public: - const uint32_t objectId; - Sub(rclcpp::Node& node, const uint32_t id, std::function callback) : - BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback), - objectId(id) {} - }; - } -} \ No newline at end of file +class Sub : public BaseSub { +public: + const uint32_t objectId; + Sub(rclcpp::Node& node, const uint32_t id, std::function callback) : + BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback), + objectId(id) {} +}; +} // namespace CartesianTrajectory +} // namespace ROSChannels diff --git a/atos/common/roschannels/commandchannels.hpp b/atos/common/roschannels/commandchannels.hpp index 490ccd7dd..1d3d29597 100644 --- a/atos/common/roschannels/commandchannels.hpp +++ b/atos/common/roschannels/commandchannels.hpp @@ -5,317 +5,386 @@ */ #pragma once +#include "atos_interfaces/msg/object_id_array.hpp" +#include "atos_interfaces/msg/object_trigger_start.hpp" #include "roschannel.hpp" #include "std_msgs/msg/empty.hpp" -#include "std_msgs/msg/u_int8.hpp" #include "std_msgs/msg/string.hpp" -#include "atos_interfaces/msg/object_id_array.hpp" -#include "atos_interfaces/msg/object_trigger_start.hpp" +#include "std_msgs/msg/u_int8.hpp" //! ROSChannels namespace namespace ROSChannels { - namespace Init { - const std::string topicName = "init"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace Connect { - const std::string topicName = "connect"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace Arm { - const std::string topicName = "arm"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace Disarm { - const std::string topicName = "disarm"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace Start { - const std::string topicName = "start"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace StartObject { - const std::string topicName = "start_object"; - using message_type = atos_interfaces::msg::ObjectTriggerStart; - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node) : - BasePub(node, topicName) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback) : BaseSub(node, topicName, callback) {} - }; - } - - namespace Stop { - const std::string topicName = "stop"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace Disconnect { - const std::string topicName = "disconnect"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace Failure { - const std::string topicName = "failure"; - using message_type = std_msgs::msg::UInt8; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace GetStatus { - const std::string topicName = "get_status"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace GetStatusResponse { - const std::string topicName = "get_status_response"; - using message_type = std_msgs::msg::String; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace AllClear { - const std::string topicName = "all_clear"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace Abort { - const std::string topicName = "abort"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace Exit { - const std::string topicName = "exit"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace Replay { - const std::string topicName = "replay"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace ObjectsConnected { - const std::string topicName = "objects_connected"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node) : BasePub(node, topicName) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback) : BaseSub(node, topicName, callback) {} - }; - } - - namespace ConnectedObjectIds { - const std::string topicName = "connected_object_ids"; - using message_type = atos_interfaces::msg::ObjectIdArray; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node) : BasePub(node, topicName) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback) : BaseSub(node, topicName, callback) {} - }; - } - - namespace ResetTestObjects { - const std::string topicName = "reset_test_objects"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace ReloadObjectSettings { - const std::string topicName = "reload_object_settings"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - -} \ No newline at end of file +namespace Init { +const std::string topicName = "init"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Init + +namespace Connect { +const std::string topicName = "connect"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Connect + +namespace Arm { +const std::string topicName = "arm"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Arm + +namespace Disarm { +const std::string topicName = "disarm"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Disarm + +namespace Start { +const std::string topicName = "start"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Start + +namespace StartObject { +const std::string topicName = "start_object"; +using message_type = atos_interfaces::msg::ObjectTriggerStart; + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node) : + BasePub(node, topicName) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, std::function callback) : + BaseSub(node, topicName, callback) {} +}; +} // namespace StartObject + +namespace Stop { +const std::string topicName = "stop"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Stop + +namespace Disconnect { +const std::string topicName = "disconnect"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Disconnect + +namespace Failure { +const std::string topicName = "failure"; +using message_type = std_msgs::msg::UInt8; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Failure + +namespace GetStatus { +const std::string topicName = "get_status"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace GetStatus + +namespace GetStatusResponse { +const std::string topicName = "get_status_response"; +using message_type = std_msgs::msg::String; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace GetStatusResponse + +namespace AllClear { +const std::string topicName = "all_clear"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace AllClear + +namespace Abort { +const std::string topicName = "abort"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Abort + +namespace Exit { +const std::string topicName = "exit"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Exit + +namespace Replay { +const std::string topicName = "replay"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace Replay + +namespace ObjectsConnected { +const std::string topicName = "objects_connected"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node) : + BasePub(node, topicName) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, std::function callback) : + BaseSub(node, topicName, callback) {} +}; +} // namespace ObjectsConnected + +namespace ConnectedObjectIds { +const std::string topicName = "connected_object_ids"; +using message_type = atos_interfaces::msg::ObjectIdArray; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node) : + BasePub(node, topicName) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, std::function callback) : + BaseSub(node, topicName, callback) {} +}; +} // namespace ConnectedObjectIds + +namespace ResetTestObjects { +const std::string topicName = "reset_test_objects"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace ResetTestObjects + +namespace ReloadObjectSettings { +const std::string topicName = "reload_object_settings"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace ReloadObjectSettings + +} // namespace ROSChannels diff --git a/atos/common/roschannels/controlsignalchannel.hpp b/atos/common/roschannels/controlsignalchannel.hpp index fce5855f5..9be5198d8 100644 --- a/atos/common/roschannels/controlsignalchannel.hpp +++ b/atos/common/roschannels/controlsignalchannel.hpp @@ -5,23 +5,27 @@ */ #pragma once -#include "roschannel.hpp" #include "atos_interfaces/msg/control_signal_percentage.hpp" +#include "roschannel.hpp" namespace ROSChannels { - namespace ControlSignal { - const std::string topicName = "control_signal"; - using message_type = atos_interfaces::msg::ControlSignalPercentage; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); +namespace ControlSignal { +const std::string topicName = "control_signal"; +using message_type = atos_interfaces::msg::ControlSignalPercentage; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } -} \ No newline at end of file +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace ControlSignal +} // namespace ROSChannels diff --git a/atos/common/roschannels/customcommandaction.hpp b/atos/common/roschannels/customcommandaction.hpp index 5108b7d5a..51ac87a45 100644 --- a/atos/common/roschannels/customcommandaction.hpp +++ b/atos/common/roschannels/customcommandaction.hpp @@ -10,22 +10,22 @@ namespace ROSChannels { namespace CustomCommandAction { -const std::string topicName = "custom_command_action"; -using message_type = atos_interfaces::msg::CustomCommandAction; +const std::string topicName = "custom_command_action"; +using message_type = atos_interfaces::msg::CustomCommandAction; const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); class Pub : public BasePub { public: - Pub(rclcpp::Node &node, const rclcpp::QoS &qos = defaultQoS) - : BasePub(node, topicName, qos) {} + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} }; class Sub : public BaseSub { public: - Sub(rclcpp::Node &node, - std::function callback, - const rclcpp::QoS &qos = defaultQoS) - : BaseSub(node, topicName, callback, qos) {} + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} }; } // namespace CustomCommandAction -} // namespace ROSChannels \ No newline at end of file +} // namespace ROSChannels diff --git a/atos/common/roschannels/gnsspathchannel.hpp b/atos/common/roschannels/gnsspathchannel.hpp index 3be4004d2..54ec29a8a 100644 --- a/atos/common/roschannels/gnsspathchannel.hpp +++ b/atos/common/roschannels/gnsspathchannel.hpp @@ -5,28 +5,33 @@ */ #pragma once -#include "roschannel.hpp" #include "foxglove_msgs/msg/geo_json.hpp" +#include "roschannel.hpp" namespace ROSChannels { - namespace GNSSPath { - const std::string topicName = "gnss_path"; - using message_type = foxglove_msgs::msg::GeoJSON; +namespace GNSSPath { +const std::string topicName = "gnss_path"; +using message_type = foxglove_msgs::msg::GeoJSON; - class Pub : public BasePub { - public: - const uint32_t objectId; - Pub(rclcpp::Node& node, const uint32_t id) : - BasePub(node, "object_" + std::to_string(id) + "/" + topicName, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), - objectId(id) {} - }; +class Pub : public BasePub { +public: + const uint32_t objectId; + Pub(rclcpp::Node& node, const uint32_t id) : + BasePub(node, + "object_" + std::to_string(id) + "/" + topicName, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), + objectId(id) {} +}; - class Sub : public BaseSub { - public: - const uint32_t objectId; - Sub(rclcpp::Node& node, const uint32_t id, std::function callback) : - BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), - objectId(id) {} - }; - } -} \ No newline at end of file +class Sub : public BaseSub { +public: + const uint32_t objectId; + Sub(rclcpp::Node& node, const uint32_t id, std::function callback) : + BaseSub(node, + "object_" + std::to_string(id) + "/" + topicName, + callback, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), + objectId(id) {} +}; +} // namespace GNSSPath +} // namespace ROSChannels diff --git a/atos/common/roschannels/navsatfixchannel.hpp b/atos/common/roschannels/navsatfixchannel.hpp index 45c345b8f..858a533aa 100644 --- a/atos/common/roschannels/navsatfixchannel.hpp +++ b/atos/common/roschannels/navsatfixchannel.hpp @@ -5,49 +5,52 @@ */ #pragma once +#include "CRSTransformation.hpp" // TODO: also remove this +#include "monitorchannel.hpp" // TODO: remove this when making translator node that translates monr to various other msg types.. #include "roschannel.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "monitorchannel.hpp" // TODO: remove this when making translator node that translates monr to various other msg types.. -#include "CRSTransformation.hpp" // TODO: also remove this namespace ROSChannels { - namespace NavSatFix { - const std::string topicName = "gnss_fix"; - using message_type = sensor_msgs::msg::NavSatFix; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); +namespace NavSatFix { +const std::string topicName = "gnss_fix"; +using message_type = sensor_msgs::msg::NavSatFix; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1)); - class Pub : public BasePub { - public: - const uint32_t objectId; - Pub(rclcpp::Node& node, const uint32_t id, const rclcpp::QoS& qos = defaultQoS) : - BasePub(node, "object_" + std::to_string(id) + "/" + topicName, qos), - objectId(id) {} - }; +class Pub : public BasePub { +public: + const uint32_t objectId; + Pub(rclcpp::Node& node, const uint32_t id, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, "object_" + std::to_string(id) + "/" + topicName, qos), + objectId(id) {} +}; - class Sub : public BaseSub { - public: - const uint32_t objectId; - Sub(rclcpp::Node& node, const uint32_t id, std::function callback, const rclcpp::QoS& qos = defaultQoS) : - BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos), - objectId(id) {} - }; - // TODO: Remove below.. - inline message_type fromROSMonr(std::array origin, const ROSChannels::Monitor::message_type &monr) { - sensor_msgs::msg::NavSatFix msg; - msg.header.stamp = monr.atos_header.header.stamp; +class Sub : public BaseSub { +public: + const uint32_t objectId; + Sub(rclcpp::Node& node, + const uint32_t id, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos), + objectId(id) {} +}; +// TODO: Remove below.. +inline message_type fromROSMonr(std::array origin, const ROSChannels::Monitor::message_type& monr) { + sensor_msgs::msg::NavSatFix msg; + msg.header.stamp = monr.atos_header.header.stamp; - // Local coordinates to global coordinates - double offset[3] = {monr.pose.pose.position.x, monr.pose.pose.position.y, monr.pose.pose.position.z}; - double llh_0[3] = {origin[0], origin[1], origin[2]}; - CRSTransformation::llhOffsetMeters(llh_0,offset); - msg.header.frame_id = "map"; // TODO + // Local coordinates to global coordinates + double offset[3] = {monr.pose.pose.position.x, monr.pose.pose.position.y, monr.pose.pose.position.z}; + double llh_0[3] = {origin[0], origin[1], origin[2]}; + CRSTransformation::llhOffsetMeters(llh_0, offset); + msg.header.frame_id = "map"; // TODO - // Fill in the rest of the message - msg.latitude = llh_0[0]; - msg.longitude = llh_0[1]; - msg.altitude = llh_0[2]; - msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; - return msg; - } - } -} \ No newline at end of file + // Fill in the rest of the message + msg.latitude = llh_0[0]; + msg.longitude = llh_0[1]; + msg.altitude = llh_0[2]; + msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + return msg; +} +} // namespace NavSatFix +} // namespace ROSChannels diff --git a/atos/common/roschannels/objstatechangechannel.hpp b/atos/common/roschannels/objstatechangechannel.hpp index 1b63aa42c..e884039f4 100644 --- a/atos/common/roschannels/objstatechangechannel.hpp +++ b/atos/common/roschannels/objstatechangechannel.hpp @@ -5,23 +5,27 @@ */ #pragma once -#include "roschannel.hpp" #include "atos_interfaces/msg/object_state_change.hpp" +#include "roschannel.hpp" namespace ROSChannels { - namespace ObjectStateChange { - const std::string topicName = "object_state_change"; - using message_type = atos_interfaces::msg::ObjectStateChange; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); +namespace ObjectStateChange { +const std::string topicName = "object_state_change"; +using message_type = atos_interfaces::msg::ObjectStateChange; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } -} \ No newline at end of file +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace ObjectStateChange +} // namespace ROSChannels diff --git a/atos/common/roschannels/pathchannel.hpp b/atos/common/roschannels/pathchannel.hpp index 97d5b3abd..c81809c8d 100644 --- a/atos/common/roschannels/pathchannel.hpp +++ b/atos/common/roschannels/pathchannel.hpp @@ -5,28 +5,33 @@ */ #pragma once -#include "roschannel.hpp" #include "nav_msgs/msg/path.hpp" +#include "roschannel.hpp" namespace ROSChannels { - namespace Path { - const std::string topicName = "path"; - using message_type = nav_msgs::msg::Path; +namespace Path { +const std::string topicName = "path"; +using message_type = nav_msgs::msg::Path; - class Pub : public BasePub { - public: - const uint32_t objectId; - Pub(rclcpp::Node& node, const uint32_t id) : - BasePub(node, "object_" + std::to_string(id) + "/" + topicName, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), - objectId(id) {} - }; +class Pub : public BasePub { +public: + const uint32_t objectId; + Pub(rclcpp::Node& node, const uint32_t id) : + BasePub(node, + "object_" + std::to_string(id) + "/" + topicName, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), + objectId(id) {} +}; - class Sub : public BaseSub { - public: - const uint32_t objectId; - Sub(rclcpp::Node& node, const uint32_t id, std::function callback) : - BaseSub(node, "object_" + std::to_string(id) + "/" + topicName, callback, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), - objectId(id) {} - }; - } -} \ No newline at end of file +class Sub : public BaseSub { +public: + const uint32_t objectId; + Sub(rclcpp::Node& node, const uint32_t id, std::function callback) : + BaseSub(node, + "object_" + std::to_string(id) + "/" + topicName, + callback, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()), + objectId(id) {} +}; +} // namespace Path +} // namespace ROSChannels diff --git a/atos/common/roschannels/pointcloudchannel.hpp b/atos/common/roschannels/pointcloudchannel.hpp index a087cc77f..cfe62f2f9 100644 --- a/atos/common/roschannels/pointcloudchannel.hpp +++ b/atos/common/roschannels/pointcloudchannel.hpp @@ -9,22 +9,25 @@ #include namespace ROSChannels { - namespace Pointcloud { - const std::string topicName = "site_scan"; - using message_type = sensor_msgs::msg::PointCloud2; - - class Pub : public BasePub { - public: - const std::string fileName; - Pub(rclcpp::Node& node, const std::string fileName) : - BasePub(node, topicName + "/" + fileName, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} - }; +namespace Pointcloud { +const std::string topicName = "site_scan"; +using message_type = sensor_msgs::msg::PointCloud2; - class Sub : public BaseSub { - public: - const std::string fileName; - Sub(rclcpp::Node& node, const std::string fileName, std::function callback) : - BaseSub(node, topicName + "/" + fileName, callback, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} - }; - } -} \ No newline at end of file +class Pub : public BasePub { +public: + const std::string fileName; + Pub(rclcpp::Node& node, const std::string fileName) : + BasePub(node, topicName + "/" + fileName, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} +}; + +class Sub : public BaseSub { +public: + const std::string fileName; + Sub(rclcpp::Node& node, const std::string fileName, std::function callback) : + BaseSub(node, + topicName + "/" + fileName, + callback, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} +}; +} // namespace Pointcloud +} // namespace ROSChannels diff --git a/atos/common/roschannels/remotecontrolchannels.hpp b/atos/common/roschannels/remotecontrolchannels.hpp index 5ba4fbdf0..71ae17c03 100644 --- a/atos/common/roschannels/remotecontrolchannels.hpp +++ b/atos/common/roschannels/remotecontrolchannels.hpp @@ -5,90 +5,109 @@ */ #pragma once +#include "atos_interfaces/msg/manoeuvre_command.hpp" #include "roschannel.hpp" #include "std_msgs/msg/empty.hpp" #include "std_msgs/msg/int8.hpp" -#include "atos_interfaces/msg/manoeuvre_command.hpp" - namespace ROSChannels { - namespace RemoteControlEnable { - const std::string topicName = "remote_control_enable"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace RemoteControlDisable { - const std::string topicName = "remote_control_disable"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace RemoteControlManoeuvre { - const std::string topicName = "remote_control_manoeuvre"; - using message_type = atos_interfaces::msg::ManoeuvreCommand; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace BackToStart { - const std::string topicName = "back_to_start"; - using message_type = atos_interfaces::msg::ManoeuvreCommand; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } - - namespace BackToStartResponse { - const std::string topicName = "back_to_start_response"; - using message_type = std_msgs::msg::Int8; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; - - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } -} \ No newline at end of file +namespace RemoteControlEnable { +const std::string topicName = "remote_control_enable"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace RemoteControlEnable + +namespace RemoteControlDisable { +const std::string topicName = "remote_control_disable"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace RemoteControlDisable + +namespace RemoteControlManoeuvre { +const std::string topicName = "remote_control_manoeuvre"; +using message_type = atos_interfaces::msg::ManoeuvreCommand; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace RemoteControlManoeuvre + +namespace BackToStart { +const std::string topicName = "back_to_start"; +using message_type = atos_interfaces::msg::ManoeuvreCommand; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace BackToStart + +namespace BackToStartResponse { +const std::string topicName = "back_to_start_response"; +using message_type = std_msgs::msg::Int8; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); + +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; + +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace BackToStartResponse +} // namespace ROSChannels diff --git a/atos/common/roschannels/roschannel.hpp b/atos/common/roschannels/roschannel.hpp index afdbd7b57..9e2b3f312 100644 --- a/atos/common/roschannels/roschannel.hpp +++ b/atos/common/roschannels/roschannel.hpp @@ -5,34 +5,35 @@ */ #pragma once +#include #include #include -#include namespace ROSChannels { template class BasePub { public: - BasePub(rclcpp::Node& node, - const std::string& topicName, - const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) - : pub(node.create_publisher(topicName, qos)) {} - BasePub() = delete; - typename rclcpp::Publisher::SharedPtr pub; - inline virtual void publish(const T& msg) { assert(pub); pub->publish(msg); }; + BasePub(rclcpp::Node& node, const std::string& topicName, const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) : + pub(node.create_publisher(topicName, qos)) {} + BasePub() = delete; + typename rclcpp::Publisher::SharedPtr pub; + inline virtual void publish(const T& msg) { + assert(pub); + pub->publish(msg); + }; }; template class BaseSub { public: - BaseSub(rclcpp::Node& node, - const std::string& topicName, - std::function callback, - const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) - : sub(node.create_subscription(topicName, qos, callback)) {} - BaseSub() = delete; - typename rclcpp::Subscription::SharedPtr sub; + BaseSub(rclcpp::Node& node, + const std::string& topicName, + std::function callback, + const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll())) : + sub(node.create_subscription(topicName, qos, callback)) {} + BaseSub() = delete; + typename rclcpp::Subscription::SharedPtr sub; }; -} // namespace ROSChannels \ No newline at end of file +} // namespace ROSChannels diff --git a/atos/common/roschannels/scenariochannel.hpp b/atos/common/roschannels/scenariochannel.hpp index bd77a1252..aca05f3d3 100644 --- a/atos/common/roschannels/scenariochannel.hpp +++ b/atos/common/roschannels/scenariochannel.hpp @@ -11,23 +11,18 @@ namespace ROSChannels { namespace StoryBoardElementStateChange { const std::string topicName = "story_board_element_state_change"; -using message_type = atos_interfaces::msg::StoryBoardElementStateChange; +using message_type = atos_interfaces::msg::StoryBoardElementStateChange; class Pub : public BasePub { public: - Pub(rclcpp::Node &node) - : BasePub( - node, topicName, - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} + Pub(rclcpp::Node& node) : + BasePub(node, topicName, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} }; class Sub : public BaseSub { public: - Sub(rclcpp::Node &node, - std::function callback) - : BaseSub( - node, topicName, callback, - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} + Sub(rclcpp::Node& node, std::function callback) : + BaseSub(node, topicName, callback, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {} }; } // namespace StoryBoardElementStateChange -} // namespace ROSChannels \ No newline at end of file +} // namespace ROSChannels diff --git a/atos/common/roschannels/statechange.hpp b/atos/common/roschannels/statechange.hpp index 861aea7b3..ae716b343 100644 --- a/atos/common/roschannels/statechange.hpp +++ b/atos/common/roschannels/statechange.hpp @@ -5,23 +5,27 @@ */ #pragma once -#include "roschannel.hpp" #include "atos_interfaces/msg/state_change.hpp" +#include "roschannel.hpp" namespace ROSChannels { - namespace StateChange { - const std::string topicName = "state_change"; - using message_type = atos_interfaces::msg::StateChange; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); +namespace StateChange { +const std::string topicName = "state_change"; +using message_type = atos_interfaces::msg::StateChange; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } -} \ No newline at end of file +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace StateChange +} // namespace ROSChannels diff --git a/atos/common/roschannels/test_channels.hpp b/atos/common/roschannels/test_channels.hpp index 7bd227cd3..80d6bd05f 100644 --- a/atos/common/roschannels/test_channels.hpp +++ b/atos/common/roschannels/test_channels.hpp @@ -1,20 +1,23 @@ #include "roschannel.hpp" #include "std_msgs/msg/empty.hpp" - namespace ROSChannels { - namespace SampleModuleTestForInitResponse { - const std::string topicName = "sample_module_test_for_init_response"; - using message_type = std_msgs::msg::Empty; - const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); - class Pub : public BasePub { - public: - Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : BasePub(node, topicName, qos) {} - }; +namespace SampleModuleTestForInitResponse { +const std::string topicName = "sample_module_test_for_init_response"; +using message_type = std_msgs::msg::Empty; +const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepAll()); +class Pub : public BasePub { +public: + Pub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) : + BasePub(node, topicName, qos) {} +}; - class Sub : public BaseSub { - public: - Sub(rclcpp::Node& node, std::function callback, const rclcpp::QoS& qos = defaultQoS) : BaseSub(node, topicName, callback, qos) {} - }; - } -} \ No newline at end of file +class Sub : public BaseSub { +public: + Sub(rclcpp::Node& node, + std::function callback, + const rclcpp::QoS& qos = defaultQoS) : + BaseSub(node, topicName, callback, qos) {} +}; +} // namespace SampleModuleTestForInitResponse +} // namespace ROSChannels diff --git a/atos/common/shmem/shmem.cpp b/atos/common/shmem/shmem.cpp index 0afd36a1c..853af5740 100644 --- a/atos/common/shmem/shmem.cpp +++ b/atos/common/shmem/shmem.cpp @@ -6,27 +6,29 @@ #define _GNU_SOURCE -#include -#include -#include +#include +#include #include +#include #include #include -#include -#include #include -#include +#include +#include +#include #include "shmem.h" -//#define SHMEM_DEBUG /*!< Comment this line in order to suppress debug output */ +// #define SHMEM_DEBUG /*!< Comment this line in order to suppress debug output */ #ifdef SHMEM_DEBUG -#define debug_print(fmt, ...) \ - do { fprintf(stderr, "%d:%s:%d:%s():" fmt,getpid(),__FILE__,\ - __LINE__,__func__,__VA_ARGS__);} while(0) +#define debug_print(fmt, ...) \ + do { \ + fprintf(stderr, "%d:%s:%d:%s():" fmt, getpid(), __FILE__, __LINE__, __func__, __VA_ARGS__); \ + } while (0) #else -#define debug_print(fmt, ...) \ - do {} while(0) +#define debug_print(fmt, ...) \ + do { \ + } while (0) #endif using namespace std; @@ -34,7 +36,7 @@ using namespace std; #define LOCK_FILE_ENDING ".lck" #define MEMORY_HEADER_ENDING ".hdr" #define MEMORY_FILE_ENDING ".mem" -#define SHM_NAME_MAX (NAME_MAX - 4 - sizeof (LOCK_FILE_ENDING)) +#define SHM_NAME_MAX (NAME_MAX - 4 - sizeof(LOCK_FILE_ENDING)) #define MEMORY_MINSIZE 1 @@ -42,8 +44,8 @@ using namespace std; * \brief MemoryInformation Stores size and element type data for a section of anonymous memory */ typedef struct { - unsigned int numberOfElements; //!< Number of elements in the memory - size_t elementSize; //!< Size of an individual element + unsigned int numberOfElements; //!< Number of elements in the memory + size_t elementSize; //!< Size of an individual element } MemoryInformation; /*! @@ -51,58 +53,54 @@ typedef struct { */ class Memory { public: - string name = ""; //!< ASCII name of the memory - void* address = nullptr; //!< Address of the shared memory - typedef enum { - CREATED_MEMORY, - FOUND_EXISTING_MEMORY, - FAILED, - SUCCEEDED - } MemoryReturnCode; - - Memory(string memoryName) : name(memoryName) { + string name = ""; //!< ASCII name of the memory + void* address = nullptr; //!< Address of the shared memory + typedef enum { CREATED_MEMORY, FOUND_EXISTING_MEMORY, FAILED, SUCCEEDED } MemoryReturnCode; + + Memory(string memoryName) : + name(memoryName) { // Calls to shm assume the file name starts with a forward slash; ensure this matches if (name.front() != '/') { name.insert(0, "/"); } } - Memory(Memory &&other) noexcept { + Memory(Memory&& other) noexcept { debug_print("Move constructor called for memory %s\n", name.c_str()); - name = other.name; - address = other.address; - mutex = other.mutex; - fdHeader = other.fdHeader; - fdMemory = other.fdMemory; + name = other.name; + address = other.address; + mutex = other.mutex; + fdHeader = other.fdHeader; + fdMemory = other.fdMemory; virtualMemoryInfo = other.virtualMemoryInfo; - info = other.info; - - other.name = ""; - other.address = nullptr; - other.mutex = nullptr; - other.info = nullptr; - other.fdHeader = -1; - other.fdMemory = -1; + info = other.info; + + other.name = ""; + other.address = nullptr; + other.mutex = nullptr; + other.info = nullptr; + other.fdHeader = -1; + other.fdMemory = -1; other.virtualMemoryInfo = {0, 0}; } Memory& operator=(Memory&& other) noexcept { debug_print("Move assignment operator called for memory %s\n", name.c_str()); if (this != &other) { - name = other.name; - address = other.address; - mutex = other.mutex; - fdHeader = other.fdHeader; - fdMemory = other.fdMemory; + name = other.name; + address = other.address; + mutex = other.mutex; + fdHeader = other.fdHeader; + fdMemory = other.fdMemory; virtualMemoryInfo = other.virtualMemoryInfo; - info = other.info; - - other.name = ""; - other.address = nullptr; - other.mutex = nullptr; - other.info = nullptr; - other.fdHeader = -1; - other.fdMemory = -1; + info = other.info; + + other.name = ""; + other.address = nullptr; + other.mutex = nullptr; + other.info = nullptr; + other.fdHeader = -1; + other.fdMemory = -1; other.virtualMemoryInfo = {0, 0}; } return *this; @@ -113,14 +111,16 @@ class Memory { // Unmap memory if (address != nullptr) { - if (munmap(address, virtualMemoryInfo.elementSize == 0 || virtualMemoryInfo.numberOfElements == 0 ? - MEMORY_MINSIZE : virtualMemoryInfo.elementSize * virtualMemoryInfo.numberOfElements) == -1) { + if (munmap(address, + virtualMemoryInfo.elementSize == 0 || virtualMemoryInfo.numberOfElements == 0 + ? MEMORY_MINSIZE + : virtualMemoryInfo.elementSize * virtualMemoryInfo.numberOfElements) == -1) { perror("munmap"); } } if (info != nullptr) { - if (munmap(info, sizeof (*info)) == -1) { + if (munmap(info, sizeof(*info)) == -1) { perror("munmap"); } } @@ -130,7 +130,7 @@ class Memory { if (close(fdHeader) == -1) { perror("close"); } - if (close(fdMemory) == -1) { + if (close(fdMemory) == -1) { perror("close"); } } @@ -143,16 +143,34 @@ class Memory { return; } - int claim() { return sem_wait(mutex); } - int release() { return sem_post(mutex); } + int claim() { + return sem_wait(mutex); + } + int release() { + return sem_post(mutex); + } - string getHeaderFileName(void) const { return name + MEMORY_HEADER_ENDING; } - string getMemoryFileName(void) const { return name + MEMORY_FILE_ENDING; } - string getLockFileName(void) const { return name + LOCK_FILE_ENDING; } - ssize_t getTotalSize(void) const { return info == nullptr ? -1 : static_cast(info->elementSize*info->numberOfElements); } - size_t getMapSize(void) const { return virtualMemoryInfo.elementSize * virtualMemoryInfo.numberOfElements; } - int getNumberOfElements(void) const { return info == nullptr ? -1 : static_cast(info->numberOfElements); } - ssize_t getElementSize(void) const { return info == nullptr ? -1 : static_cast(info->elementSize); } + string getHeaderFileName(void) const { + return name + MEMORY_HEADER_ENDING; + } + string getMemoryFileName(void) const { + return name + MEMORY_FILE_ENDING; + } + string getLockFileName(void) const { + return name + LOCK_FILE_ENDING; + } + ssize_t getTotalSize(void) const { + return info == nullptr ? -1 : static_cast(info->elementSize * info->numberOfElements); + } + size_t getMapSize(void) const { + return virtualMemoryInfo.elementSize * virtualMemoryInfo.numberOfElements; + } + int getNumberOfElements(void) const { + return info == nullptr ? -1 : static_cast(info->numberOfElements); + } + ssize_t getElementSize(void) const { + return info == nullptr ? -1 : static_cast(info->elementSize); + } MemoryReturnCode setUp(unsigned int numberOfElements, size_t elementSize) { @@ -177,8 +195,7 @@ class Memory { } retval = CREATED_MEMORY; - } - else if (fdHeader != -1) { + } else if (fdHeader != -1) { // File existed debug_print("Memory %s found - using existing\n", name.c_str()); if ((fdMemory = shm_open(getMemoryFileName().c_str(), O_RDWR, S_IRUSR | S_IWUSR)) == -1) { @@ -187,8 +204,7 @@ class Memory { return FAILED; } retval = FOUND_EXISTING_MEMORY; - } - else { + } else { // Failed to open for other reason debug_print("Memory %s failed to open\n", name.c_str()); release(); @@ -198,7 +214,8 @@ class Memory { debug_print("Mapping memory\n", nullptr); // Map header file into virtual memory space - if ((info = static_cast(mmap(nullptr, sizeof (MemoryInformation), PROT_READ | PROT_WRITE, MAP_SHARED, fdHeader, 0))) == MAP_FAILED) { + if ((info = static_cast(mmap( + nullptr, sizeof(MemoryInformation), PROT_READ | PROT_WRITE, MAP_SHARED, fdHeader, 0))) == MAP_FAILED) { release(); perror("mmap"); return FAILED; @@ -206,19 +223,20 @@ class Memory { // If memory was created, after mapping it must be initialized with correct values if (retval == CREATED_MEMORY) { - info->elementSize = elementSize; + info->elementSize = elementSize; info->numberOfElements = numberOfElements; - } - else { - elementSize = info->elementSize; + } else { + elementSize = info->elementSize; numberOfElements = info->numberOfElements; } // Map memory file into virtual memory space - if ((address = mmap(nullptr, elementSize == 0 || numberOfElements == 0 ? - MEMORY_MINSIZE : elementSize * numberOfElements, - PROT_READ | PROT_WRITE, MAP_SHARED, - fdMemory, 0)) == MAP_FAILED) { + if ((address = mmap(nullptr, + elementSize == 0 || numberOfElements == 0 ? MEMORY_MINSIZE : elementSize * numberOfElements, + PROT_READ | PROT_WRITE, + MAP_SHARED, + fdMemory, + 0)) == MAP_FAILED) { release(); perror("mmap"); return FAILED; @@ -233,12 +251,11 @@ class Memory { MemoryReturnCode resize(unsigned int newNumberOfElements) { // Resize the system memory file - - if (ftruncate(fdMemory, newNumberOfElements == 0 ? MEMORY_MINSIZE : newNumberOfElements * getElementSize()) == -1) { + if (ftruncate(fdMemory, newNumberOfElements == 0 ? MEMORY_MINSIZE : newNumberOfElements * getElementSize()) == + -1) { perror("ftruncate"); return FAILED; - } - else { + } else { info->numberOfElements = newNumberOfElements; // Remap the virtual memory space in case it is needed return refresh(); @@ -246,17 +263,20 @@ class Memory { } MemoryReturnCode refresh() { - void * newMemoryPointer = nullptr; - newMemoryPointer = mremap(address, virtualMemoryInfo.elementSize == 0 || virtualMemoryInfo.numberOfElements == 0 ? - MEMORY_MINSIZE : virtualMemoryInfo.elementSize * virtualMemoryInfo.numberOfElements, - info->elementSize == 0 || info->numberOfElements == 0 ? - MEMORY_MINSIZE : info->elementSize * info->numberOfElements, MREMAP_MAYMOVE); + void* newMemoryPointer = nullptr; + newMemoryPointer = + mremap(address, + virtualMemoryInfo.elementSize == 0 || virtualMemoryInfo.numberOfElements == 0 + ? MEMORY_MINSIZE + : virtualMemoryInfo.elementSize * virtualMemoryInfo.numberOfElements, + info->elementSize == 0 || info->numberOfElements == 0 ? MEMORY_MINSIZE + : info->elementSize * info->numberOfElements, + MREMAP_MAYMOVE); if (newMemoryPointer == MAP_FAILED) { perror("mremap"); return FAILED; - } - else { + } else { if (newMemoryPointer != address) { debug_print("Remapped memory %s\n", name.c_str()); address = newMemoryPointer; @@ -267,11 +287,11 @@ class Memory { } private: - sem_t* mutex = nullptr; //!< Mutex synchronizing access to the memory - int fdHeader = -1; //!< File descriptor for the memory header - int fdMemory = -1; //!< File descriptor for the memory file - MemoryInformation virtualMemoryInfo = {0, 0}; //!< Information on the virtual memory map - MemoryInformation* info = nullptr; //!< Information on the shared memory + sem_t* mutex = nullptr; //!< Mutex synchronizing access to the memory + int fdHeader = -1; //!< File descriptor for the memory header + int fdMemory = -1; //!< File descriptor for the memory file + MemoryInformation virtualMemoryInfo = {0, 0}; //!< Information on the virtual memory map + MemoryInformation* info = nullptr; //!< Information on the shared memory /*! * \brief Creates the memory files necessary on the file system and ensures their size. @@ -280,13 +300,14 @@ class Memory { * \return value according to ::MemoryReturnCode */ MemoryReturnCode initialize(const unsigned int initialElements, size_t elementSize) { - if ((fdHeader = shm_open(getHeaderFileName().c_str(), O_RDWR | O_CREAT, S_IRUSR | S_IWUSR)) == -1 - || (fdMemory = shm_open(getMemoryFileName().c_str(), O_RDWR | O_CREAT, S_IRUSR | S_IWUSR)) == -1){ + if ((fdHeader = shm_open(getHeaderFileName().c_str(), O_RDWR | O_CREAT, S_IRUSR | S_IWUSR)) == -1 || + (fdMemory = shm_open(getMemoryFileName().c_str(), O_RDWR | O_CREAT, S_IRUSR | S_IWUSR)) == -1) { perror("shm_open"); return FAILED; } - if (ftruncate(fdHeader, sizeof (MemoryInformation)) == -1 - || ftruncate(fdMemory, static_cast(initialElements == 0 ? MEMORY_MINSIZE : initialElements * elementSize))) { + if (ftruncate(fdHeader, sizeof(MemoryInformation)) == -1 || + ftruncate(fdMemory, + static_cast(initialElements == 0 ? MEMORY_MINSIZE : initialElements * elementSize))) { perror("ftruncate"); return FAILED; } @@ -294,25 +315,31 @@ class Memory { } }; -/*********************************** STATIC VARIABLES ********************************************************************/ +/*********************************** STATIC VARIABLES + * ********************************************************************/ static vector sharedMemoryBlocks; -/*********************************** STATIC FUNCTION DECLARATIONS ********************************************************/ +/*********************************** STATIC FUNCTION DECLARATIONS + * ********************************************************/ static vector::iterator getMemoryByAddress(volatile void* addr); -/*********************************** FUNCTION DEFINITIONS ****************************************************************/ +/*********************************** FUNCTION DEFINITIONS + * ****************************************************************/ /*! * \brief createSharedMemory Creates a file-backed memory space on the system and sets up virtual memory mappings in the - * calling process' address space. If a memory with the specified name already exists, the memory is not modified. - * Thus, the resulting memory may not actually be the size specified by the input arguments. - * \param memoryName Name of the memory. Files will be created with endings ".mem", ".lck" and ".hdr" appended to this name + * calling process' address space. If a memory with the specified name already exists, the memory is not + *modified. Thus, the resulting memory may not actually be the size specified by the input arguments. + * \param memoryName Name of the memory. Files will be created with endings ".mem", ".lck" and ".hdr" appended to this + *name * \param initialElements Desired initial number of elements if preexisting memory not found. * \param elementSize Size of the individual elements. * \param wasCreated Boolean value indicating if the memory was created (as opposed to a preexisting memory was found) * \return Pointer to the created memory. */ -volatile void* createSharedMemory(const char* memoryName, const unsigned int initialElements, - const size_t elementSize, int *wasCreated) { +volatile void* createSharedMemory(const char* memoryName, + const unsigned int initialElements, + const size_t elementSize, + int* wasCreated) { size_t nameLength; @@ -336,8 +363,9 @@ volatile void* createSharedMemory(const char* memoryName, const unsigned int ini } // Check if specified memory has already been opened - if (any_of(sharedMemoryBlocks.begin(), sharedMemoryBlocks.end(), - [memoryName](const Memory &mem){ return mem.name.find(memoryName) != string::npos; })) { + if (any_of(sharedMemoryBlocks.begin(), sharedMemoryBlocks.end(), [memoryName](const Memory& mem) { + return mem.name.find(memoryName) != string::npos; + })) { errno = EEXIST; return nullptr; } @@ -349,22 +377,22 @@ volatile void* createSharedMemory(const char* memoryName, const unsigned int ini debug_print("Setting up memory for %s\n", memory->name.c_str()); switch (memory->setUp(initialElements, elementSize)) { - case Memory::CREATED_MEMORY: - if (wasCreated != nullptr) { - *wasCreated = 1; - } - break; - case Memory::FOUND_EXISTING_MEMORY: - if (wasCreated != nullptr) { - *wasCreated = 0; - } - break; - default: - if (wasCreated != nullptr) { - *wasCreated = 0; - } - sharedMemoryBlocks.erase(memory); - return nullptr; + case Memory::CREATED_MEMORY: + if (wasCreated != nullptr) { + *wasCreated = 1; + } + break; + case Memory::FOUND_EXISTING_MEMORY: + if (wasCreated != nullptr) { + *wasCreated = 0; + } + break; + default: + if (wasCreated != nullptr) { + *wasCreated = 0; + } + sharedMemoryBlocks.erase(memory); + return nullptr; } return memory->address; @@ -377,8 +405,8 @@ volatile void* createSharedMemory(const char* memoryName, const unsigned int ini * \return Pointer to a memory element, or end if none was found matching */ vector::iterator getMemoryByAddress(volatile void* addr) { - return find_if(sharedMemoryBlocks.begin(), sharedMemoryBlocks.end(), - [&](const Memory &mem){ return mem.address == addr; }); + return find_if( + sharedMemoryBlocks.begin(), sharedMemoryBlocks.end(), [&](const Memory& mem) { return mem.address == addr; }); } /*! @@ -395,7 +423,7 @@ void destroySharedMemory(volatile void* addr) { } // Save the names for unlinking after resource deallocation - string lockFileName = mem->getLockFileName(); + string lockFileName = mem->getLockFileName(); string headerFileName = mem->getHeaderFileName(); string memoryFileName = mem->getMemoryFileName(); closeSharedMemory(addr); @@ -429,7 +457,6 @@ void closeSharedMemory(volatile void* addr) { sharedMemoryBlocks.erase(mem); } - /*! * \brief claimSharedMemory Claims access to the memory, restricting other processes from accessing the memory. * If another process has already claimed the memory, this function blocks until access is available. @@ -445,8 +472,7 @@ volatile void* claimSharedMemory(volatile void* addr) { mem->claim(); debug_print("Claimed memory %s\n", mem->name.c_str()); mem->refresh(); - } - else { + } else { errno = ENOENT; perror(__FUNCTION__); return nullptr; @@ -455,7 +481,6 @@ volatile void* claimSharedMemory(volatile void* addr) { return mem->address; } - /*! * \brief releaseSharedMemory Releases access to the memory, allowing other processes to access the memory * \param addr Address of the memory to be released. @@ -466,8 +491,7 @@ volatile void* releaseSharedMemory(volatile void* addr) { if (mem != sharedMemoryBlocks.end()) { mem->release(); debug_print("Released memory %s\n", mem->name.c_str()); - } - else { + } else { errno = ENOENT; perror(__FUNCTION__); return nullptr; @@ -475,7 +499,6 @@ volatile void* releaseSharedMemory(volatile void* addr) { return mem->address; } - /*! * \brief resizeSharedMemory Modifies the size of shared memory. If the memory is expanded the new * area is initialized with 0. If the memory is reduced in size the end is truncated. @@ -501,7 +524,6 @@ volatile void* resizeSharedMemory(volatile void* addr, const unsigned int newNum return mem->address; } - /*! * \brief getNumberOfMemoryElements Returns the number of memory elements for a shared memory. The memory * should be claimed before this operation to ensure relevance of the return value. @@ -518,7 +540,6 @@ int getNumberOfMemoryElements(volatile void* addr) { return mem->getNumberOfElements(); } - /*! * \brief getMemorySize Returns the total size of the shared memory. * \param addr Address of the shared memory. @@ -534,7 +555,6 @@ ssize_t getMemorySize(volatile void* addr) { return mem->getTotalSize(); } - /*! * \brief getElementSize Returns the number of elements in the shared memory. * \param addr Address of the shared memory. diff --git a/atos/common/sockets/canhandler.cpp b/atos/common/sockets/canhandler.cpp index 61dac5fbe..20b0f792a 100644 --- a/atos/common/sockets/canhandler.cpp +++ b/atos/common/sockets/canhandler.cpp @@ -5,24 +5,22 @@ */ #include "canhandler.hpp" -#include #include +#include #include -#include #include +#include #include #include #include - CANHandler::CANHandler(const bool blocking) { this->blocking = blocking; } -int CANHandler::connectTo( - const std::string &interface) { +int CANHandler::connectTo(const std::string& interface) { struct ifreq ifr; can_frame frame; @@ -36,17 +34,16 @@ int CANHandler::connectTo( std::strcpy(ifr.ifr_name, interface.c_str()); ioctl(sockfd, SIOCGIFINDEX, &ifr); - addr.can_family = AF_CAN; + addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; std::cout << "Binding CAN handler to interface " << interface << std::endl; - if (bind(sockfd, reinterpret_cast(&addr), - sizeof (addr))) { + if (bind(sockfd, reinterpret_cast(&addr), sizeof(addr))) { perror("bind"); return -1; } - bytesRead = recv(sockfd, &frame, sizeof (frame), MSG_DONTWAIT); + bytesRead = recv(sockfd, &frame, sizeof(frame), MSG_DONTWAIT); if (bytesRead < 0 && errno != EAGAIN && errno != EWOULDBLOCK) { perror("recv"); close(sockfd); @@ -56,15 +53,13 @@ int CANHandler::connectTo( return 0; } -ssize_t CANHandler::receive(can_frame &frame) { +ssize_t CANHandler::receive(can_frame& frame) { ssize_t bytesRead = 0; - bytesRead = recv(sockfd, &frame, sizeof (frame), - blocking ? 0 : MSG_DONTWAIT); + bytesRead = recv(sockfd, &frame, sizeof(frame), blocking ? 0 : MSG_DONTWAIT); if (bytesRead < 0) { if (!blocking && (errno == EAGAIN || errno == EWOULDBLOCK)) { bytesRead = 0; - } - else { + } else { perror("recv"); close(sockfd); } @@ -72,15 +67,13 @@ ssize_t CANHandler::receive(can_frame &frame) { return bytesRead; } -ssize_t CANHandler::transmit(const can_frame &frame){ +ssize_t CANHandler::transmit(const can_frame& frame) { ssize_t bytesSent = 0; - bytesSent = send(sockfd, &frame, sizeof (struct can_frame), - blocking ? 0 : MSG_DONTWAIT); + bytesSent = send(sockfd, &frame, sizeof(struct can_frame), blocking ? 0 : MSG_DONTWAIT); if (bytesSent < 0) { if (!blocking && (errno == EAGAIN || errno == EWOULDBLOCK)) { bytesSent = 0; - } - else { + } else { perror("send"); close(sockfd); } diff --git a/atos/common/sockets/canhandler.hpp b/atos/common/sockets/canhandler.hpp index 6839aa423..711beee8e 100644 --- a/atos/common/sockets/canhandler.hpp +++ b/atos/common/sockets/canhandler.hpp @@ -6,23 +6,29 @@ #ifndef CANHANDLER_HPP #define CANHANDLER_HPP -#include #include +#include class CANHandler { public: CANHandler(const bool blocking = true); - void setBlocking(void) { this->blocking = true; } - void setNonblocking(void) { this->blocking = false; } - bool isBlocking() const { return this->blocking; } + void setBlocking(void) { + this->blocking = true; + } + void setNonblocking(void) { + this->blocking = false; + } + bool isBlocking() const { + return this->blocking; + } int connectTo(const std::string& interface); - ssize_t receive(can_frame &frame); - ssize_t transmit(const can_frame &frame); + ssize_t receive(can_frame& frame); + ssize_t transmit(const can_frame& frame); + private: - int sockfd = 0; + int sockfd = 0; struct sockaddr_can addr = {0}; - bool blocking = true; + bool blocking = true; }; - #endif diff --git a/atos/common/sockets/client.cpp b/atos/common/sockets/client.cpp index 8a35ffe00..80ebfe095 100644 --- a/atos/common/sockets/client.cpp +++ b/atos/common/sockets/client.cpp @@ -9,47 +9,34 @@ using namespace SocketErrors; -Client::Client(const SocketType type, const bool debug) : Socket(type, debug) {} -Client::Client( - const SocketType type, - const Address& remoteAddr, - const Port port, - const bool debug) - : Client(type, debug) { +Client::Client(const SocketType type, const bool debug) : + Socket(type, debug) {} +Client::Client(const SocketType type, const Address& remoteAddr, const Port port, const bool debug) : + Client(type, debug) { connect(remoteAddr, port); } -TCPClient::TCPClient(const bool debug) : Client(STREAM, debug) {} -TCPClient::TCPClient( - const Address& remoteAddr, - const Port port, - const bool debug) - : Client(STREAM, remoteAddr, port, debug) { -} +TCPClient::TCPClient(const bool debug) : + Client(STREAM, debug) {} +TCPClient::TCPClient(const Address& remoteAddr, const Port port, const bool debug) : + Client(STREAM, remoteAddr, port, debug) {} -UDPClient::UDPClient(const bool debug) : Client(DATAGRAM, debug) {} -UDPClient::UDPClient( - const Address& remoteAddr, - const Port port, - const bool debug) - : Client(DATAGRAM, remoteAddr, port, debug) { -} +UDPClient::UDPClient(const bool debug) : + Client(DATAGRAM, debug) {} +UDPClient::UDPClient(const Address& remoteAddr, const Port port, const bool debug) : + Client(DATAGRAM, remoteAddr, port, debug) {} /*! * \brief Client::connect Connect the client to a remote host. * \param remoteAddr Address of the remote host. * \param port Port on the remote host. */ -void Client::connect( - const Address& remoteAddr, - const Port port) { +void Client::connect(const Address& remoteAddr, const Port port) { if (remoteAddr.empty() || port == 0) { throw ArgumentError("Empty address or port specified for connect call"); } auto addr = toSockaddr(remoteAddr, port); - if (::connect(mSockfd, reinterpret_cast(&addr), - sizeof (addr)) < 0) { + if (::connect(mSockfd, reinterpret_cast(&addr), sizeof(addr)) < 0) { throw SocketConnectError(errno); } } - diff --git a/atos/common/sockets/server.cpp b/atos/common/sockets/server.cpp index 15a9c7eba..3b5490b1b 100644 --- a/atos/common/sockets/server.cpp +++ b/atos/common/sockets/server.cpp @@ -10,18 +10,13 @@ using namespace SocketErrors; -Server::Server( - const SocketType type, - const bool debug) - : BasicSocket(type, debug) { +Server::Server(const SocketType type, const bool debug) : + BasicSocket(type, debug) { setReuseAddr(true); } -Server::Server( - const SocketType type, - const Address& localAddr, - const Port port, - const bool debug) : Server(type, debug) { +Server::Server(const SocketType type, const Address& localAddr, const Port port, const bool debug) : + Server(type, debug) { bind(type, toSockaddr(localAddr, port)); } @@ -30,11 +25,9 @@ Server::Server( * locally bound. * \param localAddr Local address to which server shall be bound. */ -void Server::bind(const SocketType type, - const sockaddr_in localAddr) { +void Server::bind(const SocketType type, const sockaddr_in localAddr) { - if (::bind(mSockfd, reinterpret_cast(&localAddr), - sizeof (localAddr)) >= 0) { + if (::bind(mSockfd, reinterpret_cast(&localAddr), sizeof(localAddr)) >= 0) { return; } if (errno == EBADF) { @@ -42,29 +35,24 @@ void Server::bind(const SocketType type, if ((mSockfd = ::socket(AF_INET, type, 0)) < 0) { throw SocketCreateError(errno); } - if (::bind(mSockfd, reinterpret_cast(&localAddr), - sizeof (localAddr)) < 0) { + if (::bind(mSockfd, reinterpret_cast(&localAddr), sizeof(localAddr)) < 0) { throw SocketBindError(errno); } - } - else if (errno == EINVAL) { - if (localAddr.sin_port == getLocalAddr().sin_port - && localAddr.sin_addr.s_addr == getLocalAddr().sin_addr.s_addr) { + } else if (errno == EINVAL) { + if (localAddr.sin_port == getLocalAddr().sin_port && + localAddr.sin_addr.s_addr == getLocalAddr().sin_addr.s_addr) { // Socket already bound and address same, do nothing return; - } - else { + } else { // Socket already bound and address different, rebind close(); return bind(type, localAddr); } - } - else { + } else { throw SocketBindError(errno); } } - /*! * \brief TCPServer::await Await and accept a connection attempt from * remote client. Assumes that the local address has been @@ -88,9 +76,7 @@ Socket TCPServer::await() { * \return A new ::Socket for the accepted connection, connected * to a single remote client. */ -Socket TCPServer::await( - const Address& localAddr, - const Port port) { +Socket TCPServer::await(const Address& localAddr, const Port port) { auto saddr = toSockaddr(localAddr, port); bind(STREAM, saddr); return await(); @@ -120,9 +106,8 @@ Socket TCPServer::accept() { throw DisconnectedError(); } sockaddr_in cliaddr; - socklen_t addrlen = sizeof (cliaddr); - auto conn = ::accept(mSockfd, reinterpret_cast(&cliaddr), - &addrlen); + socklen_t addrlen = sizeof(cliaddr); + auto conn = ::accept(mSockfd, reinterpret_cast(&cliaddr), &addrlen); if (conn < 0) { throw SocketAcceptError(errno); } @@ -134,8 +119,7 @@ Socket TCPServer::accept() { * the server. This also opens the port for incoming data. * \param localEndpoint Local address to which the server shall bind. */ -void UDPServer::bind( - const HostInfo &localEndpoint) { +void UDPServer::bind(const HostInfo& localEndpoint) { auto saddr = toSockaddr(localEndpoint); return Server::bind(DATAGRAM, saddr); } @@ -147,10 +131,9 @@ void UDPServer::bind( */ std::pair, BasicSocket::HostInfo> UDPServer::recvfrom() { sockaddr_in addr; - socklen_t socksize = sizeof (addr); - auto bytesRead = ::recvfrom(mSockfd, recvBuffer.data(), recvBuffer.size(), - 0, reinterpret_cast(&addr), - &socksize); + socklen_t socksize = sizeof(addr); + auto bytesRead = ::recvfrom( + mSockfd, recvBuffer.data(), recvBuffer.size(), 0, reinterpret_cast(&addr), &socksize); if (bytesRead < 0) { if (errno == EBADF) { throw DisconnectedError(); @@ -161,7 +144,7 @@ std::pair, BasicSocket::HostInfo> UDPServer::recvfrom() { throw DisconnectedError(); } auto first = recvBuffer.cbegin(); - auto last = recvBuffer.cbegin()+bytesRead; + auto last = recvBuffer.cbegin() + bytesRead; return {std::vector(first, last), fromSockaddr(addr)}; } @@ -170,8 +153,7 @@ std::pair, BasicSocket::HostInfo> UDPServer::recvfrom() { * the entire contents of the data vector. * \param data A std::pair of data and remote host info. */ -void UDPServer::sendto( - const std::pair, const HostInfo> &data) { +void UDPServer::sendto(const std::pair, const HostInfo>& data) { return sendto(data, data.first.size()); } @@ -180,16 +162,13 @@ void UDPServer::sendto( * \param data A std::pair of data and remote host info. * \param nBytes Number of bytes from the start of the data vector to send. */ -void UDPServer::sendto( - const std::pair, const HostInfo> &data, - const size_t nBytes) { +void UDPServer::sendto(const std::pair, const HostInfo>& data, const size_t nBytes) { if (nBytes > data.first.size()) { throw ArgumentError("Bytes to send exceeds data size"); } auto saddr = toSockaddr(data.second); - auto bytesSent = ::sendto(mSockfd, data.first.data(), nBytes, 0, - reinterpret_cast(&saddr), - sizeof (saddr)); + auto bytesSent = + ::sendto(mSockfd, data.first.data(), nBytes, 0, reinterpret_cast(&saddr), sizeof(saddr)); if (bytesSent < 0) { if (errno == EBADF) { throw DisconnectedError(); @@ -198,17 +177,11 @@ void UDPServer::sendto( } } -TCPServer::TCPServer(const bool debug) : Server(STREAM, debug) {} -TCPServer::TCPServer( - const Address& localAddr, - const Port port, - const bool debug) - : Server(STREAM, localAddr, port, debug) { -} -UDPServer::UDPServer(const bool debug) : Server(DATAGRAM, debug) {} -UDPServer::UDPServer( - const Address& localAddr, - const Port port, - const bool debug) - : Server(DATAGRAM, localAddr, port, debug) { -} +TCPServer::TCPServer(const bool debug) : + Server(STREAM, debug) {} +TCPServer::TCPServer(const Address& localAddr, const Port port, const bool debug) : + Server(STREAM, localAddr, port, debug) {} +UDPServer::UDPServer(const bool debug) : + Server(DATAGRAM, debug) {} +UDPServer::UDPServer(const Address& localAddr, const Port port, const bool debug) : + Server(DATAGRAM, localAddr, port, debug) {} diff --git a/atos/common/sockets/server.hpp b/atos/common/sockets/server.hpp index 387324a37..17a878273 100644 --- a/atos/common/sockets/server.hpp +++ b/atos/common/sockets/server.hpp @@ -23,9 +23,11 @@ class TCPServer : public Server { virtual Socket await(); virtual Socket await(const Address& localAddr, const Port port); + protected: virtual void listen(); virtual Socket accept(); + private: static const unsigned int MAX_QUEUED_CONNECTIONS = 10; }; @@ -40,8 +42,9 @@ class UDPServer : public Server { std::pair, HostInfo> recvfrom(); void sendto(const std::pair, const HostInfo>& data); void sendto(const std::pair, const HostInfo>& data, const size_t nBytes); + private: static const int IO_BUFFER_SIZE = 4096; - std::vector recvBuffer = std::vector(IO_BUFFER_SIZE); - std::vector sendBuffer = std::vector(IO_BUFFER_SIZE); + std::vector recvBuffer = std::vector(IO_BUFFER_SIZE); + std::vector sendBuffer = std::vector(IO_BUFFER_SIZE); }; diff --git a/atos/common/sockets/socket.cpp b/atos/common/sockets/socket.cpp index 0f7cdc38f..b02e9f7bb 100644 --- a/atos/common/sockets/socket.cpp +++ b/atos/common/sockets/socket.cpp @@ -5,28 +5,23 @@ */ #include "socket.hpp" -#include #include #include #include +#include using namespace SocketErrors; -BasicSocket::BasicSocket() { -} +BasicSocket::BasicSocket() {} -BasicSocket::BasicSocket( - const SocketType type, - const bool debug) { +BasicSocket::BasicSocket(const SocketType type, const bool debug) { if ((mSockfd = socket(AF_INET, type, 0)) < 0) { throw SocketCreateError(errno); } setDebug(debug); } -BasicSocket::BasicSocket( - const int sockfd, - const bool debug) { +BasicSocket::BasicSocket(const int sockfd, const bool debug) { mSockfd = sockfd; setDebug(debug); } @@ -35,40 +30,39 @@ BasicSocket::~BasicSocket() { close(); } -BasicSocket::BasicSocket( - const BasicSocket& other) { +BasicSocket::BasicSocket(const BasicSocket& other) { mSockfd = dup(other.mSockfd); - mDebug = other.mDebug; + mDebug = other.mDebug; } BasicSocket::BasicSocket(BasicSocket&& other) { - mSockfd = other.mSockfd; - mDebug = other.mDebug; + mSockfd = other.mSockfd; + mDebug = other.mDebug; other.mSockfd = -1; } -BasicSocket& BasicSocket::operator=( - const BasicSocket& other) { +BasicSocket& BasicSocket::operator=(const BasicSocket& other) { if (this != &other) { mSockfd = dup(other.mSockfd); - mDebug = other.mDebug; + mDebug = other.mDebug; } return *this; } -BasicSocket& BasicSocket::operator=(BasicSocket &&other) { +BasicSocket& BasicSocket::operator=(BasicSocket&& other) { if (this != &other) { - mSockfd = other.mSockfd; + mSockfd = other.mSockfd; other.mSockfd = -1; } return *this; } -Socket::Socket(Socket&& other) : BasicSocket(std::move(other)) { +Socket::Socket(Socket&& other) : + BasicSocket(std::move(other)) { recvBuffer = other.recvBuffer; sendBuffer = other.sendBuffer; } -Socket& Socket::operator=(Socket &&other) { +Socket& Socket::operator=(Socket&& other) { if (this != &other) { BasicSocket::operator=(std::move(other)); recvBuffer = other.recvBuffer; @@ -77,7 +71,6 @@ Socket& Socket::operator=(Socket &&other) { return *this; } - /*! * \brief BasicSocket::getLocalAddr Returns the current address to which the socket * is bound. @@ -85,9 +78,8 @@ Socket& Socket::operator=(Socket &&other) { */ sockaddr_in BasicSocket::getLocalAddr() const { sockaddr_in ret; - socklen_t addrlen = sizeof (ret); - if (getsockname(mSockfd, reinterpret_cast(&ret), - &addrlen) < 0) { + socklen_t addrlen = sizeof(ret); + if (getsockname(mSockfd, reinterpret_cast(&ret), &addrlen) < 0) { throw SocketGetSockNameError(errno); } return ret; @@ -101,8 +93,7 @@ sockaddr_in BasicSocket::getLocalAddr() const { sockaddr_in BasicSocket::getRemoteAddr() const { sockaddr_in ret; socklen_t addrlen; - if (getpeername(mSockfd, reinterpret_cast(&ret), - &addrlen) < 0) { + if (getpeername(mSockfd, reinterpret_cast(&ret), &addrlen) < 0) { throw SocketGetPeerNameError(errno); } return ret; @@ -113,11 +104,8 @@ sockaddr_in BasicSocket::getRemoteAddr() const { * \param option Option to manipulate. * \param value Value to set on the option. */ -void BasicSocket::setOption( - const SocketOption option, - const int value) { - if (setsockopt(mSockfd, SOL_SOCKET, option, - &value, sizeof (value)) < 0) { +void BasicSocket::setOption(const SocketOption option, const int value) { + if (setsockopt(mSockfd, SOL_SOCKET, option, &value, sizeof(value)) < 0) { throw SetSockOptError(mSocketOptionNames[option], errno); } } @@ -127,11 +115,8 @@ void BasicSocket::setOption( * \param option Option to manipulate. * \param value Value to set on the option. */ -void BasicSocket::setOption( - const SocketOption option, - const struct linger value) { - if (setsockopt(mSockfd, SOL_SOCKET, static_cast(option), - &value, sizeof (value)) < 0) { +void BasicSocket::setOption(const SocketOption option, const struct linger value) { + if (setsockopt(mSockfd, SOL_SOCKET, static_cast(option), &value, sizeof(value)) < 0) { throw SetSockOptError(mSocketOptionNames[option], errno); } } @@ -141,9 +126,7 @@ void BasicSocket::setOption( * \param option Option to manipulate. * \param value Value to set on the option. */ -void BasicSocket::setOption( - const FileOption option, - const bool value) { +void BasicSocket::setOption(const FileOption option, const bool value) { auto opt = fcntl(mSockfd, F_GETFL); if (opt < 0) { throw FileControlGetError(mFileOptionNames[option], errno); @@ -159,8 +142,8 @@ void BasicSocket::setOption( * \return Value of the option. */ int BasicSocket::getOption(const SocketOption option) const { - int retval = 0; - socklen_t retsize = sizeof (retval); + int retval = 0; + socklen_t retsize = sizeof(retval); if (getsockopt(mSockfd, SOL_SOCKET, option, &retval, &retsize) < 0) { throw GetSockOptError(mSocketOptionNames.at(option), errno); } @@ -185,8 +168,7 @@ int BasicSocket::getOption(const FileOption option) const { * socket. * \param enable Enable or disable debugging (defaults to true). */ -void BasicSocket::setDebug( - const bool enable) { +void BasicSocket::setDebug(const bool enable) { mDebug = enable; } @@ -195,8 +177,7 @@ void BasicSocket::setDebug( * for the socket. * \param reuseAddr Enable or disable reuse of address (defaults to true). */ -void BasicSocket::setReuseAddr( - const bool reuseAddr) { +void BasicSocket::setReuseAddr(const bool reuseAddr) { return setOption(REUSEADDR, reuseAddr); } @@ -206,8 +187,7 @@ void BasicSocket::setReuseAddr( * \param keepAlive Enable or disable sending of keep-alive messages * on connection-oriented sockets (defaults to true). */ -void BasicSocket::setKeepAlive( - const bool keepAlive) { +void BasicSocket::setKeepAlive(const bool keepAlive) { return setOption(KEEPALIVE, keepAlive); } @@ -219,8 +199,7 @@ void BasicSocket::setKeepAlive( * \param linger Enable or disable lingering for the socket * (defaults to true). */ -void BasicSocket::setLinger( - const bool linger) { +void BasicSocket::setLinger(const bool linger) { struct linger lingerOption; lingerOption.l_onoff = linger; return setOption(LINGER, lingerOption); @@ -233,11 +212,10 @@ void BasicSocket::setLinger( * for the socket. * \param seconds Timeout to set. */ -void BasicSocket::setLingerSeconds( - const int seconds) { +void BasicSocket::setLingerSeconds(const int seconds) { struct linger lingerOption; lingerOption.l_linger = std::max(seconds, 0); - lingerOption.l_onoff = seconds > 0; + lingerOption.l_onoff = seconds > 0; return setOption(LINGER, lingerOption); } @@ -246,8 +224,7 @@ void BasicSocket::setLingerSeconds( * the socket. * \param blocking Enable or disable blocking operation (defaults to true). */ -void BasicSocket::setBlocking( - const bool blocking) { +void BasicSocket::setBlocking(const bool blocking) { setOption(NONBLOCKING, !blocking); } @@ -302,8 +279,7 @@ void BasicSocket::open(const SocketType type) { BasicSocket::Address BasicSocket::getRemoteIP() const { char ipStr[INET_ADDRSTRLEN]; const auto remoteAddr = getRemoteAddr(); - if (inet_ntop(remoteAddr.sin_family, &remoteAddr.sin_addr, - ipStr, sizeof (ipStr)) == nullptr) { + if (inet_ntop(remoteAddr.sin_family, &remoteAddr.sin_addr, ipStr, sizeof(ipStr)) == nullptr) { throw NtopError(errno); } return Address(ipStr); @@ -334,16 +310,13 @@ Socket::Port BasicSocket::getLocalPort() const { * \param port Port to convert. * \return Struct representation of a socket address. */ -sockaddr_in BasicSocket::toSockaddr( - const Address &addr, - const Port port) { +sockaddr_in BasicSocket::toSockaddr(const Address& addr, const Port port) { sockaddr_in saddr; saddr.sin_family = AF_INET; - saddr.sin_port = htons(port); + saddr.sin_port = htons(port); if (addr.empty()) { saddr.sin_addr.s_addr = INADDR_ANY; - } - else { + } else { if (inet_pton(saddr.sin_family, addr.c_str(), &saddr.sin_addr) < 0) { throw PtonError(errno); } @@ -367,10 +340,9 @@ sockaddr_in BasicSocket::toSockaddr(const HostInfo& info) { * \param addr Struct representation of socket address. * \return String representation of socket address. */ -BasicSocket::HostInfo BasicSocket::fromSockaddr( - const struct sockaddr_in &addr) { +BasicSocket::HostInfo BasicSocket::fromSockaddr(const struct sockaddr_in& addr) { char ipAddr[INET6_ADDRSTRLEN]; - if (inet_ntop(addr.sin_family, &addr.sin_addr, ipAddr, sizeof (ipAddr)) == nullptr) { + if (inet_ntop(addr.sin_family, &addr.sin_addr, ipAddr, sizeof(ipAddr)) == nullptr) { throw NtopError(errno); } return {std::string(ipAddr), ntohs(addr.sin_port)}; @@ -384,10 +356,9 @@ std::vector Socket::recv(const MessageOption option) { auto bytesRead = ::recv(mSockfd, recvBuffer.data(), recvBuffer.size(), option); if (bytesRead > 0) { auto first = recvBuffer.cbegin(); - auto last = recvBuffer.cbegin()+bytesRead; + auto last = recvBuffer.cbegin() + bytesRead; return std::vector(first, last); - } - else if (bytesRead == 0) { + } else if (bytesRead == 0) { try { // Zero length datagrams are allowed in UDP if (getType() == DATAGRAM || recvBuffer.size() == 0) { @@ -396,12 +367,10 @@ std::vector Socket::recv(const MessageOption option) { } catch (GetSockOptError&) {} close(); throw DisconnectedError(); - } - else { + } else { if (errno == EBADF) { throw DisconnectedError(); - } - else if (!getBlocking() && (errno == EAGAIN || errno == EWOULDBLOCK)) { + } else if (!getBlocking() && (errno == EAGAIN || errno == EWOULDBLOCK)) { return std::vector(); } throw SocketRecvError(errno); @@ -413,10 +382,7 @@ std::vector Socket::recv(const MessageOption option) { * \param data Data to transmit. * \param nBytes Number of bytes to transmit from start of data vector. */ -void Socket::send( - const std::vector& data, - const size_t nBytes, - const MessageOption option) { +void Socket::send(const std::vector& data, const size_t nBytes, const MessageOption option) { if (nBytes > data.size()) { throw ArgumentError("Bytes to send exceeds data size"); } @@ -436,7 +402,7 @@ void Socket::send( * data buffer passed as parameter. * \param data Data to transmit. */ -void Socket::send(const std::vector &data, const MessageOption option) { +void Socket::send(const std::vector& data, const MessageOption option) { return send(data, data.size(), option); } @@ -446,8 +412,7 @@ void Socket::send(const std::vector &data, const MessageOption option) { * \param timeout Time to wait (seconds) before returning no data present. * \return True if data is present, false if not. */ -bool BasicSocket::haveInput( - const double timeout) { +bool BasicSocket::haveInput(const double timeout) { int status; fd_set fds; struct timeval tv; @@ -459,14 +424,11 @@ bool BasicSocket::haveInput( while (true) { if (!(status = select(mSockfd + 1, &fds, nullptr, nullptr, &tv))) { return false; - } - else if (status > 0 && FD_ISSET(mSockfd, &fds)) { + } else if (status > 0 && FD_ISSET(mSockfd, &fds)) { return true; - } - else if (status > 0) { + } else if (status > 0) { throw RuntimeError("???", errno); - } - else if (errno != EINTR) { + } else if (errno != EINTR) { throw SocketSelectError(errno); } } @@ -478,14 +440,13 @@ bool BasicSocket::haveInput( * \param timeout Time to spend flushing incoming data. * \return 0 if successful, -1 otherwise. */ -int BasicSocket::preCloseFlush( - const std::chrono::system_clock::duration& timeout) { +int BasicSocket::preCloseFlush(const std::chrono::system_clock::duration& timeout) { const auto start = std::chrono::system_clock::now(); char discard[99]; if (shutdown(mSockfd, SHUT_WR) != -1) { while (std::chrono::system_clock::now() < start + timeout) { while (haveInput(0.01)) { - if (!::read(mSockfd, discard, sizeof (discard))) { + if (!::read(mSockfd, discard, sizeof(discard))) { return 0; } } diff --git a/atos/common/sockets/socket.hpp b/atos/common/sockets/socket.hpp index a7e71003e..8fd85b7ea 100644 --- a/atos/common/sockets/socket.hpp +++ b/atos/common/sockets/socket.hpp @@ -8,32 +8,25 @@ #define SOCKET_DEBUG #include "socketexceptions.hpp" -#include #include -#include -#include -#include +#include #include +#include #include +#include #include +#include -class BasicSocket -{ +class BasicSocket { public: - typedef enum { - STREAM = SOCK_STREAM, - DATAGRAM = SOCK_DGRAM - } SocketType; + typedef enum { STREAM = SOCK_STREAM, DATAGRAM = SOCK_DGRAM } SocketType; typedef uint16_t Port; typedef std::string Address; typedef struct { Address address; Port port; } HostInfo; - typedef enum { - NO_OPTION = 0, - NOSIGNAL = MSG_NOSIGNAL - } MessageOption; + typedef enum { NO_OPTION = 0, NOSIGNAL = MSG_NOSIGNAL } MessageOption; BasicSocket(); BasicSocket(const SocketType type, const bool debug = false); @@ -55,36 +48,31 @@ class BasicSocket SocketType getType() const; Address getRemoteIP() const; - Address getLocalIP() const {throw std::runtime_error("Not implemented");} + Address getLocalIP() const { + throw std::runtime_error("Not implemented"); + } Port getRemotePort() const; Port getLocalPort() const; void close(); void open(const SocketType type); + protected: typedef enum { - DEBUG = SO_DEBUG, + DEBUG = SO_DEBUG, REUSEADDR = SO_REUSEADDR, KEEPALIVE = SO_KEEPALIVE, - LINGER = SO_LINGER, - TYPE = SO_TYPE + LINGER = SO_LINGER, + TYPE = SO_TYPE } SocketOption; - typedef enum { - NONBLOCKING = O_NONBLOCK - } FileOption; - std::map mSocketOptionNames = { - {DEBUG, "SO_DEBUG"}, - {REUSEADDR, "SO_REUSEADDR"}, - {KEEPALIVE, "SO_KEEPALIVE"}, - {LINGER, "SO_LINGER"}, - {TYPE, "SO_TYPE"} - }; - std::map mMessageOptionNames = { - {NOSIGNAL, "MSG_NOSIGNAL"} - }; - std::map mFileOptionNames = { - {NONBLOCKING, "O_NONBLOCK"} - }; + typedef enum { NONBLOCKING = O_NONBLOCK } FileOption; + std::map mSocketOptionNames = {{DEBUG, "SO_DEBUG"}, + {REUSEADDR, "SO_REUSEADDR"}, + {KEEPALIVE, "SO_KEEPALIVE"}, + {LINGER, "SO_LINGER"}, + {TYPE, "SO_TYPE"}}; + std::map mMessageOptionNames = {{NOSIGNAL, "MSG_NOSIGNAL"}}; + std::map mFileOptionNames = {{NONBLOCKING, "O_NONBLOCK"}}; void setOption(const SocketOption option, const int value); void setOption(const SocketOption option, const struct linger value); @@ -107,12 +95,11 @@ class BasicSocket int preCloseFlush(const std::chrono::system_clock::duration& timeout); }; -class Socket : public BasicSocket -{ +class Socket : public BasicSocket { public: - using BasicSocket::BasicSocket; - Socket(const Socket&) : BasicSocket() {} + Socket(const Socket&) : + BasicSocket() {} Socket(Socket&& other); Socket& operator=(const Socket& other); Socket& operator=(Socket&& other); @@ -123,9 +110,6 @@ class Socket : public BasicSocket private: static const int IO_BUFFER_SIZE = 4096; - std::vector recvBuffer = std::vector(IO_BUFFER_SIZE); - std::vector sendBuffer = std::vector(IO_BUFFER_SIZE); + std::vector recvBuffer = std::vector(IO_BUFFER_SIZE); + std::vector sendBuffer = std::vector(IO_BUFFER_SIZE); }; - - - diff --git a/atos/common/sockets/socketexceptions.hpp b/atos/common/sockets/socketexceptions.hpp index 8f08650fa..deb36142a 100644 --- a/atos/common/sockets/socketexceptions.hpp +++ b/atos/common/sockets/socketexceptions.hpp @@ -5,11 +5,11 @@ */ #pragma once -#include -#include #include #include +#include #include +#include namespace SocketErrors { @@ -22,36 +22,51 @@ class StdErrorPrinter { } }; -class ArgumentError : public std::invalid_argument, public StdErrorPrinter { -public: - ArgumentError() : std::invalid_argument("") {} - ArgumentError(const std::string& msg) - : std::invalid_argument(msg) {} - ArgumentError(const std::string& msg, const int errorNo) - : std::invalid_argument(msg + " (" + strerror(errorNo) + ")") {} +class ArgumentError + : public std::invalid_argument + , public StdErrorPrinter { +public: + ArgumentError() : + std::invalid_argument("") {} + ArgumentError(const std::string& msg) : + std::invalid_argument(msg) {} + ArgumentError(const std::string& msg, const int errorNo) : + std::invalid_argument(msg + " (" + strerror(errorNo) + ")") {} virtual ~ArgumentError() {} }; class NtopError final : public ArgumentError { public: - NtopError(const int errorNo) : ArgumentError("Failed to stringify IP address", errorNo) { perror("inet_ntop"); } + NtopError(const int errorNo) : + ArgumentError("Failed to stringify IP address", errorNo) { + perror("inet_ntop"); + } }; class PtonError final : public ArgumentError { public: - PtonError(const int errorNo) : ArgumentError("Failed to parse IP string", errorNo) { perror("inet_pton"); } + PtonError(const int errorNo) : + ArgumentError("Failed to parse IP string", errorNo) { + perror("inet_pton"); + } }; -class RuntimeError : public std::runtime_error, public StdErrorPrinter { -public: - RuntimeError() : std::runtime_error("") {} - RuntimeError(const std::string& msg) - : std::runtime_error(msg) {} - RuntimeError(const std::string& msg, const int errorNo) - : std::runtime_error(msg + " (" + strerror(errorNo) + ")") { +class RuntimeError + : public std::runtime_error + , public StdErrorPrinter { +public: + RuntimeError() : + std::runtime_error("") {} + RuntimeError(const std::string& msg) : + std::runtime_error(msg) {} + RuntimeError(const std::string& msg, const int errorNo) : + std::runtime_error(msg + " (" + strerror(errorNo) + ")") { mErrorNo = errorNo; } virtual ~RuntimeError() {} - int getErrorNo() const { return mErrorNo; } + int getErrorNo() const { + return mErrorNo; + } + private: int mErrorNo = 0; }; @@ -67,37 +82,53 @@ class DisconnectedError : public RuntimeError { */ class ConfigurationError : public RuntimeError { public: - ConfigurationError(const std::string& msg, const int errorNo) : RuntimeError(msg, errorNo) {} + ConfigurationError(const std::string& msg, const int errorNo) : + RuntimeError(msg, errorNo) {} }; class GetSockOptError final : public ConfigurationError { public: - GetSockOptError(const std::string& optionName, const int errorNo) - : ConfigurationError("Failed to get socket option " + optionName, errorNo) { perror("getsockopt"); } + GetSockOptError(const std::string& optionName, const int errorNo) : + ConfigurationError("Failed to get socket option " + optionName, errorNo) { + perror("getsockopt"); + } }; class SetSockOptError final : public ConfigurationError { public: - SetSockOptError(const std::string& optionName, const int errorNo) - : ConfigurationError("Failed to set socket option " + optionName, errorNo) { perror("setsockopt"); } + SetSockOptError(const std::string& optionName, const int errorNo) : + ConfigurationError("Failed to set socket option " + optionName, errorNo) { + perror("setsockopt"); + } }; class FileControlError : public ConfigurationError { public: - FileControlError(const std::string& msg, const std::string& optionName, const int errorNo) : ConfigurationError(msg + " for option " + optionName, errorNo) { perror("fcntl"); } + FileControlError(const std::string& msg, const std::string& optionName, const int errorNo) : + ConfigurationError(msg + " for option " + optionName, errorNo) { + perror("fcntl"); + } }; class FileControlGetError final : public FileControlError { public: - FileControlGetError(const std::string& optionName, const int errorNo) : FileControlError("Failed to get file status flags", optionName, errorNo) {} + FileControlGetError(const std::string& optionName, const int errorNo) : + FileControlError("Failed to get file status flags", optionName, errorNo) {} }; class FileControlSetError final : public FileControlError { public: - FileControlSetError(const std::string& optionName, const int errorNo) : FileControlError("Failed to set file status flags", optionName, errorNo) {} + FileControlSetError(const std::string& optionName, const int errorNo) : + FileControlError("Failed to set file status flags", optionName, errorNo) {} }; class SocketGetPeerNameError final : public ConfigurationError { public: - SocketGetPeerNameError(const int errorNo) : ConfigurationError("Failed to get remote socket address", errorNo) { perror("getpeername"); } + SocketGetPeerNameError(const int errorNo) : + ConfigurationError("Failed to get remote socket address", errorNo) { + perror("getpeername"); + } }; class SocketGetSockNameError final : public ConfigurationError { public: - SocketGetSockNameError(const int errorNo) : ConfigurationError("Failed to get bound local socket address", errorNo) { perror("getsockname"); } + SocketGetSockNameError(const int errorNo) : + ConfigurationError("Failed to get bound local socket address", errorNo) { + perror("getsockname"); + } }; /*! @@ -106,52 +137,65 @@ class SocketGetSockNameError final : public ConfigurationError { */ class SocketOperationError : public RuntimeError { public: - SocketOperationError(const std::string& opName, const int errorNo) - : RuntimeError("Failed at " + opName + " call", errorNo) { perror(opName.c_str()); } + SocketOperationError(const std::string& opName, const int errorNo) : + RuntimeError("Failed at " + opName + " call", errorNo) { + perror(opName.c_str()); + } }; class SocketCreateError final : public SocketOperationError { public: - SocketCreateError(const int errorNo) : SocketOperationError("socket", errorNo) {} + SocketCreateError(const int errorNo) : + SocketOperationError("socket", errorNo) {} }; class SocketRecvError final : public SocketOperationError { public: - SocketRecvError(const int errorNo) : SocketOperationError("recv", errorNo) {} + SocketRecvError(const int errorNo) : + SocketOperationError("recv", errorNo) {} }; class SocketRecvFromError final : public SocketOperationError { public: - SocketRecvFromError(const int errorNo) : SocketOperationError("recvfrom", errorNo) {} + SocketRecvFromError(const int errorNo) : + SocketOperationError("recvfrom", errorNo) {} }; class SocketSendError final : public SocketOperationError { public: - SocketSendError(const int errorNo) : SocketOperationError("send", errorNo) {} + SocketSendError(const int errorNo) : + SocketOperationError("send", errorNo) {} }; class SocketSendToError final : public SocketOperationError { public: - SocketSendToError(const int errorNo) : SocketOperationError("sendto", errorNo) {} + SocketSendToError(const int errorNo) : + SocketOperationError("sendto", errorNo) {} }; class SocketSelectError final : public SocketOperationError { public: - SocketSelectError(const int errorNo) : SocketOperationError("select", errorNo) {} + SocketSelectError(const int errorNo) : + SocketOperationError("select", errorNo) {} }; class SocketConnectError final : public SocketOperationError { public: - SocketConnectError(const int errorNo) : SocketOperationError("connect", errorNo) {} + SocketConnectError(const int errorNo) : + SocketOperationError("connect", errorNo) {} }; class SocketDuplicateError final : public SocketOperationError { public: - SocketDuplicateError(const int errorNo) : SocketOperationError("dup", errorNo) {} + SocketDuplicateError(const int errorNo) : + SocketOperationError("dup", errorNo) {} }; class SocketBindError final : public SocketOperationError { public: - SocketBindError(const int errorNo) : SocketOperationError("bind", errorNo) {} + SocketBindError(const int errorNo) : + SocketOperationError("bind", errorNo) {} }; class SocketListenError final : public SocketOperationError { public: - SocketListenError(const int errorNo) : SocketOperationError("listen", errorNo) {} + SocketListenError(const int errorNo) : + SocketOperationError("listen", errorNo) {} }; class SocketAcceptError final : public SocketOperationError { public: - SocketAcceptError(const int errorNo) : SocketOperationError("accept", errorNo) {} + SocketAcceptError(const int errorNo) : + SocketOperationError("accept", errorNo) {} }; -} +} // namespace SocketErrors diff --git a/atos/common/sockets/tcphandler.cpp b/atos/common/sockets/tcphandler.cpp index 42986395b..d9eb0b54c 100644 --- a/atos/common/sockets/tcphandler.cpp +++ b/atos/common/sockets/tcphandler.cpp @@ -6,8 +6,8 @@ /** * @file tcphandler.cpp - * @author Adam Eriksson - * @brief TCP handler class that can be either server or client. + * @author Adam Eriksson + * @brief TCP handler class that can be either server or client. * @version 0.1 * @date 2020-04-07 */ @@ -23,45 +23,39 @@ * If all variabels are define it as initialise a server or client. * create new struct sockaddr_in addr. * @param P Port number. - * @param IP IP addres. - * @param CorS Choose to create Server or Client. Acceptable Input is "Server" or "Client". + * @param IP IP addres. + * @param CorS Choose to create Server or Client. Acceptable Input is "Server" or "Client". * @param sockoptionServer Set options in setsockoption * @param flagsock Set flags on socket like O_NONBLOCK */ -TCPHandler::TCPHandler(int P, const std::string IP, std::string CorS, int sockoptionServer, int flag) -{ - PORT = P; - IPaddr = IP; - sockoption = sockoptionServer; - flagsock = flag; +TCPHandler::TCPHandler(int P, const std::string IP, std::string CorS, int sockoptionServer, int flag) { + PORT = P; + IPaddr = IP; + sockoption = sockoptionServer; + flagsock = flag; if (CorS == "Server") { - ClientorServer = CorS; - errorState = static_cast(CreateServer()); - } - else if (CorS == "Client") { - ClientorServer = CorS; - errorState = static_cast(CreateClient()); - } + ClientorServer = CorS; + errorState = static_cast(CreateServer()); + } else if (CorS == "Client") { + ClientorServer = CorS; + errorState = static_cast(CreateClient()); + } } /** * @brief Construct a new TCPHandler::TCPHandler object * But without the initialise variabels. * create new struct sockaddr_in addr. */ -TCPHandler::TCPHandler() -{ -} +TCPHandler::TCPHandler() {} /** * @brief Destroy the TCPHandler::TCPHandler object - * + * */ -TCPHandler::~TCPHandler() -{ +TCPHandler::~TCPHandler() { TCPHandlerclose(); } - /*! * \brief TCPHandler::TCPHandler Move constructor, invalidate * data of copied object after moving it to constructed @@ -69,134 +63,127 @@ TCPHandler::~TCPHandler() * \param other Object to be moved into constructed object */ TCPHandler::TCPHandler(TCPHandler&& other) : - PORT(other.PORT), - IPaddr(other.IPaddr), - sockoption(other.sockoption), - flagsock(other.flagsock), - ConnectionON(other.ConnectionON), - servaddr(other.servaddr), - cliaddr(other.cliaddr), - sockserver(other.sockserver), - sockfd(other.sockfd), - errorState(other.errorState), - ClientorServer(other.ClientorServer) -{ - other.sockfd = -1; - other.sockserver = -1; - other.errorState = FAILURE; + PORT(other.PORT), + IPaddr(other.IPaddr), + sockoption(other.sockoption), + flagsock(other.flagsock), + ConnectionON(other.ConnectionON), + servaddr(other.servaddr), + cliaddr(other.cliaddr), + sockserver(other.sockserver), + sockfd(other.sockfd), + errorState(other.errorState), + ClientorServer(other.ClientorServer) { + other.sockfd = -1; + other.sockserver = -1; + other.errorState = FAILURE; other.ConnectionON = -1; other.IPaddr.clear(); other.PORT = -1; - memset(&other.servaddr, 0, sizeof (other.servaddr)); - memset(&other.cliaddr, 0, sizeof (other.cliaddr)); + memset(&other.servaddr, 0, sizeof(other.servaddr)); + memset(&other.cliaddr, 0, sizeof(other.cliaddr)); } /** - * @brief Define the TCPHandler as a Client - * initialise the Client with port to read and write to. + * @brief Define the TCPHandler as a Client + * initialise the Client with port to read and write to. * Ipaddres to read from. - * + * * @param P port number - * @param IP IP addres - * @return int success = 1, fail = -1. + * @param IP IP addres + * @return int success = 1, fail = -1. */ -int TCPHandler::CreateClient(int P, const std::string IP) -{ - PORT = P; - IPaddr = IP; +int TCPHandler::CreateClient(int P, const std::string IP) { + PORT = P; + IPaddr = IP; return CreateClient(); } /** * @brief define TCPHandler as a client. - * need to initlise the port, before calling on this function. - * - * @return int success = 1, fail = -1. + * need to initlise the port, before calling on this function. + * + * @return int success = 1, fail = -1. */ -int TCPHandler::CreateClient() -{ +int TCPHandler::CreateClient() { if (PORT == -1) { - perror("\nError:TCPHandler CreateClient, Port number not defined in TCPhandler class\n"); - return(FAILURE); - } - if( ClientorServer== "Server") { - perror("\nError: TCPHandler CreateClient, Class already declared as Server \n"); + perror("\nError:TCPHandler CreateClient, Port number not defined in TCPhandler class\n"); + return (FAILURE); + } + if (ClientorServer == "Server") { + perror("\nError: TCPHandler CreateClient, Class already declared as Server \n"); return FAILURE; - } - ClientorServer = "Client"; + } + ClientorServer = "Client"; - if ( (sockfd = socket(AF_INET, SOCK_STREAM | flagsock, 0)) < 0 ) { - perror("Error: TCPHandler CreateClient: Socket creation failed"); + if ((sockfd = socket(AF_INET, SOCK_STREAM | flagsock, 0)) < 0) { + perror("Error: TCPHandler CreateClient: Socket creation failed"); return FAILURE; - } + } memset(&servaddr, 0, sizeof(servaddr)); - + servaddr.sin_family = AF_INET; - servaddr.sin_port = htons(PORT); - - if(!IPaddr.empty()) { - if(inet_pton(AF_INET, IPaddr.c_str(), &servaddr.sin_addr) <= 0) { - perror("\nError: TCPHandler CreateClient, Invalid address/ Address not supported \n"); + servaddr.sin_port = htons(PORT); + + if (!IPaddr.empty()) { + if (inet_pton(AF_INET, IPaddr.c_str(), &servaddr.sin_addr) <= 0) { + perror("\nError: TCPHandler CreateClient, Invalid address/ Address not supported \n"); return FAILURE; - } - } - else { - perror("\n Error:TCPHandler CreateClient, No Ipaddres declared for Client\n"); + } + } else { + perror("\n Error:TCPHandler CreateClient, No Ipaddres declared for Client\n"); return FAILURE; - } + } return TCPHandlerConnect(); } /** * @brief Define the TCPHandler as a Server. - * initialise the Server with port to read and write to. - * Ipaddres to read from + * initialise the Server with port to read and write to. + * Ipaddres to read from * create new struct sockaddr_in addr. - * + * * @param P Port number. * @param IP IP address - * @return int success = 1, fail = -1. + * @return int success = 1, fail = -1. */ -int TCPHandler::CreateServer(int P, std::string IP, int sockoption) -{ - PORT = P; - IPaddr = IP; +int TCPHandler::CreateServer(int P, std::string IP, int sockoption) { + PORT = P; + IPaddr = IP; if (ClientorServer == "Client") { - perror("\nError: TCPHandler CreateServer, Class already declared as Client \n"); - return(FAILURE); - } - + perror("\nError: TCPHandler CreateServer, Class already declared as Client \n"); + return (FAILURE); + } + ClientorServer = "Server"; - if ( (sockfd = socket(AF_INET, SOCK_STREAM |flagsock, 0)) < 0 ) { - perror("Error: TCPHandler CreateServer, Socket creation failed"); - return(FAILURE); - } - if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, &sockoption, sizeof(sockoption))) - { - perror("Error: TCPHandler CreateServer setsockopt failed"); - return(FAILURE); - } + if ((sockfd = socket(AF_INET, SOCK_STREAM | flagsock, 0)) < 0) { + perror("Error: TCPHandler CreateServer, Socket creation failed"); + return (FAILURE); + } + if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, &sockoption, sizeof(sockoption))) { + perror("Error: TCPHandler CreateServer setsockopt failed"); + return (FAILURE); + } memset(&cliaddr, 0, sizeof(cliaddr)); memset(&servaddr, 0, sizeof(servaddr)); servaddr.sin_family = AF_INET; - servaddr.sin_port = htons(PORT); + servaddr.sin_port = htons(PORT); if (!IPaddr.empty()) { - if(inet_pton(AF_INET, IPaddr.c_str(), &servaddr.sin_addr) <= 0) { - perror("\nError: TCPHandler CreateServer, Invalid address/ Address not supported \n"); + if (inet_pton(AF_INET, IPaddr.c_str(), &servaddr.sin_addr) <= 0) { + perror("\nError: TCPHandler CreateServer, Invalid address/ Address not supported \n"); return FAILURE; - } - } - else { - servaddr.sin_addr.s_addr = INADDR_ANY; - } + } + } else { + servaddr.sin_addr.s_addr = INADDR_ANY; + } - if(TCPHandlerbind() < 0) { + if (TCPHandlerbind() < 0) { ConnectionON = FAILURE; return FAILURE; - } - if(TCPHandlerListen() < 0) { + } + if (TCPHandlerListen() < 0) { ConnectionON = FAILURE; return FAILURE; - } + } return TCPHandlerAccept(); } @@ -204,184 +191,164 @@ int TCPHandler::CreateServer(int P, std::string IP, int sockoption) * @brief Create Server for the TCPhandler. * The port needs to be defined for this. Otherwise it will fail. * create new struct sockaddr_in addr. - * - * @return int success = 1, fail = -1. + * + * @return int success = 1, fail = -1. */ -int TCPHandler::CreateServer() -{ - if (PORT ==-1) { - perror("\nError:TCPHandler CreateServer, Port number not defined in TCPhandler class \n"); - return(FAILURE); - } +int TCPHandler::CreateServer() { + if (PORT == -1) { + perror("\nError:TCPHandler CreateServer, Port number not defined in TCPhandler class \n"); + return (FAILURE); + } if (ClientorServer == "Client") { - perror("\nError: TCPHandler CreateServer, Class already declared as Client \n"); + perror("\nError: TCPHandler CreateServer, Class already declared as Client \n"); return FAILURE; - } - + } + ClientorServer = "Server"; - if ( (sockfd = socket(AF_INET, SOCK_STREAM | flagsock, 0)) < 0 ) { - perror("Error: TCPHandler CreateServer, Socket creation failed"); - + if ((sockfd = socket(AF_INET, SOCK_STREAM | flagsock, 0)) < 0) { + perror("Error: TCPHandler CreateServer, Socket creation failed"); + return FAILURE; - } - if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, &sockoption, sizeof(sockoption))) { - perror("Error: TCPHandler CreateServer setsockopt failed"); - + } + if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, &sockoption, sizeof(sockoption))) { + perror("Error: TCPHandler CreateServer setsockopt failed"); + return FAILURE; - } + } memset(&cliaddr, 0, sizeof(cliaddr)); memset(&servaddr, 0, sizeof(servaddr)); servaddr.sin_family = AF_INET; - servaddr.sin_port = htons(PORT); - if(!IPaddr.empty()) { + servaddr.sin_port = htons(PORT); + if (!IPaddr.empty()) { if (inet_pton(AF_INET, IPaddr.c_str(), &servaddr.sin_addr) <= 0) { - perror("\nError: TCPHandler CreateServer, Invalid address/ Address not supported \n"); + perror("\nError: TCPHandler CreateServer, Invalid address/ Address not supported \n"); ConnectionON = FAILURE; return FAILURE; - } - } - else { - servaddr.sin_addr.s_addr = INADDR_ANY; - } + } + } else { + servaddr.sin_addr.s_addr = INADDR_ANY; + } - if(TCPHandlerbind() < 0) { + if (TCPHandlerbind() < 0) { ConnectionON = FAILURE; return FAILURE; - } + } - if(TCPHandlerListen() < 0) { + if (TCPHandlerListen() < 0) { ConnectionON = FAILURE; return FAILURE; - } - - return (TCPHandlerAccept()); + } + return (TCPHandlerAccept()); } /** * @brief Binds the socket in the TCPhandler - * - * @return int success = 1, fail = -1. + * + * @return int success = 1, fail = -1. */ -int TCPHandler::TCPHandlerbind() -{ - if ( bind(sockfd, (const struct sockaddr *)(&servaddr), sizeof(servaddr)) < 0 ) - { - perror("\n Error: TCPHandler TCPHandlerbind, bind failed \n"); +int TCPHandler::TCPHandlerbind() { + if (bind(sockfd, (const struct sockaddr*)(&servaddr), sizeof(servaddr)) < 0) { + perror("\n Error: TCPHandler TCPHandlerbind, bind failed \n"); return FAILURE; - } - - return(SUCCESS); + } + return (SUCCESS); } /** * @brief Connect to specific socket. - * - * @return int success = 1, fail = -1. + * + * @return int success = 1, fail = -1. */ -int TCPHandler::TCPHandlerConnect() -{ - if(connect(sockfd, (struct sockaddr *)(&servaddr), sizeof(servaddr)) < 0) - { - TCPHandlerclose(); - perror("\n Error: TCPHandler TCPHandlerConnect, Connect Failed \n"); +int TCPHandler::TCPHandlerConnect() { + if (connect(sockfd, (struct sockaddr*)(&servaddr), sizeof(servaddr)) < 0) { + TCPHandlerclose(); + perror("\n Error: TCPHandler TCPHandlerConnect, Connect Failed \n"); ConnectionON = FAILURE; return FAILURE; - - } + } ConnectionON = SUCCESS; return SUCCESS; } /** * @brief Listen for establishement of connection from the client - * - * @return int success = 1, fail = -1. + * + * @return int success = 1, fail = -1. */ -int TCPHandler::TCPHandlerListen() -{ - if(listen(sockfd, MAX_WAITING_CONNECTIONS) < 0) - { - perror("Error: TCPhandler TCPHandlerListen , Listen failed"); +int TCPHandler::TCPHandlerListen() { + if (listen(sockfd, MAX_WAITING_CONNECTIONS) < 0) { + perror("Error: TCPhandler TCPHandlerListen , Listen failed"); ConnectionON = FAILURE; return FAILURE; - } + } ConnectionON = SUCCESS; - return(SUCCESS); + return (SUCCESS); } /** * @brief Accept connection from client and establish a new socket for sending data. - * + * * @param time timer for poll function if 0 no poll waiting. * @return int int success = 1, fail = -1. */ -int TCPHandler::TCPHandlerAccept(int time) -{ +int TCPHandler::TCPHandlerAccept(int time) { socklen_t addrlen = sizeof(cliaddr); - struct pollfd pfd; - pfd.events = POLLIN; - pfd.fd = sockserver; - int timeout; + struct pollfd pfd; + pfd.events = POLLIN; + pfd.fd = sockserver; + int timeout; - if(flagsock !=SOCK_STREAM && time >= 0 ) - { - timeout = poll(&pfd, 1, time); - } - - sockserver = accept(sockfd, (struct sockaddr *)(&cliaddr), - &addrlen); + if (flagsock != SOCK_STREAM && time >= 0) { + timeout = poll(&pfd, 1, time); + } - if (sockserver < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) - { - ConnectionON = 0; - return ConnectionON; - } - else if (sockserver < 0){ - perror("Error: TCPHandler TCPHandlerAccept, Accept failed"); + sockserver = accept(sockfd, (struct sockaddr*)(&cliaddr), &addrlen); + + if (sockserver < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) { + ConnectionON = 0; + return ConnectionON; + } else if (sockserver < 0) { + perror("Error: TCPHandler TCPHandlerAccept, Accept failed"); ConnectionON = FAILURE; return FAILURE; - } + } ConnectionON = SUCCESS; - return(SUCCESS); + return (SUCCESS); } /** * @brief Closes the TCPhandler sockets. - * + * */ -void TCPHandler::TCPHandlerclose() -{ - if(sockfd) { - close(sockfd); - } - if(sockserver) { - close(sockserver); - } +void TCPHandler::TCPHandlerclose() { + if (sockfd) { + close(sockfd); + } + if (sockserver) { + close(sockserver); + } } /** * @brief Return the client address in a client server connection - * + * * @return std::string IP Address */ std::string TCPHandler::getClientIP() { - char IPstr [INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &(cliaddr.sin_addr), IPstr, INET_ADDRSTRLEN); - std::string clientIP (IPstr); - return (clientIP); + char IPstr[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(cliaddr.sin_addr), IPstr, INET_ADDRSTRLEN); + std::string clientIP(IPstr); + return (clientIP); } /** * @brief Returns the server in a client server connection - * + * * @return std::string IP Address */ std::string TCPHandler::getServerIP() { - char IPstr [INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &(servaddr.sin_addr), IPstr, INET_ADDRSTRLEN); - std::string serverIP (IPstr); - return (serverIP); + char IPstr[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(servaddr.sin_addr), IPstr, INET_ADDRSTRLEN); + std::string serverIP(IPstr); + return (serverIP); } - - - diff --git a/atos/common/sockets/tcphandler.hpp b/atos/common/sockets/tcphandler.hpp index 923c0da2a..d0bdae41d 100644 --- a/atos/common/sockets/tcphandler.hpp +++ b/atos/common/sockets/tcphandler.hpp @@ -15,38 +15,32 @@ #ifndef TCPHANDLER_H #define TCPHANDLER_H -#include -#include #include -#include -#include -#include -#include #include #include -#include #include +#include +#include +#include +#include +#include +#include +#include - -class [[deprecated("TCPHandler will be removed in a future release. Use TCPServer/TCPClient instead.")]] TCPHandler -{ +class [[deprecated("TCPHandler will be removed in a future release. Use TCPServer/TCPClient instead.")]] TCPHandler { public: + typedef enum { OFF, CLIENT, SERVER } OperationMode; - typedef enum { - OFF, - CLIENT, - SERVER - } OperationMode; - - typedef enum { - SUCCESS = 0, - FAILURE = -1 - } ErrorCode; + typedef enum { SUCCESS = 0, FAILURE = -1 } ErrorCode; TCPHandler(); - TCPHandler(int PORT, const std::string IPaddr, std::string ClientorServer= "off", int sockoption = 1, int flagsock = SOCK_STREAM); - - ~TCPHandler(); + TCPHandler(int PORT, + const std::string IPaddr, + std::string ClientorServer = "off", + int sockoption = 1, + int flagsock = SOCK_STREAM); + + ~TCPHandler(); TCPHandler(TCPHandler&& other); @@ -54,26 +48,24 @@ class [[deprecated("TCPHandler will be removed in a future release. Use TCPServe TCPHandler(const TCPHandler& other) = delete; int CreateClient(int PORT, const std::string IPaddr); - int CreateClient(); - int CreateServer(int PORT, const std::string IPaddr, int sockoption = 1 ); - int CreateServer(); - int TCPHandlerbind(); + int CreateClient(); + int CreateServer(int PORT, const std::string IPaddr, int sockoption = 1); + int CreateServer(); + int TCPHandlerbind(); template - int receiveTCP(std::vector &msg, int timeout) { + int receiveTCP(std::vector& msg, int timeout) { int bytesread; struct pollfd pfd; - assert(sizeof (T) == sizeof (char)); + assert(sizeof(T) == sizeof(char)); pfd.events = POLLIN; if (ClientorServer == "Server") { pfd.fd = sockserver; - } - else if (ClientorServer == "Client") { + } else if (ClientorServer == "Client") { pfd.fd = sockfd; - } - else { - std::cerr <<"\n TCP handler is not defined as Server nor Client\n"<< std::endl; + } else { + std::cerr << "\n TCP handler is not defined as Server nor Client\n" << std::endl; return FAILURE; } @@ -81,19 +73,18 @@ class [[deprecated("TCPHandler will be removed in a future release. Use TCPServe std::cout << "Timeout: " << timeout << ", fd: " << pfd.fd << std::endl; timeout = poll(&pfd, 1, timeout); if (timeout == 0) { - std::cout <<"\n TCP handler timed out in the recive call.\n Time before timeout is set to " - << timeout <<"ms" << std::endl; + std::cout << "\n TCP handler timed out in the recive call.\n Time before timeout is set to " << timeout + << "ms" << std::endl; return 0; - } - else if (timeout < 0) { + } else if (timeout < 0) { std::cerr << "\n TCP handler poll error" << std::endl; return FAILURE; } } - + if (timeout >= 0) { - - bytesread = recv(pfd.fd, msg.data() , msg.size(), MSG_DONTWAIT); + + bytesread = recv(pfd.fd, msg.data(), msg.size(), MSG_DONTWAIT); if (bytesread > 0) { return bytesread; } @@ -105,13 +96,11 @@ class [[deprecated("TCPHandler will be removed in a future release. Use TCPServe if (bytesread < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) { if (timeout == 0) { return 0; - } - else { + } else { std::cerr << "Poll indicated data present but no data was received" << std::endl; return FAILURE; } } - } return FAILURE; } @@ -119,56 +108,63 @@ class [[deprecated("TCPHandler will be removed in a future release. Use TCPServe template int sendTCP(const std::vector& msg) { int sendfd = 0; - assert(sizeof (T) == sizeof (char)); + assert(sizeof(T) == sizeof(char)); if (ClientorServer == "Server") { sendfd = sockserver; - } - else if (ClientorServer == "Client") { + } else if (ClientorServer == "Client") { sendfd = sockfd; - } - else { - std::cout <<"\n TCP handler is not defined as Server nor Client"<< std::endl; + } else { + std::cout << "\n TCP handler is not defined as Server nor Client" << std::endl; return FAILURE; } return send(sendfd, msg.data(), msg.size(), 0); } - int TCPHandlerListen(); - int TCPHandlerAccept(int time = 0); + int TCPHandlerListen(); + int TCPHandlerAccept(int time = 0); + + int TCPHandlerConnect(); + void TCPHandlerclose(); - int TCPHandlerConnect(); - void TCPHandlerclose(); - /** - * @brief returns std::string of the private variabel ClientorServer. Tells if class is declared as client or server. - * - */ - std::string getClientorServer()const {return ClientorServer; } - + * @brief returns std::string of the private variabel ClientorServer. Tells if class is declared as client or + * server. + * + */ + std::string getClientorServer() const { + return ClientorServer; + } + /** - * @brief get the value on connectionON private vairbel, - * which tells you if tcp connection is established - * - * @return int 1 = connection is established, -1 = no connection established. - */ - int getConnectionOn() const {return static_cast(ConnectionON);} - bool isConnected() const { return connected; } + * @brief get the value on connectionON private vairbel, + * which tells you if tcp connection is established + * + * @return int 1 = connection is established, -1 = no connection established. + */ + int getConnectionOn() const { + return static_cast(ConnectionON); + } + bool isConnected() const { + return connected; + } /** - * @brief Returns the class errorstate - * - * @return int 1 = setup of client or server succeded, -1 = setup of client or sever failed - */ - int getErrorState()const {return static_cast(errorState);} + * @brief Returns the class errorstate + * + * @return int 1 = setup of client or server succeded, -1 = setup of client or sever failed + */ + int getErrorState() const { + return static_cast(errorState); + } std::string getClientIP(); std::string getServerIP(); - /*Port number for the TCP handler*/ - int PORT = -1; - /*IPaddress for the TCP handler (optional)*/ + /*Port number for the TCP handler*/ + int PORT = -1; + /*IPaddress for the TCP handler (optional)*/ std::string IPaddr; /*If the TCP handler should send data as broadcast or not 1> is true */ - int sockoption = 1; - int flagsock = SOCK_STREAM; + int sockoption = 1; + int flagsock = SOCK_STREAM; // flag that tells us if TCP connection is established private: bool connected = false; @@ -177,14 +173,12 @@ class [[deprecated("TCPHandler will be removed in a future release. Use TCPServe struct sockaddr_in servaddr; struct sockaddr_in cliaddr; - int sockserver; - int sockfd; + int sockserver; + int sockfd; int ConnectionON; ErrorCode errorState = FAILURE; - - std::string ClientorServer; - + std::string ClientorServer; }; #endif diff --git a/atos/common/sockets/test_can.cpp b/atos/common/sockets/test_can.cpp index b3b653d0c..0bf1467d2 100644 --- a/atos/common/sockets/test_can.cpp +++ b/atos/common/sockets/test_can.cpp @@ -17,7 +17,7 @@ int main(int argc, char** argv) { // TODO test receive functionality can_frame frame; - frame.can_id = 0x123; + frame.can_id = 0x123; frame.data[0] = 0x0B; frame.data[1] = 0x0E; frame.data[2] = 0x0E; @@ -29,7 +29,7 @@ int main(int argc, char** argv) { assert(bytesSent > 0); // TODO exact byte count std::cout << "Sent " << bytesSent << " bytes" << std::endl; - frame.can_id = 0x100; + frame.can_id = 0x100; frame.data[0] = 0x0D; frame.data[1] = 0x0E; frame.data[2] = 0x0A; @@ -46,5 +46,4 @@ int main(int argc, char** argv) { std::cout << "Sent " << bytesSent << " bytes" << std::endl; std::cout << "CAN handler test successful" << std::endl; return 0; - } diff --git a/atos/common/sockets/test_socket.cpp b/atos/common/sockets/test_socket.cpp index cfc22b7dc..2e1975797 100644 --- a/atos/common/sockets/test_socket.cpp +++ b/atos/common/sockets/test_socket.cpp @@ -4,17 +4,16 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include "server.hpp" #include "client.hpp" -#include -#include +#include "server.hpp" #include +#include #include +#include static bool test_udp(); static bool test_loopback_data(); - static void run_udp_server(std::promise& ret, const std::string& localIP, const uint16_t port); static void run_tcp_server(std::promise& ret, const std::string& localIP, const uint16_t port); static void run_udp_client(std::promise& ret, const std::string& remoteIP, const uint16_t port); @@ -22,48 +21,64 @@ static void run_tcp_client(std::promise& ret, const std::string& remoteIP, int main(int, char**) { bool result = true; - //result &= test_udp(); - //exit(EXIT_FAILURE); + // result &= test_udp(); + // exit(EXIT_FAILURE); result &= test_loopback_data(); exit(result ? EXIT_SUCCESS : EXIT_FAILURE); } bool test_udp() { std::promise a; - //run_udp_client(a, "127.0.0.1", 12345); + // run_udp_client(a, "127.0.0.1", 12345); run_udp_server(a, "127.0.0.1", 12345); } bool test_loopback_data() { - std::string localIP = "127.0.0.1"; + std::string localIP = "127.0.0.1"; std::string remoteIP = "127.0.0.1"; - uint16_t tcpPort = 12345; - uint16_t udpPort = 12345; - auto timeout = std::chrono::seconds(1); - bool result = true; - - std::promise tsrp; //!< tcp server result promise - std::promise tcrp; //!< tcp client result promise - auto tsrf = tsrp.get_future(); //!< tcp server result future - auto tcrf = tcrp.get_future(); //!< tcp client result future - std::promise usrp; //!< udp server result promise - std::promise ucrp; //!< udp client result promise - auto usrf = usrp.get_future(); //!< udp server result future - auto ucrf = ucrp.get_future(); //!< udp client result future + uint16_t tcpPort = 12345; + uint16_t udpPort = 12345; + auto timeout = std::chrono::seconds(1); + bool result = true; + + std::promise tsrp; //!< tcp server result promise + std::promise tcrp; //!< tcp client result promise + auto tsrf = tsrp.get_future(); //!< tcp server result future + auto tcrf = tcrp.get_future(); //!< tcp client result future + std::promise usrp; //!< udp server result promise + std::promise ucrp; //!< udp client result promise + auto usrf = usrp.get_future(); //!< udp server result future + auto ucrf = ucrp.get_future(); //!< udp client result future std::thread tst(run_tcp_server, std::ref(tsrp), std::ref(localIP), tcpPort); std::thread tct(run_tcp_client, std::ref(tcrp), std::ref(remoteIP), tcpPort); std::thread ust(run_udp_server, std::ref(usrp), std::ref(localIP), udpPort); std::thread uct(run_udp_client, std::ref(ucrp), std::ref(remoteIP), udpPort); - try { result = tsrf.wait_for(timeout) == std::future_status::ready && tsrf.get(); } - catch (std::exception& e) { std::cout << "TCP server " << e.what() << std::endl; result = false; } - try { result = tcrf.wait_for(timeout) == std::future_status::ready && tcrf.get(); } - catch (std::exception& e) { std::cout << "TCP client " << e.what() << std::endl; result = false; } - try { result = usrf.wait_for(timeout) == std::future_status::ready && usrf.get(); } - catch (std::exception& e) { std::cout << "UDP server " << e.what() << std::endl; result = false; } - try { result = ucrf.wait_for(timeout) == std::future_status::ready && ucrf.get(); } - catch (std::exception& e) { std::cout << "UDP client " << e.what() << std::endl; result = false; } + try { + result = tsrf.wait_for(timeout) == std::future_status::ready && tsrf.get(); + } catch (std::exception& e) { + std::cout << "TCP server " << e.what() << std::endl; + result = false; + } + try { + result = tcrf.wait_for(timeout) == std::future_status::ready && tcrf.get(); + } catch (std::exception& e) { + std::cout << "TCP client " << e.what() << std::endl; + result = false; + } + try { + result = usrf.wait_for(timeout) == std::future_status::ready && usrf.get(); + } catch (std::exception& e) { + std::cout << "UDP server " << e.what() << std::endl; + result = false; + } + try { + result = ucrf.wait_for(timeout) == std::future_status::ready && ucrf.get(); + } catch (std::exception& e) { + std::cout << "UDP client " << e.what() << std::endl; + result = false; + } tst.join(); tct.join(); @@ -72,7 +87,6 @@ bool test_loopback_data() { return result; } - void run_udp_server(std::promise& ret, const std::string& localIP, const uint16_t port) { try { UDPServer udpServer(localIP, port, true); @@ -107,14 +121,11 @@ void run_tcp_server(std::promise& ret, const std::string& localIP, const u } } -void run_udp_client( - std::promise& ret, - const std::string& remoteIP, - const uint16_t port) { +void run_udp_client(std::promise& ret, const std::string& remoteIP, const uint16_t port) { try { UDPClient udpClient(remoteIP, port, true); std::string testString = "testing some UDP data"; - auto testData = std::vector(testString.begin(), testString.end()); + auto testData = std::vector(testString.begin(), testString.end()); udpClient.send(testData); auto rdata = udpClient.recv(); ret.set_value(testData == rdata); @@ -126,14 +137,11 @@ void run_udp_client( } } -void run_tcp_client( - std::promise& ret, - const std::string& remoteIP, - const uint16_t port) { +void run_tcp_client(std::promise& ret, const std::string& remoteIP, const uint16_t port) { try { TCPClient tcpClient(remoteIP, port, true); std::string testString = "testing some TCP data"; - auto testData = std::vector(testString.begin(), testString.end()); + auto testData = std::vector(testString.begin(), testString.end()); tcpClient.send(testData); auto rdata = tcpClient.recv(); ret.set_value(testData == rdata); diff --git a/atos/common/sockets/udphandler.cpp b/atos/common/sockets/udphandler.cpp index a985a3519..c4d37a37d 100644 --- a/atos/common/sockets/udphandler.cpp +++ b/atos/common/sockets/udphandler.cpp @@ -6,8 +6,8 @@ /** * @file udphandler.cpp - * @author Adam Eriksson - * @brief UDP handler class that can be either server or client. + * @author Adam Eriksson + * @brief UDP handler class that can be either server or client. * @version 0.1 * @date 2020-04-07 */ @@ -16,7 +16,6 @@ #include #include - #include "udphandler.hpp" /** @@ -25,42 +24,33 @@ * create new struct sockaddr_in servaddr. * @param P Port number. * @param IP IP address. - * @param b UDP broadcast option, if b>0 then it creates a brodcast client or server. - * @param CorS Choose to create Server or Client. Acceptable Input is "Server" or "Client". + * @param b UDP broadcast option, if b>0 then it creates a brodcast client or server. + * @param CorS Choose to create Server or Client. Acceptable Input is "Server" or "Client". */ -UDPHandler::UDPHandler(int P, const std::string IP, int b, std::string CorS) -{ - PORT = P; - IPaddr = IP; - broadcast = b; - - if (CorS == "Server") - { - ClientorServer = CorS; - errorState = static_cast(CreateServer()); - } - else if (CorS == "Client") - { - ClientorServer = CorS; - errorState = static_cast(CreateClient()); - } - - +UDPHandler::UDPHandler(int P, const std::string IP, int b, std::string CorS) { + PORT = P; + IPaddr = IP; + broadcast = b; + + if (CorS == "Server") { + ClientorServer = CorS; + errorState = static_cast(CreateServer()); + } else if (CorS == "Client") { + ClientorServer = CorS; + errorState = static_cast(CreateClient()); + } } /** * @brief Construct a new UDPHandler::UDPHandler object * But without the initialise variabels. * create new struct sockaddr_in servaddr. */ -UDPHandler::UDPHandler() -{ -} +UDPHandler::UDPHandler() {} /** * @brief Destroy the UDPHandler::UDPHandler object - * + * */ -UDPHandler::~UDPHandler() -{ +UDPHandler::~UDPHandler() { UDPHandlerclose(); } @@ -71,100 +61,92 @@ UDPHandler::~UDPHandler() * \param other Object to be moved into constructed object */ UDPHandler::UDPHandler(UDPHandler&& other) : - PORT(other.PORT), - IPaddr(other.IPaddr), - broadcast(other.broadcast), - servaddr(other.servaddr), - cliaddr(other.cliaddr), - sockfd(other.sockfd), - ClientorServer(other.ClientorServer), - errorState(other.errorState) -{ - other.sockfd = -1; + PORT(other.PORT), + IPaddr(other.IPaddr), + broadcast(other.broadcast), + servaddr(other.servaddr), + cliaddr(other.cliaddr), + sockfd(other.sockfd), + ClientorServer(other.ClientorServer), + errorState(other.errorState) { + other.sockfd = -1; other.errorState = UDPHANDLER_FAIL; other.IPaddr.clear(); - other.PORT = -1; + other.PORT = -1; other.broadcast = -1; - memset(&other.servaddr, 0, sizeof (other.servaddr)); - memset(&other.cliaddr, 0, sizeof (other.cliaddr)); + memset(&other.servaddr, 0, sizeof(other.servaddr)); + memset(&other.cliaddr, 0, sizeof(other.cliaddr)); } - /** - * @brief Define the UDPHandler as a Client - * initialise the Client with port to read and write to. - * Ipaddres to read from and if it should broadcast or not. - * + * @brief Define the UDPHandler as a Client + * initialise the Client with port to read and write to. + * Ipaddres to read from and if it should broadcast or not. + * * @param P port number - * @param IP IP addres + * @param IP IP addres * @param b if b>0 then the client is a brodcast client - * @return int success = 1, fail = -1. + * @return int success = 1, fail = -1. */ -int UDPHandler::CreateClient(int P, const std::string IP, int b) -{ - PORT = P; - IPaddr = IP; - broadcast = b; +int UDPHandler::CreateClient(int P, const std::string IP, int b) { + PORT = P; + IPaddr = IP; + broadcast = b; return CreateClient(); } /** * @brief define UDPHandler as a client. - * need to initlise the port, before calling on this function. - * - * @return int success = 1, fail = -1. + * need to initlise the port, before calling on this function. + * + * @return int success = 1, fail = -1. */ -int UDPHandler::CreateClient() -{ +int UDPHandler::CreateClient() { if (PORT == -1) { - perror("\nError: UDPHandler CreateClient, Port number not defined in UDPhandler class\n"); - return(UDPHANDLER_FAIL); - } - if( ClientorServer== "Server") { - perror("\nError: UDPHandler CreateClient, Class already declared as Server \n"); - return(UDPHANDLER_FAIL); - } - ClientorServer = "Client"; + perror("\nError: UDPHandler CreateClient, Port number not defined in UDPhandler class\n"); + return (UDPHANDLER_FAIL); + } + if (ClientorServer == "Server") { + perror("\nError: UDPHandler CreateClient, Class already declared as Server \n"); + return (UDPHANDLER_FAIL); + } + ClientorServer = "Client"; - if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) { - perror("Error: UDPHandler CreateClient, Socket creation failed"); - return(UDPHANDLER_FAIL); - } - if (broadcast>0) { - if(setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { - perror("Error: UDPHandler CreateClient, Setsocket to bradcast failed"); - } - } + if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + perror("Error: UDPHandler CreateClient, Socket creation failed"); + return (UDPHANDLER_FAIL); + } + if (broadcast > 0) { + if (setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { + perror("Error: UDPHandler CreateClient, Setsocket to bradcast failed"); + } + } memset(&servaddr, 0, sizeof(servaddr)); - + servaddr.sin_family = AF_INET; if (PORT < USHRT_MAX) { servaddr.sin_port = htons(static_cast(PORT)); - } - else { + } else { std::cerr << "UDP port configuration invalid: " << PORT << std::endl; return UDPHANDLER_FAIL; } - - - return( setIP(IPaddr)); + return (setIP(IPaddr)); } /** * @brief Define the UDPHandler as a Server. - * initialise the Server with port to read and write to. - * Ipaddres to read from and if it should broadcast or not. + * initialise the Server with port to read and write to. + * Ipaddres to read from and if it should broadcast or not. * create new struct sockaddr_in cliaddr. - * + * * @param P Port number. * @param IP IP address * @param b brodcast option brodcast if b>0 - * @return int success = 1, fail = -1. + * @return int success = 1, fail = -1. */ -int UDPHandler::CreateServer(int P, const std::string IP, int b) -{ - PORT =P; - IPaddr = IP; +int UDPHandler::CreateServer(int P, const std::string IP, int b) { + PORT = P; + IPaddr = IP; broadcast = b; return CreateServer(); @@ -173,144 +155,132 @@ int UDPHandler::CreateServer(int P, const std::string IP, int b) * @brief Create Server for the UDphandler. * The port needs to be defined for this. Otherwise it will fail. * create new struct sockaddr_in cliaddr. - * - * @return int success = 1, fail = -1. + * + * @return int success = 1, fail = -1. */ -int UDPHandler::CreateServer() -{ - if(PORT ==-1) { - perror("\nError: UDPHandler CreateServer, Port number not defined in UDPhandler class \n"); - return(UDPHANDLER_FAIL); - } - if(ClientorServer == "Client") { - perror("\nError: UDPHandler CreateServer, Class already declared as Client \n"); - return(UDPHANDLER_FAIL); - } - +int UDPHandler::CreateServer() { + if (PORT == -1) { + perror("\nError: UDPHandler CreateServer, Port number not defined in UDPhandler class \n"); + return (UDPHANDLER_FAIL); + } + if (ClientorServer == "Client") { + perror("\nError: UDPHandler CreateServer, Class already declared as Client \n"); + return (UDPHANDLER_FAIL); + } + ClientorServer = "Server"; - if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) { - perror("Error: UDPHandler CreateServer, Socket creation failed"); - return(UDPHANDLER_FAIL); - } + if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + perror("Error: UDPHandler CreateServer, Socket creation failed"); + return (UDPHANDLER_FAIL); + } - if(setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { - perror("Error: UDPHandler CreateServer, Setsocket to bradcast failed "); + if (setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { + perror("Error: UDPHandler CreateServer, Setsocket to bradcast failed "); return UDPHANDLER_FAIL; - } + } memset(&servaddr, 0, sizeof(servaddr)); memset(&cliaddr, 0, sizeof(cliaddr)); servaddr.sin_family = AF_INET; if (PORT < USHRT_MAX) { servaddr.sin_port = htons(static_cast(PORT)); - } - else { + } else { std::cerr << "UDP port configuration invalid: " << PORT << std::endl; return UDPHANDLER_FAIL; } - - return(setIP(IPaddr)); - + return (setIP(IPaddr)); } - /** * @brief Binds the socket in the udphandler - * - * @return int success = 1, fail = -1. + * + * @return int success = 1, fail = -1. */ -int UDPHandler::UDPHandlerbind() -{ - if ( bind(sockfd, (const struct sockaddr *)(&servaddr), sizeof(servaddr)) < 0 ) { - perror("Error: UDPHandler bind failed "); - return(UDPHANDLER_FAIL); - } - - return(UDPHANDLER_SUCCESS); +int UDPHandler::UDPHandlerbind() { + if (bind(sockfd, (const struct sockaddr*)(&servaddr), sizeof(servaddr)) < 0) { + perror("Error: UDPHandler bind failed "); + return (UDPHANDLER_FAIL); + } + return (UDPHANDLER_SUCCESS); } /** * @brief Connect to specific socket. - * - * @return int success = 1, fail = -1. + * + * @return int success = 1, fail = -1. */ -int UDPHandler::UDPHandlerConnect() -{ - // Need to disolve the existing association for the connect call/socket, when using the same client to reconnect to a new IP - // with the connect or sendto functions. - // Therefore tempoary change the socket behaviour to AF_UNSPEC, before trying to connect to another IP again. read "man connect" in Unix for more info - int res = -1; - struct sockaddr tmp; - memset(&tmp, 0, sizeof(tmp)); - tmp.sa_family = AF_UNSPEC; - res = connect (sockfd, (const struct sockaddr*) (&tmp), sizeof(tmp)); - res = connect(sockfd, (const struct sockaddr *)(&servaddr), sizeof(servaddr)); - if (res < 0) { - printf("Errno in UDPHandler UDPhandlerConnect! %s\n", strerror(errno)); - return res; - } +int UDPHandler::UDPHandlerConnect() { + // Need to disolve the existing association for the connect call/socket, when using the same client to reconnect to + // a new IP with the connect or sendto functions. Therefore tempoary change the socket behaviour to AF_UNSPEC, + // before trying to connect to another IP again. read "man connect" in Unix for more info + int res = -1; + struct sockaddr tmp; + memset(&tmp, 0, sizeof(tmp)); + tmp.sa_family = AF_UNSPEC; + res = connect(sockfd, (const struct sockaddr*)(&tmp), sizeof(tmp)); + res = connect(sockfd, (const struct sockaddr*)(&servaddr), sizeof(servaddr)); + if (res < 0) { + printf("Errno in UDPHandler UDPhandlerConnect! %s\n", strerror(errno)); + return res; + } - return res; + return res; } /** * @brief Closes the UDphandler socket. - * + * */ -void UDPHandler::UDPHandlerclose() -{ - close(sockfd); +void UDPHandler::UDPHandlerclose() { + close(sockfd); } /** * @brief Set the new IP/interface given the sockets and parts already configured - * + * * @param IPaddr std::string IPaddres to bind the class to */ int UDPHandler::setIP(const std::string IPaddr) { - if(!IPaddr.empty()) { - if(inet_pton(AF_INET, IPaddr.c_str(), &servaddr.sin_addr)<=0) { - perror("\nError: UDPHandler CreateClient, Invalid address/ Address not supported \n"); - return UDPHANDLER_FAIL; - } - } - else { - servaddr.sin_addr.s_addr = INADDR_ANY; - } - - if(ClientorServer == "Client") { - return (UDPHandlerConnect()); - } - else if (ClientorServer == "Server") { - return (UDPHandlerbind()); - } - else { - perror("\nError: UDPHandler not Configured properly, set Client or Server"); - return UDPHANDLER_FAIL; - } + if (!IPaddr.empty()) { + if (inet_pton(AF_INET, IPaddr.c_str(), &servaddr.sin_addr) <= 0) { + perror("\nError: UDPHandler CreateClient, Invalid address/ Address not supported \n"); + return UDPHANDLER_FAIL; + } + } else { + servaddr.sin_addr.s_addr = INADDR_ANY; + } + + if (ClientorServer == "Client") { + return (UDPHandlerConnect()); + } else if (ClientorServer == "Server") { + return (UDPHandlerbind()); + } else { + perror("\nError: UDPHandler not Configured properly, set Client or Server"); + return UDPHANDLER_FAIL; + } } /** * @brief Return the client address in a client server connection - * + * * @return std::string IP Address */ -std::string UDPHandler::getClientIP (){ - char IPstr [INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &(cliaddr.sin_addr), IPstr, INET_ADDRSTRLEN); - std::string clientIP (IPstr); - return (clientIP); +std::string UDPHandler::getClientIP() { + char IPstr[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(cliaddr.sin_addr), IPstr, INET_ADDRSTRLEN); + std::string clientIP(IPstr); + return (clientIP); } /** * @brief Returns the server in a client server connection - * + * * @return std::string IP Address */ -std::string UDPHandler::getServerIP (){ - char IPstr [INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &(servaddr.sin_addr), IPstr, INET_ADDRSTRLEN); - std::string serverIP (IPstr); - return (serverIP); +std::string UDPHandler::getServerIP() { + char IPstr[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &(servaddr.sin_addr), IPstr, INET_ADDRSTRLEN); + std::string serverIP(IPstr); + return (serverIP); } diff --git a/atos/common/sockets/udphandler.hpp b/atos/common/sockets/udphandler.hpp index d54ea6a9b..96b77219e 100644 --- a/atos/common/sockets/udphandler.hpp +++ b/atos/common/sockets/udphandler.hpp @@ -7,57 +7,49 @@ #ifndef UDPHANDLER_H #define UDPHANDLER_H -#include -#include +#include #include +#include #include +#include #include -#include -#include +#include -class [[deprecated("UDPHandler will be removed in a future release. Use UDPServer/UDPClient instead.")]] UDPHandler -{ +class [[deprecated("UDPHandler will be removed in a future release. Use UDPServer/UDPClient instead.")]] UDPHandler { public: + typedef enum { UDPHANDLER_SUCCESS = 1, UDPHANDLER_FAIL = -1 } ERROR_UDPHANDLER; - typedef enum { - UDPHANDLER_SUCCESS = 1, - UDPHANDLER_FAIL = -1 - } ERROR_UDPHANDLER; + UDPHandler(); + UDPHandler(int PORT, const std::string IPaddr, int broadcast, std::string ClientorServer = "off"); - UDPHandler(); - UDPHandler(int PORT, const std::string IPaddr, int broadcast, std::string ClientorServer= "off"); - - ~UDPHandler(); + ~UDPHandler(); UDPHandler(UDPHandler&& other); //! Copy constructor unexpected for file descriptors UDPHandler(const UDPHandler& other) = delete; - int CreateClient(int PORT, const std::string IPaddr, int broadcast = 0); - int CreateClient(); + int CreateClient(); int CreateServer(int PORT, const std::string IPaddr, int broadcast = 0); - int CreateServer(); - int UDPHandlerbind(); + int CreateServer(); + int UDPHandlerbind(); template - int receiveUDP(std::vector &msg) { + int receiveUDP(std::vector& msg) { int bytesread = 0; socklen_t len; - assert(sizeof (T) == sizeof (char)); + assert(sizeof(T) == sizeof(char)); struct sockaddr* addr; if (ClientorServer == "Server") { - len = sizeof(cliaddr); - addr = reinterpret_cast(&cliaddr); - } - else if (ClientorServer == "Client") { - len = sizeof(servaddr); - addr = reinterpret_cast(&servaddr); - } - else { - std::cerr <<"\n UDP handler is not defined as Server nor Client\n"<< std::endl; + len = sizeof(cliaddr); + addr = reinterpret_cast(&cliaddr); + } else if (ClientorServer == "Client") { + len = sizeof(servaddr); + addr = reinterpret_cast(&servaddr); + } else { + std::cerr << "\n UDP handler is not defined as Server nor Client\n" << std::endl; return UDPHANDLER_FAIL; } @@ -66,7 +58,7 @@ class [[deprecated("UDPHandler will be removed in a future release. Use UDPServe return bytesread; } if (bytesread == 0) { - std::cerr <<"\n UDP socket closed unexpectedly\n"<< std::endl; + std::cerr << "\n UDP socket closed unexpectedly\n" << std::endl; return UDPHANDLER_FAIL; } if (bytesread < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) { @@ -76,70 +68,73 @@ class [[deprecated("UDPHandler will be removed in a future release. Use UDPServe } template - long sendUDP(const std::vector &msg) { - socklen_t len = 0; - struct sockaddr * addr = nullptr; - assert(sizeof (T) == sizeof (char)); + long sendUDP(const std::vector& msg) { + socklen_t len = 0; + struct sockaddr* addr = nullptr; + assert(sizeof(T) == sizeof(char)); if (ClientorServer == "Server") { - len = sizeof(servaddr); - addr = reinterpret_cast(&cliaddr); - } - else if (ClientorServer == "Client") { - len = sizeof(cliaddr); - addr = reinterpret_cast(&servaddr); - } - else { - std::cout <<"\n UDP handler is not defined as Server nor Client, is defined as: "<(&cliaddr); + } else if (ClientorServer == "Client") { + len = sizeof(cliaddr); + addr = reinterpret_cast(&servaddr); + } else { + std::cout << "\n UDP handler is not defined as Server nor Client, is defined as: " << ClientorServer + << std::endl; return -1; } ssize_t res = -1; - res = sendto(sockfd, msg.data(), msg.size(), 0, addr, len); - if (res < 0){ + res = sendto(sockfd, msg.data(), msg.size(), 0, addr, len); + if (res < 0) { printf("Errno in UDPHandler SendUDP! %s\n", strerror(errno)); } - return res; } int UDPHandlerConnect(); - void UDPHandlerclose(); + void UDPHandlerclose(); /** - * @brief returns std::string of the private variabel ClientorServer. Tells if class is declared as client or server. - * - */ - std::string getClientorServer()const {return ClientorServer;} + * @brief returns std::string of the private variabel ClientorServer. Tells if class is declared as client or + * server. + * + */ + std::string getClientorServer() const { + return ClientorServer; + } /** - * @brief Get the state of the class. - * - * @return int 1 if clint/server setup succeded, -1 clint/server setup failed. - */ - int getErrorState() const {return static_cast(errorState);} + * @brief Get the state of the class. + * + * @return int 1 if clint/server setup succeded, -1 clint/server setup failed. + */ + int getErrorState() const { + return static_cast(errorState); + } int setIP(const std::string IP); /** * @brief returns class variabel IPaddr - * + * * @return std::string "Ipaddr that the was set wehn configuring client or server" */ - std::string getIPaddr() const {return static_cast(IPaddr);} - - std::string getClientIP (); - std::string getServerIP (); - /*Port number for the UDP handler*/ - int PORT = -1; - /*IPaddress for the UDP handler (optional)*/ - + std::string getIPaddr() const { + return static_cast(IPaddr); + } + + std::string getClientIP(); + std::string getServerIP(); + /*Port number for the UDP handler*/ + int PORT = -1; + /*IPaddress for the UDP handler (optional)*/ + std::string IPaddr; // TODO: Move to private given no function is usiong it atm - /*If the UDP handler should send data as brodcast or not 1> is true */ - int broadcast = -1; - + /*If the UDP handler should send data as brodcast or not 1> is true */ + int broadcast = -1; + private: struct sockaddr_in servaddr; struct sockaddr_in cliaddr; - int sockfd; - std::string ClientorServer; + int sockfd; + std::string ClientorServer; ERROR_UDPHANDLER errorState = UDPHANDLER_FAIL; - - }; #endif diff --git a/atos/common/testUtils/testUtils.hpp b/atos/common/testUtils/testUtils.hpp index cdecae6ab..cac546c94 100644 --- a/atos/common/testUtils/testUtils.hpp +++ b/atos/common/testUtils/testUtils.hpp @@ -6,86 +6,74 @@ #pragma once +#include "module.hpp" #include #include #include #include -#include "module.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -namespace testUtils -{ +namespace testUtils { // Sleep for timeout ms or until a subscriber has registered for the topic // if toBeAvailable is true, then it will wait for the number of // subscribers to be > 0, if false it will wait for the number of // subscribers to be 0 -void waitForSubscriber( - std::shared_ptr node, - const std::string & topicName, - bool toBeAvailable = true, - std::chrono::milliseconds timeout = std::chrono::milliseconds(1), - std::chrono::microseconds sleepPeriod = std::chrono::seconds(1)) -{ - using std::chrono::duration_cast; - using std::chrono::microseconds; - using std::chrono::steady_clock; - auto start = steady_clock::now(); - microseconds timeSlept(0); - auto predicate = [&node, &topicName, &toBeAvailable]() -> bool { - if (toBeAvailable) { - // the subscriber is available if the count is gt 0 - return node->count_subscribers(topicName) > 0; - } else { - // the subscriber is no longer available when the count is 0 - return node->count_subscribers(topicName) == 0; - } - }; - while (!predicate() && timeSlept < duration_cast(timeout)) { - rclcpp::Event::SharedPtr graphEvent = node->get_graph_event(); - node->wait_for_graph_change(graphEvent, sleepPeriod); - timeSlept = duration_cast(steady_clock::now() - start); - } - int64_t timeSleptCount = - std::chrono::duration_cast(timeSlept).count(); - printf( - "Waited %" PRId64 " microseconds for the subscriber to %s topic '%s'\n", - timeSleptCount, toBeAvailable ? "connect to" : "disconnect from", - topicName.c_str()); +void waitForSubscriber(std::shared_ptr node, + const std::string& topicName, + bool toBeAvailable = true, + std::chrono::milliseconds timeout = std::chrono::milliseconds(1), + std::chrono::microseconds sleepPeriod = std::chrono::seconds(1)) { + using std::chrono::duration_cast; + using std::chrono::microseconds; + using std::chrono::steady_clock; + auto start = steady_clock::now(); + microseconds timeSlept(0); + auto predicate = [&node, &topicName, &toBeAvailable]() -> bool { + if (toBeAvailable) { + // the subscriber is available if the count is gt 0 + return node->count_subscribers(topicName) > 0; + } else { + // the subscriber is no longer available when the count is 0 + return node->count_subscribers(topicName) == 0; + } + }; + while (!predicate() && timeSlept < duration_cast(timeout)) { + rclcpp::Event::SharedPtr graphEvent = node->get_graph_event(); + node->wait_for_graph_change(graphEvent, sleepPeriod); + timeSlept = duration_cast(steady_clock::now() - start); + } + int64_t timeSleptCount = std::chrono::duration_cast(timeSlept).count(); + printf("Waited %" PRId64 " microseconds for the subscriber to %s topic '%s'\n", + timeSleptCount, + toBeAvailable ? "connect to" : "disconnect from", + topicName.c_str()); } - // Sleep for timeout ms or until the service is available to the client - template - void waitForService( - std::shared_ptr> client, - const DurationT & timeout) -{ - while (!client->wait_for_service(timeout)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); - return; - } - } +// Sleep for timeout ms or until the service is available to the client +template +void waitForService(std::shared_ptr> client, const DurationT& timeout) { + while (!client->wait_for_service(timeout)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return; + } + } } template -void waitForFuture( - rclcpp::Executor & executor, - std::shared_future & future, - const DurationT & timeout) -{ - using rclcpp::FutureReturnCode; - rclcpp::FutureReturnCode futureRet; - auto startTime = std::chrono::steady_clock::now(); - futureRet = executor.spin_until_future_complete(future, timeout); - auto elapsedTime = std::chrono::steady_clock::now() - startTime; - EXPECT_EQ(FutureReturnCode::SUCCESS, futureRet) << - "future failed to be set after: " << - std::chrono::duration_cast(elapsedTime).count() << - " milliseconds\n"; +void waitForFuture(rclcpp::Executor& executor, std::shared_future& future, const DurationT& timeout) { + using rclcpp::FutureReturnCode; + rclcpp::FutureReturnCode futureRet; + auto startTime = std::chrono::steady_clock::now(); + futureRet = executor.spin_until_future_complete(future, timeout); + auto elapsedTime = std::chrono::steady_clock::now() - startTime; + EXPECT_EQ(FutureReturnCode::SUCCESS, futureRet) + << "future failed to be set after: " << std::chrono::duration_cast(elapsedTime).count() + << " milliseconds\n"; } -} // namespace testUtils \ No newline at end of file +} // namespace testUtils diff --git a/atos/common/tests/test_relativetrajectory.cpp b/atos/common/tests/test_relativetrajectory.cpp index 027054488..9149a3773 100644 --- a/atos/common/tests/test_relativetrajectory.cpp +++ b/atos/common/tests/test_relativetrajectory.cpp @@ -1,9 +1,9 @@ #include "../trajectory.hpp" -#include #include -#include +#include #include #include +#include #define TIME_TOL_S 0.000001 #define POS_TOL_M 0.001 #define VEL_TOL_M_S 0.001 @@ -30,10 +30,8 @@ int main(int argc, char** argv) { curvature_test(); mode_test(); exit(EXIT_SUCCESS); - } - catch (std::runtime_error& e) { - std::cerr << "Test " << __FILE__ << " failed: " << std::endl - << e.what() << std::endl; + } catch (std::runtime_error& e) { + std::cerr << "Test " << __FILE__ << " failed: " << std::endl << e.what() << std::endl; exit(EXIT_FAILURE); } } @@ -60,13 +58,11 @@ void time_test() { using namespace std::chrono_literals; using namespace std::chrono; - std::vector t1s = - {0ms, 100ms, 100ms, 200ms}; - std::vector t2s = - {0ms, 100ms, 200ms, 100ms}; + std::vector t1s = {0ms, 100ms, 100ms, 200ms}; + std::vector t2s = {0ms, 100ms, 200ms, 100ms}; std::vector out = t1s; - for (unsigned long i=0; i < out.size(); ++i) { + for (unsigned long i = 0; i < out.size(); ++i) { p1.setTime(t1s[i]); p2.setTime(t2s[i]); auto pRel = p1.relativeTo(p2); @@ -98,65 +94,60 @@ void position_test() { double z; } pos; - std::vector p1s = - {{ 0.0, 0.0, 0.0}, - { 10.0, -5.0, 2.2}, - { -10.0, 5.0, 2.2}, - { 5001.0, 4001.0, 3001.0}, - { 0.0, 0.0, 0.0}, - { 4.0, 5.0, 2.0}, - { 5.0, 5.0, 2.0}, - { 4.0, 5.0, 2.0}, - { 4.0, 5.0, 2.0}, - { 2.0, 3.0, 0.0}, - { 1.0, 2.0, 0.0}, - {61.11111, -20.0, 0.0}}; - std::vector p2s = - {{ 0.0, 0.0, 0.0, 0.0}, - { 5.0, 5.0, 1.3, 0.0}, - { 10.0, -5.0, 4.2, 0.0}, - { 1.0, 1.0, 1.0, 0.0}, - { 0.0, 0.0, 0.0, 98.0}, - { 0.0, 0.0, 0.0, 90.0}, - { 0.0, 0.0, 0.0, 45.0}, - { 0.0, 0.0, 0.0, 180.0}, - { 0.0, 0.0, 0.0, -90.0}, - { -2.0, -3.0, -1.0, 90.0}, - { 2.0, 3.0, 0.0, 270.0}, - { 110.0, -36.0, 0.0, 161.8781398}}; - std::vector out = - {{ 0.0, 0.0, 0.0}, - { 5.0, -10.0, 0.9}, - { -20.0, 10.0, -2.0}, - { 5000.0, 4000.0, 3000.0}, - { 0.0, 0.0, 0.0}, - { 5.0, -4.0, 2.0}, - {7.07107, 0.0, 2.0}, - { -4.0, -5.0, 2.0}, - { -5.0, 4.0, 2.0}, - { 6.0, -4.0, 1.0}, - { 1.0, -1.0, 0.0}, - {51.44048, 0.0, 0.0}}; + std::vector p1s = {{0.0, 0.0, 0.0}, + {10.0, -5.0, 2.2}, + {-10.0, 5.0, 2.2}, + {5001.0, 4001.0, 3001.0}, + {0.0, 0.0, 0.0}, + {4.0, 5.0, 2.0}, + {5.0, 5.0, 2.0}, + {4.0, 5.0, 2.0}, + {4.0, 5.0, 2.0}, + {2.0, 3.0, 0.0}, + {1.0, 2.0, 0.0}, + {61.11111, -20.0, 0.0}}; + std::vector p2s = {{0.0, 0.0, 0.0, 0.0}, + {5.0, 5.0, 1.3, 0.0}, + {10.0, -5.0, 4.2, 0.0}, + {1.0, 1.0, 1.0, 0.0}, + {0.0, 0.0, 0.0, 98.0}, + {0.0, 0.0, 0.0, 90.0}, + {0.0, 0.0, 0.0, 45.0}, + {0.0, 0.0, 0.0, 180.0}, + {0.0, 0.0, 0.0, -90.0}, + {-2.0, -3.0, -1.0, 90.0}, + {2.0, 3.0, 0.0, 270.0}, + {110.0, -36.0, 0.0, 161.8781398}}; + std::vector out = {{0.0, 0.0, 0.0}, + {5.0, -10.0, 0.9}, + {-20.0, 10.0, -2.0}, + {5000.0, 4000.0, 3000.0}, + {0.0, 0.0, 0.0}, + {5.0, -4.0, 2.0}, + {7.07107, 0.0, 2.0}, + {-4.0, -5.0, 2.0}, + {-5.0, 4.0, 2.0}, + {6.0, -4.0, 1.0}, + {1.0, -1.0, 0.0}, + {51.44048, 0.0, 0.0}}; - for (unsigned long i=0; i < out.size(); ++i) { + for (unsigned long i = 0; i < out.size(); ++i) { p1.setXCoord(p1s[i].x); p1.setYCoord(p1s[i].y); p1.setZCoord(p1s[i].z); p2.setXCoord(p2s[i].x); p2.setYCoord(p2s[i].y); p2.setZCoord(p2s[i].z); - p2.setHeading(p2s[i].hdg * M_PI/180.0); + p2.setHeading(p2s[i].hdg * M_PI / 180.0); auto pRel = p1.relativeTo(p2); - if (std::abs(pRel.getXCoord() - out[i].x) > POS_TOL_M - || std::abs(pRel.getYCoord() - out[i].y) > POS_TOL_M - || std::abs(pRel.getZCoord() - out[i].z) > POS_TOL_M) { + if (std::abs(pRel.getXCoord() - out[i].x) > POS_TOL_M || std::abs(pRel.getYCoord() - out[i].y) > POS_TOL_M || + std::abs(pRel.getZCoord() - out[i].z) > POS_TOL_M) { std::stringstream ss; ss << "Relative position not within error tolerance:" << std::endl; ss << "p1: (" << p1s[i].x << "," << p1s[i].y << "," << p1s[i].z << ")" << std::endl; ss << "p2: (" << p2s[i].x << "," << p2s[i].y << "," << p2s[i].z << "), hdg: " << p2s[i].hdg << std::endl; ss << "exp: (" << out[i].x << "," << out[i].y << "," << out[i].z << ")" << std::endl; - ss << "act: (" << pRel.getXCoord() << "," << pRel.getYCoord() - << "," << pRel.getZCoord() << ")"; + ss << "act: (" << pRel.getXCoord() << "," << pRel.getYCoord() << "," << pRel.getZCoord() << ")"; throw std::runtime_error(ss.str()); } } @@ -167,30 +158,21 @@ void heading_test() { set_default(p1); set_default(p2); - std::vector h1s = - {0.0, 90.0, -90.0, 0.0, 0.0, - 20.0, 20.0, 20.0, -20.0, -20.0, - 370.0, -180.0}; - std::vector h2s = - {0.0, 0.0, 0.0, 90.0, -90.0, - 0.0, 20.0, 45.0, -50.0, -10.0, - 0.0, 180.0}; - std::vector out = - {0.0, 90.0, 270.0, 270.0, 90.0, - 20.0, 0.0, 335.0, 30.0, 350.0, - 10.0, 0.0}; + std::vector h1s = {0.0, 90.0, -90.0, 0.0, 0.0, 20.0, 20.0, 20.0, -20.0, -20.0, 370.0, -180.0}; + std::vector h2s = {0.0, 0.0, 0.0, 90.0, -90.0, 0.0, 20.0, 45.0, -50.0, -10.0, 0.0, 180.0}; + std::vector out = {0.0, 90.0, 270.0, 270.0, 90.0, 20.0, 0.0, 335.0, 30.0, 350.0, 10.0, 0.0}; - for (unsigned long i=0; i < out.size(); ++i) { - p1.setHeading(h1s[i]*M_PI/180.0); - p2.setHeading(h2s[i]*M_PI/180.0); + for (unsigned long i = 0; i < out.size(); ++i) { + p1.setHeading(h1s[i] * M_PI / 180.0); + p2.setHeading(h2s[i] * M_PI / 180.0); auto pRel = p1.relativeTo(p2); - if (std::abs(pRel.getHeading()*180.0/M_PI - out[i]) > HDG_TOL_DEG) { + if (std::abs(pRel.getHeading() * 180.0 / M_PI - out[i]) > HDG_TOL_DEG) { std::stringstream ss; ss << "Relative heading not within error tolerance:" << std::endl; ss << "p1: " << h1s[i] << std::endl; ss << "p2: " << h2s[i] << std::endl; ss << "exp: " << out[i] << std::endl; - ss << "act: " << pRel.getHeading()*180.0/M_PI; + ss << "act: " << pRel.getHeading() * 180.0 / M_PI; throw std::runtime_error(ss.str()); } } @@ -208,88 +190,84 @@ void velocity_test() { } test_data; std::vector p1s = - // Equidirectional lateral and longitudinal - {{0.0, 0.0, 0.0}, - {0.0, 5.0, 0.0}, - {0.0,-5.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 5.0}, - {0.0, 0.0,-5.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 5.0, 0.0}, - // Right angles - {150.0, 5.0, -2.0}, - {280.0, -1.0, 2.0}, - {280.0, 0.0, 0.0}, - {190.0, 0.0, 0.0}, - // Angled - {130.0, 4.0, 0.0}, - {-60.0, 20.0, 0.0} - }; + // Equidirectional lateral and longitudinal + {{0.0, 0.0, 0.0}, + {0.0, 5.0, 0.0}, + {0.0, -5.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 5.0}, + {0.0, 0.0, -5.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 5.0, 0.0}, + // Right angles + {150.0, 5.0, -2.0}, + {280.0, -1.0, 2.0}, + {280.0, 0.0, 0.0}, + {190.0, 0.0, 0.0}, + // Angled + {130.0, 4.0, 0.0}, + {-60.0, 20.0, 0.0}}; std::vector p2s = - // Equidirectional lateral and longitudinal - {{0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 5.0, 0.0}, - {0.0,-5.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 5.0}, - {0.0, 0.0,-5.0}, - {0.0, 10.0, 0.0}, - // Right angles - {60.0, 1.0, 0.0}, - {190.0, 1.0, 1.5}, - {190.0, 0.0, 0.0}, - {10.0, 4.0, 0.0}, - // Angled - {10.0, 4.0, 0.0}, - {-30.0, 20.0, 0.0} - }; + // Equidirectional lateral and longitudinal + {{0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 5.0, 0.0}, + {0.0, -5.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 5.0}, + {0.0, 0.0, -5.0}, + {0.0, 10.0, 0.0}, + // Right angles + {60.0, 1.0, 0.0}, + {190.0, 1.0, 1.5}, + {190.0, 0.0, 0.0}, + {10.0, 4.0, 0.0}, + // Angled + {10.0, 4.0, 0.0}, + {-30.0, 20.0, 0.0}}; std::vector out = - // Equidirectional lateral and longitudinal - {{0.0, 0.0, 0.0}, - {0.0, 5.0, 0.0}, - {0.0, -5.0, 0.0}, - {0.0, -5.0, 0.0}, - {0.0, 5.0, 0.0}, - {0.0, 0.0, 5.0}, - {0.0, 0.0,-5.0}, - {0.0, 0.0,-5.0}, - {0.0, 0.0, 5.0}, - {0.0, -5.0, 0.0}, - // Right angles - {90.0, 5.0, -3.0}, - {90.0, 0.5, 1.0}, - {90.0, 0.0, 0.0}, - {180.0, 4.0, 0.0}, - // Angled - {120.0, 6.0, -3.464101615}, - {330.0, 2.679491924, 10.0} - }; + // Equidirectional lateral and longitudinal + {{0.0, 0.0, 0.0}, + {0.0, 5.0, 0.0}, + {0.0, -5.0, 0.0}, + {0.0, -5.0, 0.0}, + {0.0, 5.0, 0.0}, + {0.0, 0.0, 5.0}, + {0.0, 0.0, -5.0}, + {0.0, 0.0, -5.0}, + {0.0, 0.0, 5.0}, + {0.0, -5.0, 0.0}, + // Right angles + {90.0, 5.0, -3.0}, + {90.0, 0.5, 1.0}, + {90.0, 0.0, 0.0}, + {180.0, 4.0, 0.0}, + // Angled + {120.0, 6.0, -3.464101615}, + {330.0, 2.679491924, 10.0}}; - assert(p1s.size() == p2s.size() - && p2s.size() == out.size()); - for (unsigned long i=0; i < out.size(); ++i) { - p1.setHeading(p1s[i].hdg*M_PI/180.0); + assert(p1s.size() == p2s.size() && p2s.size() == out.size()); + for (unsigned long i = 0; i < out.size(); ++i) { + p1.setHeading(p1s[i].hdg * M_PI / 180.0); p1.setLongitudinalVelocity(p1s[i].vlon); p1.setLateralVelocity(p1s[i].vlat); - p2.setHeading(p2s[i].hdg*M_PI/180.0); + p2.setHeading(p2s[i].hdg * M_PI / 180.0); p2.setLongitudinalVelocity(p2s[i].vlon); p2.setLateralVelocity(p2s[i].vlat); auto pRel = p1.relativeTo(p2); - if (std::abs(pRel.getHeading()*180.0/M_PI - out[i].hdg) > HDG_TOL_DEG - || std::abs(pRel.getLongitudinalVelocity() - out[i].vlon) > VEL_TOL_M_S - || std::abs(pRel.getLateralVelocity() - out[i].vlat) > VEL_TOL_M_S) { + if (std::abs(pRel.getHeading() * 180.0 / M_PI - out[i].hdg) > HDG_TOL_DEG || + std::abs(pRel.getLongitudinalVelocity() - out[i].vlon) > VEL_TOL_M_S || + std::abs(pRel.getLateralVelocity() - out[i].vlat) > VEL_TOL_M_S) { std::stringstream ss; ss << "Relative velocity not within error tolerance:" << std::endl; ss << "p1: (hdg:" << p1s[i].hdg << ",vlon:" << p1s[i].vlon << ",vlat:" << p1s[i].vlat << ")" << std::endl; ss << "p2: (hdg:" << p2s[i].hdg << ",vlon:" << p2s[i].vlon << ",vlat:" << p2s[i].vlat << ")" << std::endl; ss << "exp: (hdg:" << out[i].hdg << ",vlon:" << out[i].vlon << ",vlat:" << out[i].vlat << ")" << std::endl; - ss << "act: (hdg:" << pRel.getHeading()*180.0/M_PI << ",vlon:" << pRel.getLongitudinalVelocity() + ss << "act: (hdg:" << pRel.getHeading() * 180.0 / M_PI << ",vlon:" << pRel.getLongitudinalVelocity() << ",vlat:" << pRel.getLateralVelocity() << ")"; throw std::runtime_error(ss.str()); } @@ -308,110 +286,107 @@ void acceleration_test() { } test_data; std::vector p1s = - // Equidirectional lateral and longitudinal - {{0.0, 0.0, 0.0}, - {0.0, 5.0, 0.0}, - {0.0,-5.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 5.0}, - {0.0, 0.0,-5.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 5.0, 0.0}, - // Right angles - {150.0, 5.0, -2.0}, - {280.0, -1.0, 2.0}, - {280.0, 0.0, 0.0}, - {190.0, 0.0, 0.0}, - // Angled - {130.0, 4.0, 0.0}, - {-60.0, 20.0, 0.0} - }; + // Equidirectional lateral and longitudinal + {{0.0, 0.0, 0.0}, + {0.0, 5.0, 0.0}, + {0.0, -5.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 5.0}, + {0.0, 0.0, -5.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 5.0, 0.0}, + // Right angles + {150.0, 5.0, -2.0}, + {280.0, -1.0, 2.0}, + {280.0, 0.0, 0.0}, + {190.0, 0.0, 0.0}, + // Angled + {130.0, 4.0, 0.0}, + {-60.0, 20.0, 0.0}}; std::vector p2s = - // Equidirectional lateral and longitudinal - {{0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 5.0, 0.0}, - {0.0,-5.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 5.0}, - {0.0, 0.0,-5.0}, - {0.0, 10.0, 0.0}, - // Right angles - {60.0, 1.0, 0.0}, - {190.0, 1.0, 1.5}, - {190.0, 0.0, 0.0}, - {10.0, 4.0, 0.0}, - // Angled - {10.0, 4.0, 0.0}, - {-30.0, 20.0, 0.0} - }; + // Equidirectional lateral and longitudinal + {{0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 5.0, 0.0}, + {0.0, -5.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 5.0}, + {0.0, 0.0, -5.0}, + {0.0, 10.0, 0.0}, + // Right angles + {60.0, 1.0, 0.0}, + {190.0, 1.0, 1.5}, + {190.0, 0.0, 0.0}, + {10.0, 4.0, 0.0}, + // Angled + {10.0, 4.0, 0.0}, + {-30.0, 20.0, 0.0}}; std::vector out = - // Equidirectional lateral and longitudinal - {{0.0, 0.0, 0.0}, - {0.0, 5.0, 0.0}, - {0.0, -5.0, 0.0}, - {0.0, -5.0, 0.0}, - {0.0, 5.0, 0.0}, - {0.0, 0.0, 5.0}, - {0.0, 0.0,-5.0}, - {0.0, 0.0,-5.0}, - {0.0, 0.0, 5.0}, - {0.0, -5.0, 0.0}, - // Right angles - {90.0, 5.0, -3.0}, - {90.0, 0.5, 1.0}, - {90.0, 0.0, 0.0}, - {180.0, 4.0, 0.0}, - // Angled - {120.0, 6.0, -3.464101615}, - {330.0, 2.679491924, 10.0} - }; + // Equidirectional lateral and longitudinal + {{0.0, 0.0, 0.0}, + {0.0, 5.0, 0.0}, + {0.0, -5.0, 0.0}, + {0.0, -5.0, 0.0}, + {0.0, 5.0, 0.0}, + {0.0, 0.0, 5.0}, + {0.0, 0.0, -5.0}, + {0.0, 0.0, -5.0}, + {0.0, 0.0, 5.0}, + {0.0, -5.0, 0.0}, + // Right angles + {90.0, 5.0, -3.0}, + {90.0, 0.5, 1.0}, + {90.0, 0.0, 0.0}, + {180.0, 4.0, 0.0}, + // Angled + {120.0, 6.0, -3.464101615}, + {330.0, 2.679491924, 10.0}}; - for (unsigned long i=0; i < out.size(); ++i) { - p1.setHeading(p1s[i].hdg*M_PI/180.0); + for (unsigned long i = 0; i < out.size(); ++i) { + p1.setHeading(p1s[i].hdg * M_PI / 180.0); p1.setLongitudinalAcceleration(p1s[i].alon); p1.setLateralAcceleration(p1s[i].alat); - p2.setHeading(p2s[i].hdg*M_PI/180.0); + p2.setHeading(p2s[i].hdg * M_PI / 180.0); p2.setLongitudinalAcceleration(p2s[i].alon); p2.setLateralAcceleration(p2s[i].alat); auto pRel = p1.relativeTo(p2); - if (std::abs(pRel.getHeading()*180.0/M_PI - out[i].hdg) > HDG_TOL_DEG - || std::abs(pRel.getLongitudinalAcceleration() - out[i].alon) > ACC_TOL_M_S2 - || std::abs(pRel.getLateralAcceleration() - out[i].alat) > ACC_TOL_M_S2) { + if (std::abs(pRel.getHeading() * 180.0 / M_PI - out[i].hdg) > HDG_TOL_DEG || + std::abs(pRel.getLongitudinalAcceleration() - out[i].alon) > ACC_TOL_M_S2 || + std::abs(pRel.getLateralAcceleration() - out[i].alat) > ACC_TOL_M_S2) { std::stringstream ss; ss << "Relative acceleration not within error tolerance:" << std::endl; ss << "p1: (hdg:" << p1s[i].hdg << ",alon:" << p1s[i].alon << ",alat:" << p1s[i].alat << ")" << std::endl; ss << "p2: (hdg:" << p2s[i].hdg << ",alon:" << p2s[i].alon << ",alat:" << p2s[i].alat << ")" << std::endl; ss << "exp: (hdg:" << out[i].hdg << ",alon:" << out[i].alon << ",alat:" << out[i].alat << ")" << std::endl; - ss << "act: (hdg:" << pRel.getHeading()*180.0/M_PI << ",alon:" << pRel.getLongitudinalAcceleration() + ss << "act: (hdg:" << pRel.getHeading() * 180.0 / M_PI << ",alon:" << pRel.getLongitudinalAcceleration() << ",alat:" << pRel.getLateralAcceleration() << ")"; throw std::runtime_error(ss.str()); } } } -void curvature_test() { - -} +void curvature_test() {} void mode_test() { traj_pt p1(rclcpp::get_logger("test")), p2(rclcpp::get_logger("test")); set_default(p1); set_default(p2); - std::vector m1s = - {traj_pt::CONTROLLED_BY_VEHICLE, traj_pt::CONTROLLED_BY_DRIVE_FILE, - traj_pt::CONTROLLED_BY_VEHICLE, traj_pt::CONTROLLED_BY_DRIVE_FILE}; - std::vector m2s = - {traj_pt::CONTROLLED_BY_VEHICLE, traj_pt::CONTROLLED_BY_DRIVE_FILE, - traj_pt::CONTROLLED_BY_DRIVE_FILE, traj_pt::CONTROLLED_BY_VEHICLE}; + std::vector m1s = {traj_pt::CONTROLLED_BY_VEHICLE, + traj_pt::CONTROLLED_BY_DRIVE_FILE, + traj_pt::CONTROLLED_BY_VEHICLE, + traj_pt::CONTROLLED_BY_DRIVE_FILE}; + std::vector m2s = {traj_pt::CONTROLLED_BY_VEHICLE, + traj_pt::CONTROLLED_BY_DRIVE_FILE, + traj_pt::CONTROLLED_BY_DRIVE_FILE, + traj_pt::CONTROLLED_BY_VEHICLE}; std::vector out = m1s; - for (unsigned long i=0; i < out.size(); ++i) { + for (unsigned long i = 0; i < out.size(); ++i) { p1.setMode(m1s[i]); p2.setMode(m2s[i]); auto pRel = p1.relativeTo(p2); diff --git a/atos/common/trajectory.cpp b/atos/common/trajectory.cpp index 4a41b192d..6ab59d478 100644 --- a/atos/common/trajectory.cpp +++ b/atos/common/trajectory.cpp @@ -3,15 +3,14 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include -#include -#include -#include -#include -#include -#include "regexpatterns.hpp" #include "trajectory.hpp" #include "CRSTransformation.hpp" // xyz2llh +#include "regexpatterns.hpp" +#include +#include +#include +#include +#include #if ROS_FOXY #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -19,41 +18,41 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -namespace ATOS{ -const std::regex Trajectory::fileHeaderPattern("TRAJECTORY;(" + RegexPatterns::intPattern + ");(" - + RegexPatterns::namePattern + ");" + RegexPatterns::versionPattern + ";(" - + RegexPatterns::intPattern + ");"); -const std::regex Trajectory::fileLinePattern("LINE;(" + RegexPatterns::floatPattern + ");(" - + RegexPatterns::floatPattern + ");(" + RegexPatterns::floatPattern + ");(" - + RegexPatterns::floatPattern + ")?;(" + RegexPatterns::floatPattern + ");(" - + RegexPatterns::floatPattern + ")?;(" + RegexPatterns::floatPattern + ")?;(" - + RegexPatterns::floatPattern + ")?;(" + RegexPatterns::floatPattern + ")?;(" - + RegexPatterns::floatPattern + ");(" + RegexPatterns::intPattern + ");ENDLINE;"); +namespace ATOS { +const std::regex Trajectory::fileHeaderPattern("TRAJECTORY;(" + RegexPatterns::intPattern + ");(" + + RegexPatterns::namePattern + ");" + RegexPatterns::versionPattern + + ";(" + RegexPatterns::intPattern + ");"); +const std::regex Trajectory::fileLinePattern( + "LINE;(" + RegexPatterns::floatPattern + ");(" + RegexPatterns::floatPattern + ");(" + RegexPatterns::floatPattern + + ");(" + RegexPatterns::floatPattern + ")?;(" + RegexPatterns::floatPattern + ");(" + RegexPatterns::floatPattern + + ")?;(" + RegexPatterns::floatPattern + ")?;(" + RegexPatterns::floatPattern + ")?;(" + RegexPatterns::floatPattern + + ")?;(" + RegexPatterns::floatPattern + ");(" + RegexPatterns::intPattern + ");ENDLINE;"); const std::regex Trajectory::fileFooterPattern("ENDTRAJECTORY;"); -Trajectory::Trajectory(const Trajectory& other) : Loggable(other.get_logger()) { - this->id = other.id; - this->name = other.name; +Trajectory::Trajectory(const Trajectory& other) : + Loggable(other.get_logger()) { + this->id = other.id; + this->name = other.name; this->version = other.version; - this->points = std::vector(other.points); + this->points = std::vector(other.points); } - Trajectory& Trajectory::operator=(const Trajectory& other) { - this->id = other.id; - this->name = other.name; +Trajectory& Trajectory::operator=(const Trajectory& other) { + this->id = other.id; + this->name = other.name; this->version = other.version; - this->points = std::vector(other.points); + this->points = std::vector(other.points); return *this; } -atos_interfaces::msg::CartesianTrajectory Trajectory::toCartesianTrajectory(){ +atos_interfaces::msg::CartesianTrajectory Trajectory::toCartesianTrajectory() { atos_interfaces::msg::CartesianTrajectory trajMsg; - for (const auto& point : this->points){ + for (const auto& point : this->points) { atos_interfaces::msg::CartesianTrajectoryPoint pointMsg; // Time - auto millis = point.getTime().count(); - pointMsg.time_from_start.sec = millis/1000; - pointMsg.time_from_start.nanosec = (millis%1000)*1000000; + auto millis = point.getTime().count(); + pointMsg.time_from_start.sec = millis / 1000; + pointMsg.time_from_start.nanosec = (millis % 1000) * 1000000; // Position pointMsg.pose.position.x = point.getPosition().x(); @@ -64,7 +63,7 @@ atos_interfaces::msg::CartesianTrajectory Trajectory::toCartesianTrajectory(){ tf2::Quaternion q; q.setRPY(0, 0, point.getHeading()); tf2::convert(q, pointMsg.pose.orientation); - + // Velocity TODO convert longitudinal / lateral into xyz coordinate system pointMsg.twist.linear.x = point.getLongitudinalVelocity(); pointMsg.twist.linear.y = point.getLateralVelocity(); @@ -80,15 +79,14 @@ atos_interfaces::msg::CartesianTrajectory Trajectory::toCartesianTrajectory(){ return trajMsg; } -nav_msgs::msg::Path Trajectory::toPath() const -{ +nav_msgs::msg::Path Trajectory::toPath() const { nav_msgs::msg::Path path; path.header.frame_id = "map"; - path.header.stamp = rclcpp::Time(0); - auto rosTimeOffset = rclcpp::Time(std::chrono::system_clock::now().time_since_epoch().count()); - for (const auto& point : this->points){ + path.header.stamp = rclcpp::Time(0); + auto rosTimeOffset = rclcpp::Time(std::chrono::system_clock::now().time_since_epoch().count()); + for (const auto& point : this->points) { geometry_msgs::msg::PoseStamped pose; - pose.header.stamp = rosTimeOffset + rclcpp::Duration(point.getTime()); + pose.header.stamp = rosTimeOffset + rclcpp::Duration(point.getTime()); pose.pose.position.x = point.getPosition().x(); pose.pose.position.y = point.getPosition().y(); pose.pose.position.z = point.getPosition().z(); @@ -104,27 +102,27 @@ nav_msgs::msg::Path Trajectory::toPath() const return path; } -foxglove_msgs::msg::GeoJSON Trajectory::toGeoJSON(std::array llh_0) const { +foxglove_msgs::msg::GeoJSON Trajectory::toGeoJSON(std::array llh_0) const { foxglove_msgs::msg::GeoJSON geoJSON; std::stringstream ss; ss << std::fixed; // supress scientific notation ss << std::setprecision(12); - for (const auto& point : this->points){ - double llh[3] = {llh_0[0], llh_0[1], llh_0[2]}; + for (const auto& point : this->points) { + double llh[3] = {llh_0[0], llh_0[1], llh_0[2]}; double offset[3] = {point.getXCoord(), point.getYCoord(), point.getZCoord()}; CRSTransformation::llhOffsetMeters(llh, offset); ss << "[" << llh[1] << "," << llh[0] << "],"; // Flipped order } std::string positions = ss.str(); - positions = positions.substr(0, positions.size()-1); // remove trailing , + positions = positions.substr(0, positions.size() - 1); // remove trailing , - - std::string geojson = - R"({ + std::string geojson = + R"({ "type": "Feature", "geometry": { "type": "LineString", - "coordinates": [)" + positions + R"(] + "coordinates": [)" + + positions + R"(] }, "properties": { "name": "" @@ -132,20 +130,21 @@ foxglove_msgs::msg::GeoJSON Trajectory::toGeoJSON(std::array llh_0) co })"; geoJSON.geojson = geojson; return geoJSON; - } -void Trajectory::initializeFromCartesianTrajectory(const atos_interfaces::msg::CartesianTrajectory &traj) { +void Trajectory::initializeFromCartesianTrajectory(const atos_interfaces::msg::CartesianTrajectory& traj) { using namespace std::chrono; // TODO: add name to traj - - for (const auto &tp : traj.points){ + + for (const auto& tp : traj.points) { TrajectoryPoint point(logger); - point.setTime(duration_cast(seconds{tp.time_from_start.sec} + nanoseconds{tp.time_from_start.nanosec})); + point.setTime( + duration_cast(seconds{tp.time_from_start.sec} + nanoseconds{tp.time_from_start.nanosec})); point.setXCoord(tp.pose.position.x); point.setYCoord(tp.pose.position.y); point.setZCoord(tp.pose.position.z); - tf2::Matrix3x3 m(tf2::Quaternion(tp.pose.orientation.x, tp.pose.orientation.y, tp.pose.orientation.z, tp.pose.orientation.w)); + tf2::Matrix3x3 m( + tf2::Quaternion(tp.pose.orientation.x, tp.pose.orientation.y, tp.pose.orientation.z, tp.pose.orientation.w)); double roll, pitch, yaw = 0; m.getRPY(roll, pitch, yaw); point.setHeading(yaw); @@ -153,13 +152,13 @@ void Trajectory::initializeFromCartesianTrajectory(const atos_interfaces::msg::C point.setLateralVelocity(tp.twist.linear.y); point.setLongitudinalAcceleration(tp.acceleration.linear.x); point.setLateralAcceleration(tp.acceleration.linear.y); - point.setCurvature(0); // TODO: Support - point.setMode(ATOS::Trajectory::TrajectoryPoint::ModeType::CONTROLLED_BY_DRIVE_FILE); //TODO: Support + point.setCurvature(0); // TODO: Support + point.setMode(ATOS::Trajectory::TrajectoryPoint::ModeType::CONTROLLED_BY_DRIVE_FILE); // TODO: Support this->points.push_back(point); } } -void Trajectory::initializeFromFile(const std::string &fileName) { +void Trajectory::initializeFromFile(const std::string& fileName) { using namespace std; char trajDirPath[PATH_MAX]; @@ -167,9 +166,9 @@ void Trajectory::initializeFromFile(const std::string &fileName) { smatch match; ifstream file; bool isHeaderParsedSuccessfully = false; - unsigned long nPoints = 0; + unsigned long nPoints = 0; - UtilGetTrajDirectoryPath(trajDirPath, sizeof (trajDirPath)); + UtilGetTrajDirectoryPath(trajDirPath, sizeof(trajDirPath)); string trajFilePath(trajDirPath); trajFilePath += fileName; @@ -183,39 +182,32 @@ void Trajectory::initializeFromFile(const std::string &fileName) { for (unsigned long lineCount = 0; getline(file, line); lineCount++) { if (lineCount == 0) { if (regex_search(line, match, this->fileHeaderPattern)) { - this->id = stoi(match[1]); - this->name = match[2]; + this->id = stoi(match[1]); + this->name = match[2]; this->version = 0; - nPoints = stoul(match[6]); + nPoints = stoul(match[6]); this->points.reserve(nPoints); isHeaderParsedSuccessfully = true; - } - else { + } else { errMsg = "The header of trajectory file <" + trajFilePath + "> is badly formatted"; break; } - } - else if (lineCount > 0 && !isHeaderParsedSuccessfully) { + } else if (lineCount > 0 && !isHeaderParsedSuccessfully) { errMsg = "Attempt to parse trajectory file <" + trajFilePath + "> before encountering header"; break; - } - else if (lineCount > nPoints + 1) { - errMsg = "Trajectory line count of file <" + trajFilePath - + "> does not match specified line count"; + } else if (lineCount > nPoints + 1) { + errMsg = "Trajectory line count of file <" + trajFilePath + "> does not match specified line count"; break; - } - else if (lineCount == nPoints + 1) { + } else if (lineCount == nPoints + 1) { if (regex_search(line, match, fileFooterPattern)) { file.close(); RCLCPP_DEBUG(get_logger(), "Closed <%s>", trajFilePath.c_str()); return; - } - else { + } else { errMsg = "Final line of trajectory file <" + trajFilePath + "> badly formatted"; break; } - } - else { + } else { if (regex_search(line, match, fileLinePattern)) { TrajectoryPoint point(get_logger()); point.setTime(stod(match[1])); @@ -235,10 +227,8 @@ void Trajectory::initializeFromFile(const std::string &fileName) { point.setCurvature(stod(match[10])); point.setMode(static_cast(stoi(match[11]))); points.push_back(point); - } - else { - errMsg = "Line " + to_string(lineCount) + " of trajectory file <" - + trajFilePath + "> badly formatted"; + } else { + errMsg = "Line " + to_string(lineCount) + " of trajectory file <" + trajFilePath + "> badly formatted"; break; } } @@ -248,7 +238,6 @@ void Trajectory::initializeFromFile(const std::string &fileName) { throw invalid_argument(errMsg); } - CartesianPosition Trajectory::TrajectoryPoint::getISOPosition() const { CartesianPosition retval; retval.xCoord_m = this->getXCoord(); @@ -259,8 +248,8 @@ CartesianPosition Trajectory::TrajectoryPoint::getISOPosition() const { RCLCPP_WARN(get_logger(), "Casting trajectory point to cartesian position: optional z value assumed to be 0"); retval.zCoord_m = 0.0; } - retval.heading_rad = this->getHeading(); - retval.isHeadingValid = true; + retval.heading_rad = this->getHeading(); + retval.isHeadingValid = true; retval.isPositionValid = true; return retval; } @@ -268,17 +257,17 @@ CartesianPosition Trajectory::TrajectoryPoint::getISOPosition() const { SpeedType Trajectory::TrajectoryPoint::getISOVelocity() const { SpeedType retval; try { - retval.longitudinal_m_s = getLongitudinalVelocity(); + retval.longitudinal_m_s = getLongitudinalVelocity(); retval.isLongitudinalValid = true; } catch (std::out_of_range) { - retval.longitudinal_m_s = 0.0; + retval.longitudinal_m_s = 0.0; retval.isLongitudinalValid = false; } try { - retval.lateral_m_s = getLateralVelocity(); + retval.lateral_m_s = getLateralVelocity(); retval.isLateralValid = true; } catch (std::out_of_range) { - retval.lateral_m_s = 0.0; + retval.lateral_m_s = 0.0; retval.isLateralValid = false; } return retval; @@ -287,17 +276,17 @@ SpeedType Trajectory::TrajectoryPoint::getISOVelocity() const { AccelerationType Trajectory::TrajectoryPoint::getISOAcceleration() const { AccelerationType retval; try { - retval.longitudinal_m_s2 = getLongitudinalAcceleration(); + retval.longitudinal_m_s2 = getLongitudinalAcceleration(); retval.isLongitudinalValid = true; } catch (std::out_of_range) { - retval.longitudinal_m_s2 = 0.0; + retval.longitudinal_m_s2 = 0.0; retval.isLongitudinalValid = false; } try { - retval.lateral_m_s2 = getLateralAcceleration(); + retval.lateral_m_s2 = getLateralAcceleration(); retval.isLateralValid = true; } catch (std::out_of_range) { - retval.lateral_m_s2 = 0.0; + retval.lateral_m_s2 = 0.0; retval.isLateralValid = false; } return retval; @@ -308,8 +297,7 @@ AccelerationType Trajectory::TrajectoryPoint::getISOAcceleration() const { * \param other * \return */ -Trajectory::TrajectoryPoint Trajectory::TrajectoryPoint::relativeTo( - const TrajectoryPoint &other) const { +Trajectory::TrajectoryPoint Trajectory::TrajectoryPoint::relativeTo(const TrajectoryPoint& other) const { using namespace Eigen; TrajectoryPoint relative(get_logger()); @@ -319,27 +307,24 @@ Trajectory::TrajectoryPoint Trajectory::TrajectoryPoint::relativeTo( AngleAxis R3(-other.getHeading(), Vector3d::UnitZ()); Rotation2Dd R(-relative.getHeading()); - relative.setPosition(R3*(zeroNaNs(this->getPosition()) - - zeroNaNs(other.getPosition()))); + relative.setPosition(R3 * (zeroNaNs(this->getPosition()) - zeroNaNs(other.getPosition()))); - auto thisVel = zeroNaNs(this->getVelocity()); + auto thisVel = zeroNaNs(this->getVelocity()); auto otherVel = zeroNaNs(other.getVelocity()); - relative.setVelocity(R.inverse()*(R*thisVel - otherVel)); + relative.setVelocity(R.inverse() * (R * thisVel - otherVel)); - auto thisAcc = zeroNaNs(this->getAcceleration()); + auto thisAcc = zeroNaNs(this->getAcceleration()); auto otherAcc = zeroNaNs(other.getAcceleration()); - relative.setAcceleration(R.inverse()*(R*thisAcc - otherAcc)); + relative.setAcceleration(R.inverse() * (R * thisAcc - otherAcc)); // K(t) = ||r'(t) x r''(t)|| / ||r'(t)||³ - auto rPrim = R*this->getVelocity(); - auto rBis = R*this->getAcceleration(); + auto rPrim = R * this->getVelocity(); + auto rBis = R * this->getAcceleration(); Eigen::Vector3d rPrim3(rPrim[0], rPrim[1], 0.0); Eigen::Vector3d rBis3(rBis[0], rBis[1], 0.0); if (rPrim3.norm() > 0.001) { - relative.setCurvature(rPrim3.cross(rBis3).norm() - / std::pow(rPrim3.norm(), 3)); - } - else { + relative.setCurvature(rPrim3.cross(rBis3).norm() / std::pow(rPrim3.norm(), 3)); + } else { relative.setCurvature(0.0); } @@ -347,8 +332,7 @@ Trajectory::TrajectoryPoint Trajectory::TrajectoryPoint::relativeTo( return relative; } -Trajectory Trajectory::relativeTo( - const Trajectory &other) const { +Trajectory Trajectory::relativeTo(const Trajectory& other) const { using namespace Eigen; // TODO check that ranges are sorted by time if (this->version != other.version) { @@ -357,11 +341,12 @@ Trajectory Trajectory::relativeTo( } Trajectory relative(get_logger()); - relative.id = this->id; - relative.name = this->name + "_rel_" + other.name; + relative.id = this->id; + relative.name = this->name + "_rel_" + other.name; relative.version = this->version; for (auto trajPt = this->points.begin(); trajPt != this->points.end(); ++trajPt) { - auto nearestTrajPtInOther = getNearest(other.points.begin(), other.points.end(), std::chrono::duration(trajPt->getTime()).count()); + auto nearestTrajPtInOther = getNearest( + other.points.begin(), other.points.end(), std::chrono::duration(trajPt->getTime()).count()); // TODO maybe a check on time difference here relative.points.push_back(trajPt->relativeTo(*nearestTrajPtInOther)); } @@ -369,23 +354,25 @@ Trajectory Trajectory::relativeTo( return relative; } -Trajectory::const_iterator Trajectory::getNearest( - const_iterator first, - const_iterator last, - const double &time) { +Trajectory::const_iterator Trajectory::getNearest(const_iterator first, const_iterator last, const double& time) { // Assumption: input range is sorted by time // Get first element with larger time than requested const_iterator after = std::lower_bound(first, last, time, [](const TrajectoryPoint& trajPt, const double& t) { return std::chrono::duration(trajPt.getTime()).count() < t; }); - if (after == first) return first; - if (after == last) return last - 1; + if (after == first) + return first; + if (after == last) + return last - 1; const_iterator before = after - 1; // Return the element nearest to the requested time - return (std::chrono::duration(after->getTime()).count() - time) < (time - std::chrono::duration(before->getTime()).count()) ? after : before; + return (std::chrono::duration(after->getTime()).count() - time) < + (time - std::chrono::duration(before->getTime()).count()) + ? after + : before; } std::string Trajectory::TrajectoryPoint::getFormatString() const { @@ -415,9 +402,7 @@ std::string Trajectory::toString() const { std::string retval = ""; std::stringstream ss(retval); ss << "Trajectory:" - << "\n Name: " << this->name - << "\n ID: " << this->id - << "\n Version: " << this->version; + << "\n Name: " << this->name << "\n ID: " << this->id << "\n Version: " << this->version; for (const auto& point : points) { ss << "\n\t" << point.toString(); } @@ -430,200 +415,197 @@ std::string Trajectory::toString() const { * \param other Trajectory to append. * \return New trajectory, concatenation of two. */ -Trajectory Trajectory::appendedWith( - const Trajectory &other) { +Trajectory Trajectory::appendedWith(const Trajectory& other) { if (!other.isValid()) { throw std::invalid_argument("Attempted to append invalid trajectory"); - } - else if (!this->isValid()) { + } else if (!this->isValid()) { throw std::invalid_argument("Attempted to append to invalid trajectory"); } Trajectory newTrajectory = this->points.empty() ? Trajectory(other) : Trajectory(*this); - newTrajectory.name = this->name + "_app_" + other.name; + newTrajectory.name = this->name + "_app_" + other.name; if (this->points.empty() || other.points.empty()) { return newTrajectory; } auto firstTrajEndTime = points.back().getTime(); // TODO maybe a time offset between the two trajectories? - auto secondTrajStartTime = newTrajectory.points.front().getTime(); // TODO maybe a time offset between the two trajectories? - - std::transform(other.points.begin(), other.points.end(), - std::back_inserter(newTrajectory.points), - [&](TrajectoryPoint otherPt) { - otherPt.setTime(otherPt.getTime() - secondTrajStartTime + firstTrajEndTime); - return otherPt; - }); + auto secondTrajStartTime = + newTrajectory.points.front().getTime(); // TODO maybe a time offset between the two trajectories? + + std::transform( + other.points.begin(), other.points.end(), std::back_inserter(newTrajectory.points), [&](TrajectoryPoint otherPt) { + otherPt.setTime(otherPt.getTime() - secondTrajStartTime + firstTrajEndTime); + return otherPt; + }); return newTrajectory; } /*! - * \brief Trajectory::TrajectoryPoint::rescaleToVelocity Returns a copy of the trajectory rescaled to match a certain constant speed. + * \brief Trajectory::TrajectoryPoint::rescaleToVelocity Returns a copy of the trajectory rescaled to match a certain + * constant speed. * \param vel_m_s Speed to which trajectory is to be reduced * \return Trajectory */ -Trajectory Trajectory::rescaledToVelocity( - const double vel_m_s) const { +Trajectory Trajectory::rescaledToVelocity(const double vel_m_s) const { if (!this->isValid()) { throw std::invalid_argument("Attempted to rescale invalid trajectory"); } Trajectory newTrajectory = Trajectory(*this); - newTrajectory.name = newTrajectory.name + "_rescaled"; + newTrajectory.name = newTrajectory.name + "_rescaled"; if (newTrajectory.points.empty()) { return newTrajectory; } - Eigen::Vector2d maxVel_m_s = std::max_element(newTrajectory.points.begin(), newTrajectory.points.end(), [](const TrajectoryPoint& pt1, const TrajectoryPoint& pt2) - { return pt1.getVelocity().norm() < pt2.getVelocity().norm(); }).base()->getVelocity(); + Eigen::Vector2d maxVel_m_s = std::max_element(newTrajectory.points.begin(), + newTrajectory.points.end(), + [](const TrajectoryPoint& pt1, const TrajectoryPoint& pt2) { + return pt1.getVelocity().norm() < pt2.getVelocity().norm(); + }) + .base() + ->getVelocity(); if (vel_m_s > maxVel_m_s.norm()) { RCLCPP_DEBUG(get_logger(), "Requested max velocity is larger than current max velocity"); return newTrajectory; } double scaleFactor = vel_m_s / maxVel_m_s.norm(); - double startTime = std::chrono::duration(newTrajectory.points.front().getTime()).count(); + double startTime = std::chrono::duration(newTrajectory.points.front().getTime()).count(); for (auto& point : newTrajectory.points) { - point.setVelocity(point.getVelocity()*scaleFactor); - point.setTime(startTime + (std::chrono::duration(point.getTime()).count()-startTime)/scaleFactor); - point.setAcceleration(point.getAcceleration()*pow(scaleFactor, 2)); + point.setVelocity(point.getVelocity() * scaleFactor); + point.setTime(startTime + (std::chrono::duration(point.getTime()).count() - startTime) / scaleFactor); + point.setAcceleration(point.getAcceleration() * pow(scaleFactor, 2)); } return newTrajectory; } -Trajectory Trajectory::createWilliamsonTurn( - double turnRadius, - double acceleration, - double minSpeed, - double maxSpeed, - TrajectoryPoint startPoint, - std::chrono::milliseconds startTime) -{ +Trajectory Trajectory::createWilliamsonTurn(double turnRadius, + double acceleration, + double minSpeed, + double maxSpeed, + TrajectoryPoint startPoint, + std::chrono::milliseconds startTime) { using namespace std::chrono; using Eigen::MatrixXd; - double topSpeed = turnRadius*2 / 3.6; // Set top speed depending on radius. + double topSpeed = turnRadius * 2 / 3.6; // Set top speed depending on radius. // Limit top speed. std::clamp(topSpeed, minSpeed, maxSpeed); const int calculatedNoOfPoints = 200; // TODO: Make this variable dynamic depending on turnRadius - double radius = turnRadius; - double headingRad = startPoint.getHeading(); + double radius = turnRadius; + double headingRad = startPoint.getHeading(); - Eigen::VectorXd theta0; //First section - Eigen::VectorXd theta1; //Second section - Eigen::VectorXd endStraight; //Third section + Eigen::VectorXd theta0; // First section + Eigen::VectorXd theta1; // Second section + Eigen::VectorXd endStraight; // Third section Eigen::Matrix xyM; Eigen::Matrix resM; - Eigen::Array headingArray; - Eigen::Array speedArray; - Eigen::Array accelerationArray; - Eigen::Array timeArray; - - - //Calculate length of each section - double len0 = (M_PI * 2 * radius) / 4; - double len1 = 3 * ((M_PI * 2 * radius) / 4); - double len2 = radius * 2; + Eigen::Array headingArray; + Eigen::Array speedArray; + Eigen::Array accelerationArray; + Eigen::Array timeArray; + + // Calculate length of each section + double len0 = (M_PI * 2 * radius) / 4; + double len1 = 3 * ((M_PI * 2 * radius) / 4); + double len2 = radius * 2; double totalLength = len0 + len1 + len2; - assert (fabs(totalLength) > 0.001); + assert(fabs(totalLength) > 0.001); - //First section + // First section int n0 = static_cast(std::round(calculatedNoOfPoints * (len0 / totalLength))); - theta0 = Eigen::VectorXd::LinSpaced(n0, M_PI, M_PI_2 - M_PI_2/(n0+1)); + theta0 = Eigen::VectorXd::LinSpaced(n0, M_PI, M_PI_2 - M_PI_2 / (n0 + 1)); for (int i = 0; i < theta0.size(); i++) { - xyM(0,i) = radius * cos(theta0[i]) + fabs(radius); - xyM(1,i) = radius * sin(theta0[i]); + xyM(0, i) = radius * cos(theta0[i]) + fabs(radius); + xyM(1, i) = radius * sin(theta0[i]); headingArray[i] = theta0[i] + M_PI_2 + M_PI; } - //second section + // second section int n1 = static_cast(std::round(calculatedNoOfPoints * (len1 / totalLength))); - theta1 = Eigen::VectorXd::LinSpaced(n1, -1*M_PI_2, M_PI - 3*M_PI_2/(n1+1)); + theta1 = Eigen::VectorXd::LinSpaced(n1, -1 * M_PI_2, M_PI - 3 * M_PI_2 / (n1 + 1)); for (int i = 0; i < theta1.size(); i++) { - xyM(0,i+n0) = radius * cos(theta1[i]) + fabs(radius); - xyM(1,i+n0) = radius * sin(theta1[i]) + 2 * fabs(radius); - headingArray[i+n0] = theta1[i] - M_PI_2 + M_PI; + xyM(0, i + n0) = radius * cos(theta1[i]) + fabs(radius); + xyM(1, i + n0) = radius * sin(theta1[i]) + 2 * fabs(radius); + headingArray[i + n0] = theta1[i] - M_PI_2 + M_PI; } - //third section - int n2 = calculatedNoOfPoints - n1 - n0; + // third section + int n2 = calculatedNoOfPoints - n1 - n0; endStraight = Eigen::VectorXd::LinSpaced(n2, fabs(radius) * 2, 0); for (int i = 0; i < n2; i++) { - xyM(0,i+n0+n1) = 0; - xyM(1,i+n0+n1) = endStraight[i]; - headingArray[i+n0+n1] = M_PI_2 + M_PI; + xyM(0, i + n0 + n1) = 0; + xyM(1, i + n0 + n1) = endStraight[i]; + headingArray[i + n0 + n1] = M_PI_2 + M_PI; } // Rotate turn to match start point - Eigen::Rotation2Dd rotM(headingRad-M_PI_2); + Eigen::Rotation2Dd rotM(headingRad - M_PI_2); resM = rotM.toRotationMatrix() * xyM; - - //Offset result matrix + // Offset result matrix for (int i = 0; i < calculatedNoOfPoints; i++) { - resM(0,i) += startPoint.getXCoord(); - resM(1,i) += startPoint.getYCoord(); + resM(0, i) += startPoint.getXCoord(); + resM(1, i) += startPoint.getYCoord(); } - //Heading in rad with offset to match ENU - headingArray += (headingRad-M_PI_2) * Eigen::ArrayXd::Ones(calculatedNoOfPoints); - + // Heading in rad with offset to match ENU + headingArray += (headingRad - M_PI_2) * Eigen::ArrayXd::Ones(calculatedNoOfPoints); - //AccelerationSection - auto accelerationPeriod = milliseconds(static_cast(topSpeed / acceleration * 1000)); + // AccelerationSection + auto accelerationPeriod = milliseconds(static_cast(topSpeed / acceleration * 1000)); double accelerationDistance = pow(topSpeed, 2) / acceleration / 2; - //Topspeed section - double topSpeedDistance = totalLength - accelerationDistance*2; - auto topSpeedPeriod = milliseconds(static_cast(topSpeedDistance / topSpeed * 1000)); + // Topspeed section + double topSpeedDistance = totalLength - accelerationDistance * 2; + auto topSpeedPeriod = milliseconds(static_cast(topSpeedDistance / topSpeed * 1000)); - auto totalRuntime = accelerationPeriod + topSpeedPeriod + accelerationPeriod; //Accelerate -> Top Speed -> Decelerate + auto totalRuntime = + accelerationPeriod + topSpeedPeriod + accelerationPeriod; // Accelerate -> Top Speed -> Decelerate auto timeStep = totalRuntime / calculatedNoOfPoints; - - //Speed for each point + // Speed for each point double currSpeed = 0; for (int i = 0; i < calculatedNoOfPoints; i++) { - timeArray[i] = i*timeStep; + timeArray[i] = i * timeStep; if (timeArray[i] < accelerationPeriod) { - currSpeed += acceleration * duration_cast(timeStep).count()/1000; + currSpeed += acceleration * duration_cast(timeStep).count() / 1000; accelerationArray[i] = acceleration; - } - else if (timeArray[i] < topSpeedPeriod + accelerationPeriod) { + } else if (timeArray[i] < topSpeedPeriod + accelerationPeriod) { if (currSpeed > topSpeed) { currSpeed = topSpeed; } accelerationArray[i] = 0; - } - else { - currSpeed -= acceleration * duration_cast(timeStep).count()/1000; + } else { + currSpeed -= acceleration * duration_cast(timeStep).count() / 1000; accelerationArray[i] = -acceleration; } speedArray[i] = currSpeed; - } Eigen::VectorXd curvatureArray(calculatedNoOfPoints); - auto v1 = -1/radius*Eigen::ArrayXd::Ones(n0); - auto v2 = -1/radius*Eigen::ArrayXd::Ones(n1); - auto v3 = -1/radius*Eigen::ArrayXd::Zero(n2); - RCLCPP_DEBUG(startPoint.get_logger(), "curvatureArray rows: %ld, cols: %ld", curvatureArray.rows(), curvatureArray.cols()); + auto v1 = -1 / radius * Eigen::ArrayXd::Ones(n0); + auto v2 = -1 / radius * Eigen::ArrayXd::Ones(n1); + auto v3 = -1 / radius * Eigen::ArrayXd::Zero(n2); + RCLCPP_DEBUG( + startPoint.get_logger(), "curvatureArray rows: %ld, cols: %ld", curvatureArray.rows(), curvatureArray.cols()); RCLCPP_DEBUG(startPoint.get_logger(), "v1 rows: %ld, cols: %ld", v1.rows(), v1.cols()); RCLCPP_DEBUG(startPoint.get_logger(), "v2 rows: %ld, cols: %ld", v2.rows(), v2.cols()); RCLCPP_DEBUG(startPoint.get_logger(), "v3 rows: %ld, cols: %ld", v3.rows(), v3.cols()); - curvatureArray << -1/radius*Eigen::ArrayXd::Ones(n0), 1/radius*Eigen::ArrayXd::Ones(n1), Eigen::ArrayXd::Zero(n2); + curvatureArray << -1 / radius * Eigen::ArrayXd::Ones(n0), 1 / radius * Eigen::ArrayXd::Ones(n1), + Eigen::ArrayXd::Zero(n2); - //create trajectory points + // create trajectory points std::vector tempVector; - for(int i = 0; i < calculatedNoOfPoints; i++) { + for (int i = 0; i < calculatedNoOfPoints; i++) { TrajectoryPoint tempPoint(startPoint.get_logger()); - tempPoint.setTime(timeArray[i]+startTime); - tempPoint.setXCoord(resM(0,i)); - tempPoint.setYCoord(resM(1,i)); + tempPoint.setTime(timeArray[i] + startTime); + tempPoint.setXCoord(resM(0, i)); + tempPoint.setYCoord(resM(1, i)); tempPoint.setZCoord(startPoint.getZCoord()); tempPoint.setHeading(headingArray[i]); tempPoint.setLongitudinalVelocity(speedArray[i]); @@ -638,11 +620,10 @@ Trajectory Trajectory::createWilliamsonTurn( Trajectory retval(startPoint.get_logger()); retval.points = tempVector; - retval.name = "Williamson_x" + std::to_string(startPoint.getXCoord()) - + "_y" + std::to_string(startPoint.getYCoord()) - + "_z" + std::to_string(startPoint.getZCoord()) - + "_hdg" + std::to_string(headingRad*180.0/M_PI); - retval.id = 0; + retval.name = "Williamson_x" + std::to_string(startPoint.getXCoord()) + "_y" + + std::to_string(startPoint.getYCoord()) + "_z" + std::to_string(startPoint.getZCoord()) + "_hdg" + + std::to_string(headingRad * 180.0 / M_PI); + retval.id = 0; retval.version = 0; return retval; } @@ -662,31 +643,28 @@ Trajectory Trajectory::reversed() const { std::reverse(newTrajectory.points.begin(), newTrajectory.points.end()); std::vector timeVector; - for (auto & point : newTrajectory.points) { - point.setHeading(point.getHeading()-M_PI); - point.setCurvature(point.getCurvature()*-1); + for (auto& point : newTrajectory.points) { + point.setHeading(point.getHeading() - M_PI); + point.setCurvature(point.getCurvature() * -1); try { - point.setLateralVelocity(point.getLateralVelocity()*-1); - } - catch (std::out_of_range) { + point.setLateralVelocity(point.getLateralVelocity() * -1); + } catch (std::out_of_range) { RCLCPP_DEBUG(get_logger(), "Ignoring uninitialized lateral velocity"); } try { - point.setLateralAcceleration(point.getLateralAcceleration()*-1); - } - catch (std::out_of_range) { + point.setLateralAcceleration(point.getLateralAcceleration() * -1); + } catch (std::out_of_range) { RCLCPP_DEBUG(get_logger(), "Ignoring uninitialized lateral acceleration"); } try { - point.setLongitudinalAcceleration(point.getLongitudinalAcceleration()*-1); - } - catch (std::out_of_range) { + point.setLongitudinalAcceleration(point.getLongitudinalAcceleration() * -1); + } catch (std::out_of_range) { RCLCPP_DEBUG(get_logger(), "Ignoring uninitialized longitudinal acceleration"); } } const_reverse_iterator origPoint = this->points.rbegin(); - iterator newPoint = newTrajectory.points.begin(); + iterator newPoint = newTrajectory.points.begin(); for (; origPoint != this->points.crend() && newPoint != newTrajectory.points.end(); ++origPoint, ++newPoint) { // t_new[i] = t_old[end] - t_old[end-i] newPoint->setTime(this->points.back().getTime() - origPoint->getTime()); @@ -704,7 +682,7 @@ void Trajectory::saveToFile(const std::string& fileName) const { using std::string, std::smatch, std::ofstream; char trajDirPath[PATH_MAX]; - UtilGetTrajDirectoryPath(trajDirPath, sizeof (trajDirPath)); + UtilGetTrajDirectoryPath(trajDirPath, sizeof(trajDirPath)); string trajFilePath(trajDirPath); trajFilePath += fileName; @@ -713,27 +691,25 @@ void Trajectory::saveToFile(const std::string& fileName) const { try { outputTraj.open(trajFilePath); RCLCPP_DEBUG(get_logger(), "Outputting trajectory to file"); - outputTraj << "TRAJECTORY;" << this->id <<";" << this->name << ";" << this->version << ";" << this->points.size() << ";" << "\n"; + outputTraj << "TRAJECTORY;" << this->id << ";" << this->name << ";" << this->version << ";" + << this->points.size() << ";" << "\n"; for (const auto& point : points) { - outputTraj << "LINE;" - << std::fixed << std::setprecision(2) << std::chrono::duration(point.getTime()).count() << ";" - << std::fixed << std::setprecision(6) << point.getXCoord() << ";" - << std::fixed << std::setprecision(6) << point.getYCoord() << ";" - << std::fixed << std::setprecision(6) << point.getZCoord() << ";" - << std::fixed << std::setprecision(6) << point.getHeading() << ";" - << std::fixed << std::setprecision(6) << point.getLongitudinalVelocity() << ";" - << ";" //point.getLateralVelocity() << ";" - << std::fixed << std::setprecision(6) <<(point.getLongitudinalAcceleration()) << ";" - << ";" //point.getLateralAcceleration() << ";" - << std::fixed << std::setprecision(6) << point.getCurvature() << ";" - << std::fixed << std::setprecision(6) << point.getMode() - <<";ENDLINE;" << "\n"; + outputTraj << "LINE;" << std::fixed << std::setprecision(2) + << std::chrono::duration(point.getTime()).count() << ";" << std::fixed + << std::setprecision(6) << point.getXCoord() << ";" << std::fixed << std::setprecision(6) + << point.getYCoord() << ";" << std::fixed << std::setprecision(6) << point.getZCoord() << ";" + << std::fixed << std::setprecision(6) << point.getHeading() << ";" << std::fixed + << std::setprecision(6) << point.getLongitudinalVelocity() << ";" + << ";" // point.getLateralVelocity() << ";" + << std::fixed << std::setprecision(6) << (point.getLongitudinalAcceleration()) << ";" + << ";" // point.getLateralAcceleration() << ";" + << std::fixed << std::setprecision(6) << point.getCurvature() << ";" << std::fixed + << std::setprecision(6) << point.getMode() << ";ENDLINE;" << "\n"; } - outputTraj << "ENDTRAJECTORY;" << "\n"; + outputTraj << "ENDTRAJECTORY;" << "\n"; outputTraj.close(); RCLCPP_DEBUG(get_logger(), "Closed file %s", trajFilePath.c_str()); - } - catch (const ofstream::failure& e) { + } catch (const ofstream::failure& e) { RCLCPP_ERROR(get_logger(), "Failed when writing to file %s", trajFilePath.c_str()); } } diff --git a/atos/common/trajectory.hpp b/atos/common/trajectory.hpp index 07d839818..b366ce74e 100644 --- a/atos/common/trajectory.hpp +++ b/atos/common/trajectory.hpp @@ -6,20 +6,20 @@ #ifndef TRAJECTORY_H #define TRAJECTORY_H -#include -#include -#include +#include +#include #include -#include +#include #include -#include +#include +#include #include -#include #include -#include +#include +#include -#include "loggable.hpp" #include "atos_interfaces/msg/cartesian_trajectory.hpp" +#include "loggable.hpp" #include "util.h" //! ATOS Namespace @@ -28,53 +28,94 @@ class Trajectory : public Loggable { public: class TrajectoryPoint : public Loggable { public: - typedef enum { - CONTROLLED_BY_DRIVE_FILE, - CONTROLLED_BY_VEHICLE - } ModeType; - - TrajectoryPoint(rclcpp::Logger log) : Loggable(log) { - position[2] = std::numeric_limits::quiet_NaN(); - velocity[0] = std::numeric_limits::quiet_NaN(); - velocity[1] = std::numeric_limits::quiet_NaN(); + typedef enum { CONTROLLED_BY_DRIVE_FILE, CONTROLLED_BY_VEHICLE } ModeType; + + TrajectoryPoint(rclcpp::Logger log) : + Loggable(log) { + position[2] = std::numeric_limits::quiet_NaN(); + velocity[0] = std::numeric_limits::quiet_NaN(); + velocity[1] = std::numeric_limits::quiet_NaN(); acceleration[0] = std::numeric_limits::quiet_NaN(); acceleration[1] = std::numeric_limits::quiet_NaN(); } - ~TrajectoryPoint() { } + ~TrajectoryPoint() {} TrajectoryPoint relativeTo(const TrajectoryPoint& other) const; - template - void setTime(const std::chrono::duration& value) { time = value; } - void setTime(const double value) { const auto timeVar = static_cast( value * 1000 ); time = std::chrono::milliseconds(timeVar); } - void setPosition(const Eigen::Vector3d& value) { position = value; } - void setXCoord(const double& value) { position[0] = value; } - void setYCoord(const double& value) { position[1] = value; } - void setZCoord(const double& value) { position[2] = value; } - void setHeading(const double& value) { heading = std::fmod(value, 2*M_PI); - heading += heading < 0 ? 2*M_PI : 0.0; } - void setVelocity(const Eigen::Vector2d& value) { velocity = value; } - void setLongitudinalVelocity(const double& value) { velocity[0] = value; } - void setLateralVelocity(const double& value) { velocity[1] = value; } - void setAcceleration(const Eigen::Vector2d& value) { acceleration = value; } - void setLongitudinalAcceleration(const double& value) { acceleration[0] = value; } - void setLateralAcceleration(const double& value) { acceleration[1] = value; } - void setCurvature(const double& value) { curvature = value; } - void setMode(const ModeType& value) { mode = value; } - - std::chrono::milliseconds getTime() const { return time; } - Eigen::Vector3d getPosition() const { return position; } - double getXCoord() const { return position[0]; } - double getYCoord() const { return position[1]; } + template + void setTime(const std::chrono::duration& value) { + time = value; + } + void setTime(const double value) { + const auto timeVar = static_cast(value * 1000); + time = std::chrono::milliseconds(timeVar); + } + void setPosition(const Eigen::Vector3d& value) { + position = value; + } + void setXCoord(const double& value) { + position[0] = value; + } + void setYCoord(const double& value) { + position[1] = value; + } + void setZCoord(const double& value) { + position[2] = value; + } + void setHeading(const double& value) { + heading = std::fmod(value, 2 * M_PI); + heading += heading < 0 ? 2 * M_PI : 0.0; + } + void setVelocity(const Eigen::Vector2d& value) { + velocity = value; + } + void setLongitudinalVelocity(const double& value) { + velocity[0] = value; + } + void setLateralVelocity(const double& value) { + velocity[1] = value; + } + void setAcceleration(const Eigen::Vector2d& value) { + acceleration = value; + } + void setLongitudinalAcceleration(const double& value) { + acceleration[0] = value; + } + void setLateralAcceleration(const double& value) { + acceleration[1] = value; + } + void setCurvature(const double& value) { + curvature = value; + } + void setMode(const ModeType& value) { + mode = value; + } + + std::chrono::milliseconds getTime() const { + return time; + } + Eigen::Vector3d getPosition() const { + return position; + } + double getXCoord() const { + return position[0]; + } + double getYCoord() const { + return position[1]; + } double getZCoord() const { if (std::isnan(position[2])) throw std::out_of_range("Uninitialized member z"); else return position[2]; } - double getHeading() const { return heading; } - Eigen::Vector2d getVelocity() const { return velocity; } + double getHeading() const { + return heading; + } + Eigen::Vector2d getVelocity() const { + return velocity; + } double getLongitudinalVelocity() const { if (std::isnan(velocity[0])) throw std::out_of_range("Uninitialized member longitudinal velocity"); @@ -87,7 +128,9 @@ class Trajectory : public Loggable { else return velocity[1]; } - Eigen::Vector2d getAcceleration() const { return acceleration; } + Eigen::Vector2d getAcceleration() const { + return acceleration; + } double getLongitudinalAcceleration() const { if (std::isnan(acceleration[0])) throw std::out_of_range("Uninitialized member longitudinal acceleration"); @@ -100,8 +143,12 @@ class Trajectory : public Loggable { else return acceleration[1]; } - double getCurvature() const { return this->curvature; } - ModeType getMode() const { return this->mode; } + double getCurvature() const { + return this->curvature; + } + ModeType getMode() const { + return this->mode; + } CartesianPosition getISOPosition() const; SpeedType getISOVelocity() const; @@ -109,18 +156,18 @@ class Trajectory : public Loggable { std::string toString() const; std::string getFormatString() const; + private: std::chrono::milliseconds time = std::chrono::milliseconds(0); - Eigen::Vector3d position; //! x, y, z [m] - double heading = 0; //! Heading ccw from x axis [rad] - Eigen::Vector2d velocity; //! Vehicle frame, x forward [m/s] + Eigen::Vector3d position; //! x, y, z [m] + double heading = 0; //! Heading ccw from x axis [rad] + Eigen::Vector2d velocity; //! Vehicle frame, x forward [m/s] Eigen::Vector2d acceleration; //! Vehicle frame, x forward [m/s] double curvature = 0; - ModeType mode = CONTROLLED_BY_VEHICLE; + ModeType mode = CONTROLLED_BY_VEHICLE; template - Eigen::Matrix zeroNaNs( - const Eigen::Matrix& v) const { + Eigen::Matrix zeroNaNs(const Eigen::Matrix& v) const { assert(v.SizeAtCompileTime != 0 && "Zero size matrix passed to zeroNaNs"); Eigen::Matrix ret(v); for (int i = 0; i < ret.SizeAtCompileTime; ++i) { @@ -134,13 +181,16 @@ class Trajectory : public Loggable { typedef std::vector::reverse_iterator reverse_iterator; typedef std::vector::const_reverse_iterator const_reverse_iterator; - Trajectory(rclcpp::Logger log) : Loggable(log) {} - ~Trajectory() { points.clear(); } + Trajectory(rclcpp::Logger log) : + Loggable(log) {} + ~Trajectory() { + points.clear(); + } Trajectory(const Trajectory& other); std::vector points; - std::string name = ""; + std::string name = ""; unsigned short version = 0; - unsigned short id = 0; + unsigned short id = 0; Trajectory& operator=(const Trajectory& other); @@ -151,20 +201,26 @@ class Trajectory : public Loggable { std::string toString() const; atos_interfaces::msg::CartesianTrajectory toCartesianTrajectory(); nav_msgs::msg::Path toPath() const; - foxglove_msgs::msg::GeoJSON toGeoJSON(std::array llh_0) const; - std::size_t size() const { return points.size(); } + foxglove_msgs::msg::GeoJSON toGeoJSON(std::array llh_0) const; + std::size_t size() const { + return points.size(); + } void saveToFile(const std::string& fileName) const; Trajectory reversed() const; Trajectory rescaledToVelocity(const double vel_m_s) const; - static Trajectory createWilliamsonTurn(double turnRadius, double acceleration, double minSpeed, double maxSpeed, - TrajectoryPoint startPoint, std::chrono::milliseconds startTime = std::chrono::milliseconds(0)); + static Trajectory createWilliamsonTurn(double turnRadius, + double acceleration, + double minSpeed, + double maxSpeed, + TrajectoryPoint startPoint, + std::chrono::milliseconds startTime = std::chrono::milliseconds(0)); Trajectory appendedWith(const Trajectory& other); - template - Trajectory delayed(const std::chrono::duration& delay) const { + template + Trajectory delayed(const std::chrono::duration& delay) const { Trajectory newTrajectory = Trajectory(*this); - newTrajectory.name = newTrajectory.name + "_delayed"; + newTrajectory.name = newTrajectory.name + "_delayed"; for (auto& trajPt : newTrajectory.points) { trajPt.setTime(trajPt.getTime() + delay); } @@ -172,6 +228,7 @@ class Trajectory : public Loggable { } bool isValid() const; + private: static const std::regex fileHeaderPattern; static const std::regex fileLinePattern; diff --git a/atos/common/type.cpp b/atos/common/type.cpp index 54b5d1992..90797b5ee 100644 --- a/atos/common/type.cpp +++ b/atos/common/type.cpp @@ -7,26 +7,23 @@ #include "type.h" #ifdef __GNUG__ #include -#include #include +#include std::string demangle(const char* name) { - int status = -4; // some arbitrary value to eliminate the compiler warning + int status = -4; // some arbitrary value to eliminate the compiler warning - std::unique_ptr res { - abi::__cxa_demangle(name, NULL, NULL, &status), - std::free - }; + std::unique_ptr res{abi::__cxa_demangle(name, NULL, NULL, &status), std::free}; - return (status==0) ? res.get() : name ; + return (status == 0) ? res.get() : name; } #else // does nothing if not g++ std::string demangle(const char* name) { - return name; + return name; } #endif diff --git a/atos/launch/launch_basic.py b/atos/launch/launch_basic.py index 6eaefbcc0..4e29f0063 100644 --- a/atos/launch/launch_basic.py +++ b/atos/launch/launch_basic.py @@ -1,12 +1,18 @@ -import sys import os +import sys + from ament_index_python.packages import get_package_prefix -sys.path.insert(0, os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path - get_package_prefix('atos'), - 'share', 'atos', 'launch')) + +sys.path.insert( + 0, + os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path + get_package_prefix("atos"), "share", "atos", "launch" + ), +) import launch_utils.launch_base as launch_base from launch import LaunchDescription + def generate_launch_description(): base_nodes = launch_base.get_base_nodes() return LaunchDescription(base_nodes) diff --git a/atos/launch/launch_full.py b/atos/launch/launch_full.py index e8601a276..005176999 100644 --- a/atos/launch/launch_full.py +++ b/atos/launch/launch_full.py @@ -1,14 +1,18 @@ -import sys import os +import sys + from ament_index_python.packages import get_package_prefix -sys.path.insert(0, os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path - get_package_prefix('atos'), - 'share', 'atos', 'launch')) + +sys.path.insert( + 0, + os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path + get_package_prefix("atos"), "share", "atos", "launch" + ), +) import launch_utils.launch_base as launch_base from launch import LaunchDescription -from launch_ros.actions import Node -from launch_graphical import get_graphical_nodes from launch_experimental import get_experimental_nodes +from launch_graphical import get_graphical_nodes def generate_launch_description(): @@ -17,10 +21,10 @@ def generate_launch_description(): graphical_nodes = get_graphical_nodes() experimental_nodes = get_experimental_nodes() - for node in graphical_nodes: - base_nodes.append(node) - + for node in graphical_nodes: + base_nodes.append(node) + for node in experimental_nodes: - base_nodes.append(node) + base_nodes.append(node) return LaunchDescription(base_nodes) diff --git a/atos/launch/launch_graphical.py b/atos/launch/launch_graphical.py index c3d03a569..7743ac56f 100644 --- a/atos/launch/launch_graphical.py +++ b/atos/launch/launch_graphical.py @@ -1,25 +1,32 @@ -import sys import os +import sys + from ament_index_python.packages import get_package_prefix -sys.path.insert(0, os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path - get_package_prefix('atos'), - 'share', 'atos', 'launch')) + +sys.path.insert( + 0, + os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path + get_package_prefix("atos"), "share", "atos", "launch" + ), +) import launch_utils.launch_base as launch_base from launch import LaunchDescription from launch_ros.actions import Node + def get_graphical_nodes(): files = launch_base.get_files() return [ Node( - package='atos', - namespace='atos', - executable='pointcloud_publisher', - name='pointcloud_publisher', - parameters=[files["params"]] + package="atos", + namespace="atos", + executable="pointcloud_publisher", + name="pointcloud_publisher", + parameters=[files["params"]], ) ] + def generate_launch_description(): base_nodes = launch_base.get_base_nodes() graphical_nodes = get_graphical_nodes() diff --git a/atos/launch/launch_integration_testing.py b/atos/launch/launch_integration_testing.py index 755d122e7..875357b7f 100644 --- a/atos/launch/launch_integration_testing.py +++ b/atos/launch/launch_integration_testing.py @@ -1,31 +1,37 @@ -import sys import os +import sys + from ament_index_python.packages import get_package_prefix -sys.path.insert(0, os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path - get_package_prefix('atos'), - 'share', 'atos', 'launch')) -from launch_ros.actions import Node -from launch import LaunchDescription + +sys.path.insert( + 0, + os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path + get_package_prefix("atos"), "share", "atos", "launch" + ), +) import launch_utils.launch_base as launch_base +from launch import LaunchDescription +from launch_ros.actions import Node def get_integration_test_nodes(): files = launch_base.get_files() return [ Node( - package='atos', - namespace='atos', - executable='integration_testing_handler', - parameters=[files["params"]] + package="atos", + namespace="atos", + executable="integration_testing_handler", + parameters=[files["params"]], ) ] + def generate_launch_description(): base_nodes = launch_base.get_base_nodes() - + integration_test_nodes = get_integration_test_nodes() for node in integration_test_nodes: base_nodes.append(node) - + return LaunchDescription(base_nodes) diff --git a/atos/launch/launch_utils/generate_cert.py b/atos/launch/launch_utils/generate_cert.py index c4b6a263b..cfd69269c 100644 --- a/atos/launch/launch_utils/generate_cert.py +++ b/atos/launch/launch_utils/generate_cert.py @@ -1,4 +1,6 @@ from OpenSSL import crypto + + def cert_gen( emailAddress="emailAddress", commonName="commonName", @@ -9,11 +11,12 @@ def cert_gen( organizationUnitName="organizationUnitName", serialNumber=0, validityStartInSeconds=0, - validityEndInSeconds=10*365*24*60*60, - KEY_FILE = "private.key", - CERT_FILE="selfsigned.crt"): - #can look at generated file using openssl: - #openssl x509 -inform pem -in selfsigned.crt -noout -text + validityEndInSeconds=10 * 365 * 24 * 60 * 60, + KEY_FILE="private.key", + CERT_FILE="selfsigned.crt", +): + # can look at generated file using openssl: + # openssl x509 -inform pem -in selfsigned.crt -noout -text # create a key pair k = crypto.PKey() k.generate_key(crypto.TYPE_RSA, 4096) @@ -31,7 +34,7 @@ def cert_gen( cert.gmtime_adj_notAfter(validityEndInSeconds) cert.set_issuer(cert.get_subject()) cert.set_pubkey(k) - cert.sign(k, 'sha512') + cert.sign(k, "sha512") with open(CERT_FILE, "wt") as f: f.write(crypto.dump_certificate(crypto.FILETYPE_PEM, cert).decode("utf-8")) with open(KEY_FILE, "wt") as f: diff --git a/atos/launch/launch_utils/launch_base.py b/atos/launch/launch_utils/launch_base.py index cfae0c69f..9cedf320f 100644 --- a/atos/launch/launch_utils/launch_base.py +++ b/atos/launch/launch_utils/launch_base.py @@ -1,15 +1,17 @@ # Don't launch this file directly, rather use the launch files one level up instead +import copy import os -from launch_ros.actions import Node +from pathlib import Path + +import rclpy.logging as logging from ament_index_python.packages import get_package_prefix, get_package_share_directory -from launch.substitutions import LaunchConfiguration, PythonExpression from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from pathlib import Path +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node + from .validate_files import validate_atos_dir -import rclpy.logging as logging -import copy def print_version(): diff --git a/atos/launch/launch_utils/validate_files.py b/atos/launch/launch_utils/validate_files.py index 38ae3584f..da71139ae 100644 --- a/atos/launch/launch_utils/validate_files.py +++ b/atos/launch/launch_utils/validate_files.py @@ -1,43 +1,54 @@ -from pathlib import Path -from .generate_cert import cert_gen -from ament_index_python.packages import get_package_prefix import os import shutil +from pathlib import Path + import rclpy.logging as logging +from ament_index_python.packages import get_package_prefix + +from .generate_cert import cert_gen + def try_create_directory(directory_path): if not os.path.exists(directory_path): - os.makedirs(directory_path), - logging.get_logger('launch').info( - f"Missing directory {directory_path}, created") + (os.makedirs(directory_path),) + logging.get_logger("launch").info( + f"Missing directory {directory_path}, created" + ) else: - logging.get_logger('launch').debug( - f"Directory {directory_path} exists, skipping") + logging.get_logger("launch").debug( + f"Directory {directory_path} exists, skipping" + ) -def validate_directory_contents(dir,copy_to_path): - from_files = dir.glob('*') + +def validate_directory_contents(dir, copy_to_path): + from_files = dir.glob("*") for from_file in from_files: if from_file.is_dir(): validate_directory(from_file, copy_to_path / Path(from_file.name)) elif from_file.is_file(): validate_file(from_file, copy_to_path / Path(from_file.name)) else: - logging.get_logger('launch').info( - f"File {from_file.name} is not a file or directory, skipping") + logging.get_logger("launch").info( + f"File {from_file.name} is not a file or directory, skipping" + ) return dir + def validate_directory(copy_from_path, copy_to_path): try_create_directory(copy_to_path) validate_directory_contents(copy_from_path, copy_to_path) return copy_to_path + def validate_file(copy_from_path, copy_to_path): if not os.path.exists(copy_to_path): shutil.copy(copy_from_path, copy_to_path, follow_symlinks=True) - logging.get_logger('launch').info( - f"Missing file {copy_to_path.name}, copied default from {copy_from_path}") + logging.get_logger("launch").info( + f"Missing file {copy_to_path.name}, copied default from {copy_from_path}" + ) return copy_to_path + def validate_certs(directory_path): try_create_directory(directory_path) @@ -48,11 +59,15 @@ def validate_certs(directory_path): cert_gen(KEY_FILE=key, CERT_FILE=cert) return [cert, key] + def validate_atos_dir(): - atos_dir = os.path.join(os.path.expanduser('~'), '.astazero', 'ATOS') + atos_dir = os.path.join(os.path.expanduser("~"), ".astazero", "ATOS") dirs_to_validate = ["conf", "pointclouds", "logs", "odr", "osc", "Catalogs"] for dir_to_validate in dirs_to_validate: - validate_directory(get_package_prefix('atos') / Path("etc") / Path(dir_to_validate), atos_dir / Path(dir_to_validate)) + validate_directory( + get_package_prefix("atos") / Path("etc") / Path(dir_to_validate), + atos_dir / Path(dir_to_validate), + ) # Certs [cert, key] = validate_certs(atos_dir / Path("certs")) @@ -60,5 +75,5 @@ def validate_atos_dir(): return { "params": atos_dir / Path("conf") / Path("params.yaml"), "cert": cert, - "key": key + "key": key, } diff --git a/atos/modules/BackToStart/inc/backtostart.hpp b/atos/modules/BackToStart/inc/backtostart.hpp index 4afc1a1d5..4c432b858 100644 --- a/atos/modules/BackToStart/inc/backtostart.hpp +++ b/atos/modules/BackToStart/inc/backtostart.hpp @@ -5,9 +5,9 @@ */ #pragma once +#include "atos_interfaces/srv/get_object_return_trajectory.hpp" #include "module.hpp" #include "trajectory.hpp" -#include "atos_interfaces/srv/get_object_return_trajectory.hpp" /*! * \brief The BackToStart class offers services to calculate a trajectory to return test objects to start position. @@ -19,8 +19,9 @@ class BackToStart : public Module { private: static inline std::string const moduleName = "back_to_start"; - rclcpp::Service::SharedPtr getObjectReturnTrajectoryService; //!< Service to request object return trajectory + rclcpp::Service::SharedPtr + getObjectReturnTrajectoryService; //!< Service to request object return trajectory - void onReturnTrajectoryRequest(const std::shared_ptr, - std::shared_ptr); + void onReturnTrajectoryRequest(const std::shared_ptr, + std::shared_ptr); }; diff --git a/atos/modules/BackToStart/src/backtostart.cpp b/atos/modules/BackToStart/src/backtostart.cpp index 150f2d490..f62fa0b36 100644 --- a/atos/modules/BackToStart/src/backtostart.cpp +++ b/atos/modules/BackToStart/src/backtostart.cpp @@ -6,10 +6,10 @@ #include "backtostart.hpp" using std::placeholders::_1, std::placeholders::_2; -BackToStart::BackToStart() : Module(BackToStart::moduleName) -{ - getObjectReturnTrajectoryService = create_service(ServiceNames::getObjectReturnTrajectory, - std::bind(&BackToStart::onReturnTrajectoryRequest, this, _1, _2)); +BackToStart::BackToStart() : + Module(BackToStart::moduleName) { + getObjectReturnTrajectoryService = create_service( + ServiceNames::getObjectReturnTrajectory, std::bind(&BackToStart::onReturnTrajectoryRequest, this, _1, _2)); this->declare_parameter("turn_radius", 5.0); this->declare_parameter("min_speed", 4.0); this->declare_parameter("max_speed", 12.0); @@ -17,52 +17,57 @@ BackToStart::BackToStart() : Module(BackToStart::moduleName) /** * @brief Callback for the get_object_return_trajectory service - * + * * @param request Includes the id of the object and the trajectory * @param response Includes the id of the object and the return trajectory -*/ -void BackToStart::onReturnTrajectoryRequest(const std::shared_ptr request, - std::shared_ptr response) { - - if (request->trajectory.points.size() == 0) { - RCLCPP_ERROR(get_logger(), "Received empty trajectory"); - response->success = false; - return; - } + */ +void BackToStart::onReturnTrajectoryRequest( + const std::shared_ptr request, + std::shared_ptr response) { + + if (request->trajectory.points.size() == 0) { + RCLCPP_ERROR(get_logger(), "Received empty trajectory"); + response->success = false; + return; + } // Get the turn radius of the williamson turn. double turnRadius; - double minSpeed; - double maxSpeed; + double minSpeed; + double maxSpeed; this->get_parameter("turn_radius", turnRadius); this->get_parameter("min_speed", minSpeed); this->get_parameter("max_speed", maxSpeed); - - // Load the trajectory in a Trajectory object - ATOS::Trajectory currentTraj(get_logger()); - currentTraj.initializeFromCartesianTrajectory(request->trajectory); - uint32_t id = request->id; + // Load the trajectory in a Trajectory object + ATOS::Trajectory currentTraj(get_logger()); + currentTraj.initializeFromCartesianTrajectory(request->trajectory); + uint32_t id = request->id; - // Create a new trajectory object for the return trajectory - ATOS::Trajectory b2sTraj(get_logger()); + // Create a new trajectory object for the return trajectory + ATOS::Trajectory b2sTraj(get_logger()); - //Add first turn - ATOS::Trajectory turn1 = ATOS::Trajectory::createWilliamsonTurn(turnRadius, 1, minSpeed, maxSpeed, currentTraj.points.back()); - b2sTraj.points.insert(b2sTraj.points.end(), std::begin(turn1.points), turn1.points.end()); + // Add first turn + ATOS::Trajectory turn1 = + ATOS::Trajectory::createWilliamsonTurn(turnRadius, 1, minSpeed, maxSpeed, currentTraj.points.back()); + b2sTraj.points.insert(b2sTraj.points.end(), std::begin(turn1.points), turn1.points.end()); - //Add reversed original traj - auto rev = currentTraj.reversed().delayed(b2sTraj.points.back().getTime()); - b2sTraj.points.insert(b2sTraj.points.end(), std::begin(rev.points), rev.points.end()); + // Add reversed original traj + auto rev = currentTraj.reversed().delayed(b2sTraj.points.back().getTime()); + b2sTraj.points.insert(b2sTraj.points.end(), std::begin(rev.points), rev.points.end()); - //Add last turn - ATOS::Trajectory turn2 = ATOS::Trajectory::createWilliamsonTurn(turnRadius, 1, minSpeed, maxSpeed, b2sTraj.points.back()); - turn2 = turn2.delayed(b2sTraj.points.back().getTime()); - b2sTraj.points.insert(b2sTraj.points.end(), std::begin(turn2.points), turn2.points.end()); + // Add last turn + ATOS::Trajectory turn2 = + ATOS::Trajectory::createWilliamsonTurn(turnRadius, 1, minSpeed, maxSpeed, b2sTraj.points.back()); + turn2 = turn2.delayed(b2sTraj.points.back().getTime()); + b2sTraj.points.insert(b2sTraj.points.end(), std::begin(turn2.points), turn2.points.end()); - // Fill the response message - response->id = id; - response->trajectory = b2sTraj.toCartesianTrajectory(); - response->success = true; - RCLCPP_INFO(get_logger(), "Calculated return trajectory for object %u with %lu points", response->id, response->trajectory.points.size()); + // Fill the response message + response->id = id; + response->trajectory = b2sTraj.toCartesianTrajectory(); + response->success = true; + RCLCPP_INFO(get_logger(), + "Calculated return trajectory for object %u with %lu points", + response->id, + response->trajectory.points.size()); }; diff --git a/atos/modules/BackToStart/src/main.cpp b/atos/modules/BackToStart/src/main.cpp index 9b67595bb..f4db3dc28 100644 --- a/atos/modules/BackToStart/src/main.cpp +++ b/atos/modules/BackToStart/src/main.cpp @@ -5,8 +5,7 @@ */ #include "backtostart.hpp" - -int main(int argc, char** argv){ +int main(int argc, char** argv) { rclcpp::init(argc, argv); std::shared_ptr btsNode = std::make_shared(); rclcpp::spin(btsNode); diff --git a/atos/modules/BackToStart/tests/main.cpp b/atos/modules/BackToStart/tests/main.cpp index 92cbc41f1..83cca6f1a 100644 --- a/atos/modules/BackToStart/tests/main.cpp +++ b/atos/modules/BackToStart/tests/main.cpp @@ -1,7 +1,6 @@ #include "gtest/gtest.h" -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/atos/modules/BackToStart/tests/test_backtostart.cpp b/atos/modules/BackToStart/tests/test_backtostart.cpp index 2b4a0248b..41df23280 100644 --- a/atos/modules/BackToStart/tests/test_backtostart.cpp +++ b/atos/modules/BackToStart/tests/test_backtostart.cpp @@ -1,121 +1,119 @@ -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "testUtils/testUtils.hpp" -#include "atos_interfaces/srv/get_object_return_trajectory.hpp" #include "atos_interfaces/msg/cartesian_trajectory_point.hpp" -#include "geometry_msgs/msg/pose.hpp" +#include "atos_interfaces/srv/get_object_return_trajectory.hpp" #include "backtostart.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "rclcpp/rclcpp.hpp" +#include "testUtils/testUtils.hpp" +#include "gtest/gtest.h" class BackToStartTest : public ::testing::Test { public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - protected: - - std::shared_ptr backToStart; - std::shared_ptr helperNode; - rclcpp::executors::SingleThreadedExecutor executor; - - void SetUp() override - { - helperNode = rclcpp::Node::make_shared("BackToStartTestHelper_node"); - backToStart = std::make_shared(); - executor.add_node(helperNode); - executor.add_node(backToStart); - } - - void TearDown() override - { - executor.remove_node(helperNode); - executor.remove_node(backToStart); - helperNode.reset(); - backToStart.reset(); - } + static void SetUpTestCase() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr backToStart; + std::shared_ptr helperNode; + rclcpp::executors::SingleThreadedExecutor executor; + + void SetUp() override { + helperNode = rclcpp::Node::make_shared("BackToStartTestHelper_node"); + backToStart = std::make_shared(); + executor.add_node(helperNode); + executor.add_node(backToStart); + } + + void TearDown() override { + executor.remove_node(helperNode); + executor.remove_node(backToStart); + helperNode.reset(); + backToStart.reset(); + } }; - -TEST_F(BackToStartTest, testServiceReturnsFalseWhenCalledWithoutTrajectory){ - // Setup - std::promise serviceCalled; - std::shared_future serviceCalledFuture(serviceCalled.get_future()); - auto failAfterTimeout = std::chrono::milliseconds(5000); - - bool receivedResponse = false; - auto returnTrajResponseCallback = [&receivedResponse, &serviceCalled](const rclcpp::Client::SharedFuture future) { - receivedResponse = future.get()->success; - serviceCalled.set_value(); - }; - - std::string serviceName = ServiceNames::getObjectReturnTrajectory; - rclcpp::Client::SharedPtr returnTrajectoryClient; - returnTrajectoryClient = helperNode->create_client(serviceName); - - auto returnTrajectoryRequest = std::make_shared(); - testUtils::waitForService(returnTrajectoryClient, std::chrono::milliseconds(1000)); - - // Act - returnTrajectoryClient->async_send_request(returnTrajectoryRequest, returnTrajResponseCallback); - // wait for service to respond and callback to execute - testUtils::waitForFuture(executor, serviceCalledFuture, failAfterTimeout); - - // Assert - ASSERT_EQ(receivedResponse, false); +TEST_F(BackToStartTest, testServiceReturnsFalseWhenCalledWithoutTrajectory) { + // Setup + std::promise serviceCalled; + std::shared_future serviceCalledFuture(serviceCalled.get_future()); + auto failAfterTimeout = std::chrono::milliseconds(5000); + + bool receivedResponse = false; + auto returnTrajResponseCallback = + [&receivedResponse, + &serviceCalled](const rclcpp::Client::SharedFuture future) { + receivedResponse = future.get()->success; + serviceCalled.set_value(); + }; + + std::string serviceName = ServiceNames::getObjectReturnTrajectory; + rclcpp::Client::SharedPtr returnTrajectoryClient; + returnTrajectoryClient = helperNode->create_client(serviceName); + + auto returnTrajectoryRequest = std::make_shared(); + testUtils::waitForService(returnTrajectoryClient, std::chrono::milliseconds(1000)); + + // Act + returnTrajectoryClient->async_send_request(returnTrajectoryRequest, returnTrajResponseCallback); + // wait for service to respond and callback to execute + testUtils::waitForFuture(executor, serviceCalledFuture, failAfterTimeout); + + // Assert + ASSERT_EQ(receivedResponse, false); } -TEST_F(BackToStartTest, testServiceReturnsTrajWithEndPointAtInputTrajStartPoint){ - // Setup - std::promise serviceCalled; - std::shared_future serviceCalledFuture(serviceCalled.get_future()); - auto failAfterTimeout = std::chrono::milliseconds(5000); - - bool receivedResponse = false; - geometry_msgs::msg::Pose returnTrajEndPoint; - auto returnTrajResponseCallback = [&receivedResponse, &returnTrajEndPoint, &serviceCalled](const rclcpp::Client::SharedFuture future) { - receivedResponse = future.get()->success; - returnTrajEndPoint = future.get()->trajectory.points.back().pose; - serviceCalled.set_value(); - }; - - std::string serviceName = ServiceNames::getObjectReturnTrajectory; - rclcpp::Client::SharedPtr returnTrajectoryClient; - returnTrajectoryClient = helperNode->create_client(serviceName); - - auto returnTrajectoryRequest = std::make_shared(); - for (int i = 0; i < 10; i++) { - atos_interfaces::msg::CartesianTrajectoryPoint trajPoint; - trajPoint.time_from_start.sec = i; - trajPoint.time_from_start.nanosec = i; - trajPoint.pose.position.x = i; - trajPoint.pose.position.y = i; - trajPoint.pose.position.z = i; - trajPoint.twist.linear.x = i; - trajPoint.twist.linear.y = 0; - trajPoint.twist.linear.z = 0; - returnTrajectoryRequest->trajectory.points.push_back(trajPoint); - } - auto trajStartPoint = returnTrajectoryRequest->trajectory.points[0].pose; - testUtils::waitForService(returnTrajectoryClient, std::chrono::milliseconds(1000)); - - // Act - returnTrajectoryClient->async_send_request(returnTrajectoryRequest, returnTrajResponseCallback); - // wait for service to respond and callback to execute - testUtils::waitForFuture(executor, serviceCalledFuture, failAfterTimeout); - - // Assert - ASSERT_EQ(receivedResponse, true); - ASSERT_EQ(returnTrajEndPoint.position.x, trajStartPoint.position.x); - ASSERT_EQ(returnTrajEndPoint.position.y, trajStartPoint.position.y); - ASSERT_EQ(returnTrajEndPoint.position.z, trajStartPoint.position.z); - ASSERT_EQ(returnTrajEndPoint.orientation.x, trajStartPoint.orientation.x); - ASSERT_EQ(returnTrajEndPoint.orientation.y, trajStartPoint.orientation.y); - ASSERT_EQ(returnTrajEndPoint.orientation.z, trajStartPoint.orientation.z); - ASSERT_EQ(returnTrajEndPoint.orientation.w, trajStartPoint.orientation.w); -} \ No newline at end of file +TEST_F(BackToStartTest, testServiceReturnsTrajWithEndPointAtInputTrajStartPoint) { + // Setup + std::promise serviceCalled; + std::shared_future serviceCalledFuture(serviceCalled.get_future()); + auto failAfterTimeout = std::chrono::milliseconds(5000); + + bool receivedResponse = false; + geometry_msgs::msg::Pose returnTrajEndPoint; + auto returnTrajResponseCallback = + [&receivedResponse, &returnTrajEndPoint, &serviceCalled]( + const rclcpp::Client::SharedFuture future) { + receivedResponse = future.get()->success; + returnTrajEndPoint = future.get()->trajectory.points.back().pose; + serviceCalled.set_value(); + }; + + std::string serviceName = ServiceNames::getObjectReturnTrajectory; + rclcpp::Client::SharedPtr returnTrajectoryClient; + returnTrajectoryClient = helperNode->create_client(serviceName); + + auto returnTrajectoryRequest = std::make_shared(); + for (int i = 0; i < 10; i++) { + atos_interfaces::msg::CartesianTrajectoryPoint trajPoint; + trajPoint.time_from_start.sec = i; + trajPoint.time_from_start.nanosec = i; + trajPoint.pose.position.x = i; + trajPoint.pose.position.y = i; + trajPoint.pose.position.z = i; + trajPoint.twist.linear.x = i; + trajPoint.twist.linear.y = 0; + trajPoint.twist.linear.z = 0; + returnTrajectoryRequest->trajectory.points.push_back(trajPoint); + } + auto trajStartPoint = returnTrajectoryRequest->trajectory.points[0].pose; + testUtils::waitForService(returnTrajectoryClient, std::chrono::milliseconds(1000)); + + // Act + returnTrajectoryClient->async_send_request(returnTrajectoryRequest, returnTrajResponseCallback); + // wait for service to respond and callback to execute + testUtils::waitForFuture(executor, serviceCalledFuture, failAfterTimeout); + + // Assert + ASSERT_EQ(receivedResponse, true); + ASSERT_EQ(returnTrajEndPoint.position.x, trajStartPoint.position.x); + ASSERT_EQ(returnTrajEndPoint.position.y, trajStartPoint.position.y); + ASSERT_EQ(returnTrajEndPoint.position.z, trajStartPoint.position.z); + ASSERT_EQ(returnTrajEndPoint.orientation.x, trajStartPoint.orientation.x); + ASSERT_EQ(returnTrajEndPoint.orientation.y, trajStartPoint.orientation.y); + ASSERT_EQ(returnTrajEndPoint.orientation.z, trajStartPoint.orientation.z); + ASSERT_EQ(returnTrajEndPoint.orientation.w, trajStartPoint.orientation.w); +} diff --git a/atos/modules/DirectControl/inc/directcontrol.hpp b/atos/modules/DirectControl/inc/directcontrol.hpp index b6e13cb0a..5f5b4cd08 100644 --- a/atos/modules/DirectControl/inc/directcontrol.hpp +++ b/atos/modules/DirectControl/inc/directcontrol.hpp @@ -5,11 +5,11 @@ */ #pragma once -#include +#include "atos_interfaces/msg/control_signal_percentage.hpp" #include "module.hpp" -#include "tcphandler.hpp" #include "server.hpp" -#include "atos_interfaces/msg/control_signal_percentage.hpp" +#include "tcphandler.hpp" +#include class DirectControl : public Module { public: diff --git a/atos/modules/DirectControl/src/directcontrol.cpp b/atos/modules/DirectControl/src/directcontrol.cpp index ea625ab6c..388955162 100644 --- a/atos/modules/DirectControl/src/directcontrol.cpp +++ b/atos/modules/DirectControl/src/directcontrol.cpp @@ -3,12 +3,12 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include #include "directcontrol.hpp" -#include "util.h" -#include "tcphandler.hpp" #include "atosTime.h" #include "atos_interfaces/msg/control_signal_percentage.hpp" +#include "tcphandler.hpp" +#include "util.h" +#include using atos_interfaces::msg::ControlSignalPercentage; using namespace ROSChannels; @@ -19,39 +19,41 @@ using namespace ROSChannels; * @brief Driver Model message, used in esmini to send * control signals to a vehicle from a driver model. */ -class DmMsg{ +class DmMsg { public: - DmMsg(){}; + DmMsg() {}; - ControlSignalPercentage toATOSMsg(){ + ControlSignalPercentage toATOSMsg() { ControlSignalPercentage cspmsg = ControlSignalPercentage(); - cspmsg.atos_header.object_id = this->objectId; - // Convert throttle brake and steering angle to integers between 0,100 and -100,100 respectively. - cspmsg.throttle = round(this->throttle * 100); - cspmsg.brake = round(this->brake * 100); + cspmsg.atos_header.object_id = this->objectId; + // Convert throttle brake and steering angle to integers between 0,100 and -100,100 respectively. + cspmsg.throttle = round(this->throttle * 100); + cspmsg.brake = round(this->brake * 100); cspmsg.steering_angle = round(this->steeringAngle * 100); return cspmsg; } /*! - * \brief Decodes a DM message from esmini compatible sender. - * if the message is not of type inputMode=1 (DRIVER_INPUT) - * the function returns - * - * \param bytes vector of (char) bytes - * \param DMMsg the data structure to be populated with contents of the message - * \return number of bytes on successfully parsed message, -1 otherwise - */ - int parseFromBytes(const std::vector& bytes){ - int idx=0; - decodeNextXBytes(4,this->version,idx,bytes); - decodeNextXBytes(4,this->inputMode,idx,bytes); - if (this->inputMode != 1) { return 0;} - decodeNextXBytes(4,this->objectId,idx,bytes); - decodeNextXBytes(4,this->frameNumber,idx,bytes); - decodeNextXBytes(8,this->throttle,idx,bytes); - decodeNextXBytes(8,this->brake,idx,bytes); - decodeNextXBytes(8,this->steeringAngle,idx,bytes); + * \brief Decodes a DM message from esmini compatible sender. + * if the message is not of type inputMode=1 (DRIVER_INPUT) + * the function returns + * + * \param bytes vector of (char) bytes + * \param DMMsg the data structure to be populated with contents of the message + * \return number of bytes on successfully parsed message, -1 otherwise + */ + int parseFromBytes(const std::vector& bytes) { + int idx = 0; + decodeNextXBytes(4, this->version, idx, bytes); + decodeNextXBytes(4, this->inputMode, idx, bytes); + if (this->inputMode != 1) { + return 0; + } + decodeNextXBytes(4, this->objectId, idx, bytes); + decodeNextXBytes(4, this->frameNumber, idx, bytes); + decodeNextXBytes(8, this->throttle, idx, bytes); + decodeNextXBytes(8, this->brake, idx, bytes); + decodeNextXBytes(8, this->steeringAngle, idx, bytes); return idx; } @@ -60,34 +62,31 @@ class DmMsg{ unsigned int inputMode; unsigned int objectId; unsigned int frameNumber; - double throttle; // range [0, 1] - double brake; // range [0, 1] - double steeringAngle; // range [-pi/2, pi/2] + double throttle; // range [0, 1] + double brake; // range [0, 1] + double steeringAngle; // range [-pi/2, pi/2] /*! - * \brief transfers x bytes, on the interval from idx to idx+x, - * from a vector of bytes into a variable - * - * \param x number of bytes to transfer - * \param field variable to receive bytes - * \param idx starting byte - * \param bytes vector of (char) bytes - */ + * \brief transfers x bytes, on the interval from idx to idx+x, + * from a vector of bytes into a variable + * + * \param x number of bytes to transfer + * \param field variable to receive bytes + * \param idx starting byte + * \param bytes vector of (char) bytes + */ template - void decodeNextXBytes(int x, T& field, int& idx, const std::vector& bytes){ + void decodeNextXBytes(int x, T& field, int& idx, const std::vector& bytes) { std::memcpy(&field, &(bytes[idx]), sizeof(T)); - if (sizeof(T) == 2){ + if (sizeof(T) == 2) { le16toh(field); - } - else if (sizeof(T) == 4){ + } else if (sizeof(T) == 4) { le32toh(field); - } - else if (sizeof(T) == 8){ + } else if (sizeof(T) == 8) { le64toh(field); - } - idx+=x; + } + idx += x; } - }; //! Message queue callbacks @@ -99,86 +98,84 @@ void DirectControl::onAllClearMessage(const AllClear::message_type::SharedPtr) { //! Class methods DirectControl::DirectControl() : - Module(moduleName), - controlSignalPub(*this), - tcpHandler(TCPPort, "", "off", 1, O_NONBLOCK), - udpServer("0.0.0.0",UDPPort) {} + Module(moduleName), + controlSignalPub(*this), + tcpHandler(TCPPort, "", "off", 1, O_NONBLOCK), + udpServer("0.0.0.0", UDPPort) {} void DirectControl::startThreads() { - receiveThread=std::make_unique(&DirectControl::readTCPSocketData, this); - receiveThreadUDP=std::make_unique(&DirectControl::readUDPSocketData, this); + receiveThread = std::make_unique(&DirectControl::readTCPSocketData, this); + receiveThreadUDP = std::make_unique(&DirectControl::readUDPSocketData, this); } -void DirectControl::joinThreads(){ - //Tear down connections - if(this->tcpHandler.isConnected()) this->tcpHandler.TCPHandlerclose(); +void DirectControl::joinThreads() { + // Tear down connections + if (this->tcpHandler.isConnected()) + this->tcpHandler.TCPHandlerclose(); this->udpServer.close(); - //Join threads + // Join threads receiveThread->join(); receiveThreadUDP->join(); } - /*! * \brief Listens for UDP data and sends a control signal on ros topic when recevied */ void DirectControl::readUDPSocketData() { - RCLCPP_INFO(get_logger(),"Listening on UDP port %d",UDPPort); - - while (!this->quit){ - try{ + RCLCPP_INFO(get_logger(), "Listening on UDP port %d", UDPPort); + + while (!this->quit) { + try { auto [data, remote] = udpServer.recvfrom(); DmMsg dmMsg; auto bytesParsed = dmMsg.parseFromBytes(data); - // TODO: add re-ordering with frame numbers? - // Have to think abt if/what kind of re-ordering makes sense for + // TODO: add re-ordering with frame numbers? + // Have to think abt if/what kind of re-ordering makes sense for // the application (driver model). - if (bytesParsed != -1){ + if (bytesParsed != -1) { ControlSignalPercentage cspmsg = dmMsg.toATOSMsg(); controlSignalPub.publish(cspmsg); } - } - catch(const SocketErrors::DisconnectedError& error){ - //If we get a disconnected exception when we do not intend to exit, propagate said exception - if (!this->quit){ + } catch (const SocketErrors::DisconnectedError& error) { + // If we get a disconnected exception when we do not intend to exit, propagate said exception + if (!this->quit) { throw error; } } } } - void DirectControl::readTCPSocketData() { std::vector data(TCP_BUFFER_SIZE); int recvData = 0; - RCLCPP_INFO(get_logger(),"Awaiting TCP connection..."); + RCLCPP_INFO(get_logger(), "Awaiting TCP connection..."); this->tcpHandler.CreateServer(); while (!this->quit) { // TODO set up TCP connection (wait until connected before continue) this->tcpHandler.TCPHandlerAccept(1000); - if (this->tcpHandler.isConnected()){ - RCLCPP_INFO(get_logger(),"Connected"); - + if (this->tcpHandler.isConnected()) { + RCLCPP_INFO(get_logger(), "Connected"); + while (!this->quit && tcpHandler.isConnected()) { data.resize(TCP_BUFFER_SIZE); std::fill(data.begin(), data.end(), 0); recvData = tcpHandler.receiveTCP(data, 0); if (recvData == TCPHandler::FAILURE) { this->tcpHandler.TCPHandlerclose(); - RCLCPP_INFO(get_logger(),"TCP connection closed unexpectedly..."); - RCLCPP_INFO(get_logger(),"Awaiting new TCP connection...");; + RCLCPP_INFO(get_logger(), "TCP connection closed unexpectedly..."); + RCLCPP_INFO(get_logger(), "Awaiting new TCP connection..."); + ; this->tcpHandler.CreateServer(); break; - } - else if (recvData > 0) { + } else if (recvData > 0) { try { this->handleISOMessage(data, static_cast(recvData)); } catch (std::invalid_argument& e) { - RCLCPP_ERROR(get_logger(),e.what()); + RCLCPP_ERROR(get_logger(), e.what()); std::fill(data.begin(), data.end(), 0); } } @@ -187,45 +184,40 @@ void DirectControl::readTCPSocketData() { } } -void DirectControl::handleISOMessage( - std::vector& byteData, - size_t receivedBytes) { +void DirectControl::handleISOMessage(std::vector& byteData, size_t receivedBytes) { while (receivedBytes > 0) { ISOMessageID recvMessage = getISOMessageType(byteData.data(), byteData.size(), false); // TODO check for RDCI (optional) // TODO if RDCI, respond with DCTI (optional) switch (recvMessage) { - case MESSAGE_ID_INVALID: - throw std::invalid_argument("Received invalid ISO message"); - case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_RDCA: - receivedBytes -= this->handleRDCAMessage(byteData); - break; - default: - throw std::invalid_argument("Received unhandled ISO message"); + case MESSAGE_ID_INVALID: + throw std::invalid_argument("Received invalid ISO message"); + case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_RDCA: + receivedBytes -= this->handleRDCAMessage(byteData); + break; + default: + throw std::invalid_argument("Received unhandled ISO message"); } } } -size_t DirectControl::handleRDCAMessage( - std::vector &byteData) { +size_t DirectControl::handleRDCAMessage(std::vector& byteData) { RequestControlActionType recvMessage; struct timeval currentTime; TimeSetToCurrentSystemTime(¤tTime); ssize_t bytesRead = decodeRDCAMessage(byteData.data(), &recvMessage, byteData.size(), currentTime, false); if (bytesRead >= 0) { byteData.erase(byteData.begin(), byteData.begin() + bytesRead); - //TODO: Implement equivalent of below line in using ROS2. - //DataDictionarySetRequestedControlAction(recvMessage.executingID, &recvMessage); + // TODO: Implement equivalent of below line in using ROS2. + // DataDictionarySetRequestedControlAction(recvMessage.executingID, &recvMessage); return static_cast(bytesRead); - } - else { + } else { // TODO respond with error (optional) throw std::invalid_argument("Failed to decode RDCA message"); } } -size_t DirectControl::handleUnknownMessage( - std::vector &byteData) { +size_t DirectControl::handleUnknownMessage(std::vector& byteData) { std::fill(byteData.begin(), byteData.end(), 0); return byteData.size(); } diff --git a/atos/modules/DirectControl/src/main.cpp b/atos/modules/DirectControl/src/main.cpp index d04f96537..f95722ba8 100644 --- a/atos/modules/DirectControl/src/main.cpp +++ b/atos/modules/DirectControl/src/main.cpp @@ -4,18 +4,18 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include -#include #include +#include -#include "util.h" #include "directcontrol.hpp" +#include "util.h" using namespace std::chrono; static std::shared_ptr dc; int main(int argc, char** argv) { - rclcpp::init(argc,argv); + rclcpp::init(argc, argv); dc = std::make_shared(); dc->initializeModule(); dc->startThreads(); diff --git a/atos/modules/EsminiAdapter/inc/esminiadapter.hpp b/atos/modules/EsminiAdapter/inc/esminiadapter.hpp index f0d90474b..eea1d986d 100644 --- a/atos/modules/EsminiAdapter/inc/esminiadapter.hpp +++ b/atos/modules/EsminiAdapter/inc/esminiadapter.hpp @@ -34,30 +34,31 @@ class EsminiAdapter : public Module { static inline std::string const moduleName = "esmini_adapter"; static inline std::filesystem::path oscFilePath; static int initializeModule(); - EsminiAdapter(EsminiAdapter const &) = delete; - EsminiAdapter &operator=(EsminiAdapter const &) = delete; + EsminiAdapter(EsminiAdapter const&) = delete; + EsminiAdapter& operator=(EsminiAdapter const&) = delete; static std::shared_ptr instance(); private: EsminiAdapter(); - ROSChannels::StoryBoardElementStateChange::Pub - storyBoardElementStateChangePub; + ROSChannels::StoryBoardElementStateChange::Pub storyBoardElementStateChangePub; ROSChannels::ConnectedObjectIds::Sub connectedObjectIdsSub; ROSChannels::Exit::Sub exitSub; ROSChannels::StateChange::Sub stateChangeSub; std::unordered_map pathPublishers; std::unordered_map gnssPathPublishers; - static std::unordered_map> monrSubscribers; + static std::unordered_map> monrSubscribers; static std::shared_ptr> objectTrajectoryService; static std::shared_ptr> testOriginService; rclcpp::CallbackGroup::SharedPtr oscFilePathClient_cb_group_; rclcpp::CallbackGroup::SharedPtr objectIdsClient_cb_group_; - rclcpp::Client::SharedPtr oscFilePathClient_; //!< Client to request the current open scenario file path - rclcpp::Client::SharedPtr objectIdsClient_; //!< Client to request the ATOS object id for each openx entity name + rclcpp::Client::SharedPtr + oscFilePathClient_; //!< Client to request the current open scenario file path + rclcpp::Client::SharedPtr + objectIdsClient_; //!< Client to request the ATOS object id for each openx entity name void onMonitorMessage(const ROSChannels::Monitor::message_type::SharedPtr monr, uint32_t id); // Below is a quickfix, fix properly later @@ -73,19 +74,16 @@ class EsminiAdapter : public Module { static std::filesystem::path getOpenDriveFile(); static void handleStoryBoardElementChange(const char* name, int type, int state, const char* full_path); static void runEsminiSimulation(); - static void getObjectStates(double timeStep, std::map>& states); - static ATOS::Trajectory getTrajectoryFromObjectState(uint32_t,std::vector& states); + static void getObjectStates(double timeStep, std::map>& states); + static ATOS::Trajectory getTrajectoryFromObjectState(uint32_t, std::vector& states); static std::string projStrFromGeoReference(RM_GeoReference& geoRef); static std::map extractTrajectories(double timeStep); - static void onRequestObjectTrajectory( - const std::shared_ptr - req, - std::shared_ptr res); + static void onRequestObjectTrajectory(const std::shared_ptr req, + std::shared_ptr res); - static void onRequestTestOrigin( - const std::shared_ptr, - std::shared_ptr); + static void onRequestTestOrigin(const std::shared_ptr, + std::shared_ptr); static std::unordered_map atosIDToObjectName; static std::unordered_map objectNameToAtosId; diff --git a/atos/modules/EsminiAdapter/inc/string_utility.hpp b/atos/modules/EsminiAdapter/inc/string_utility.hpp index 75d475665..f1be98a1b 100644 --- a/atos/modules/EsminiAdapter/inc/string_utility.hpp +++ b/atos/modules/EsminiAdapter/inc/string_utility.hpp @@ -4,15 +4,15 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #pragma once +#include #include #include -#include // Function to split a string into a vector of strings by a delimiter -void split (const std::string &s, char delim, std::vector &elems) { - std::stringstream ss(s); - std::string item; - while (std::getline(ss, item, delim)) { - elems.push_back(item); - } -} \ No newline at end of file +void split(const std::string& s, char delim, std::vector& elems) { + std::stringstream ss(s); + std::string item; + while (std::getline(ss, item, delim)) { + elems.push_back(item); + } +} diff --git a/atos/modules/EsminiAdapter/src/esminiadapter.cpp b/atos/modules/EsminiAdapter/src/esminiadapter.cpp index 6b4da4917..cfa37f09d 100644 --- a/atos/modules/EsminiAdapter/src/esminiadapter.cpp +++ b/atos/modules/EsminiAdapter/src/esminiadapter.cpp @@ -4,90 +4,84 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "esminiadapter.hpp" -#include -#include #include -#include #include #include -#include +#include #include +#include +#include #include "atos_interfaces/msg/cartesian_trajectory.hpp" #include "rclcpp/wait_for_message.hpp" -#include "trajectory.hpp" #include "string_utility.hpp" - +#include "trajectory.hpp" using namespace ROSChannels; -using TestOriginSrv = atos_interfaces::srv::GetTestOrigin; +using TestOriginSrv = atos_interfaces::srv::GetTestOrigin; using ObjectTrajectorySrv = atos_interfaces::srv::GetObjectTrajectory; -using ObjectTriggerSrv = atos_interfaces::srv::GetObjectTriggerStart; +using ObjectTriggerSrv = atos_interfaces::srv::GetObjectTriggerStart; using std::placeholders::_1; using std::placeholders::_2; using namespace std::chrono_literals; - - - -std::shared_ptr EsminiAdapter::me = nullptr; +std::shared_ptr EsminiAdapter::me = nullptr; std::unordered_map EsminiAdapter::atosIDToObjectName = std::unordered_map(); std::unordered_map EsminiAdapter::objectNameToAtosId = std::unordered_map(); -std::map EsminiAdapter::atosObjectIdToTraj = std::map(); -std::unordered_map EsminiAdapter::atosIdToEsminiId = std::unordered_map(); - -std::unordered_map> EsminiAdapter::monrSubscribers = std::unordered_map>(); -std::shared_ptr> EsminiAdapter::objectTrajectoryService = std::shared_ptr>(); -std::shared_ptr> EsminiAdapter::testOriginService = std::shared_ptr>(); +std::map EsminiAdapter::atosObjectIdToTraj = std::map(); +std::unordered_map EsminiAdapter::atosIdToEsminiId = std::unordered_map(); + +std::unordered_map> EsminiAdapter::monrSubscribers = + std::unordered_map>(); +std::shared_ptr> EsminiAdapter::objectTrajectoryService = + std::shared_ptr>(); +std::shared_ptr> EsminiAdapter::testOriginService = + std::shared_ptr>(); geographic_msgs::msg::GeoPose EsminiAdapter::testOrigin = geographic_msgs::msg::GeoPose(); -EsminiAdapter::EsminiAdapter() : Module(moduleName), - storyBoardElementStateChangePub(*this), - connectedObjectIdsSub(*this, &EsminiAdapter::onConnectedObjectIdsMessage), - exitSub(*this, &EsminiAdapter::onStaticExitMessage), - stateChangeSub(*this, &EsminiAdapter::onStaticStateChangeMessage), - applyTrajTransform(false), - testOriginSet(false) - { +EsminiAdapter::EsminiAdapter() : + Module(moduleName), + storyBoardElementStateChangePub(*this), + connectedObjectIdsSub(*this, &EsminiAdapter::onConnectedObjectIdsMessage), + exitSub(*this, &EsminiAdapter::onStaticExitMessage), + stateChangeSub(*this, &EsminiAdapter::onStaticStateChangeMessage), + applyTrajTransform(false), + testOriginSet(false) { oscFilePathClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - objectIdsClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - oscFilePathClient_ = create_client(ServiceNames::getOpenScenarioFilePath, rmw_qos_profile_services_default, oscFilePathClient_cb_group_); - objectIdsClient_ = create_client(ServiceNames::getObjectIds, rmw_qos_profile_services_default, objectIdsClient_cb_group_); + objectIdsClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + oscFilePathClient_ = create_client( + ServiceNames::getOpenScenarioFilePath, rmw_qos_profile_services_default, oscFilePathClient_cb_group_); + objectIdsClient_ = create_client( + ServiceNames::getObjectIds, rmw_qos_profile_services_default, objectIdsClient_cb_group_); declare_parameter("timestep", 0.1); - } /*! * \brief Fetches the open drive file path from the open scenario file parameter * \return Configured path -*/ -std::filesystem::path EsminiAdapter::getOpenDriveFile() -{ + */ +std::filesystem::path EsminiAdapter::getOpenDriveFile() { std::filesystem::path odrFilePath; if (SE_GetODRFilename() != nullptr) { odrFilePath = std::filesystem::path(SE_GetODRFilename()); RCLCPP_INFO(me->get_logger(), "Got ODR file %s from scenario", odrFilePath.string().c_str()); - } - else { + } else { RCLCPP_DEBUG(me->get_logger(), "No ODR file found"); } if (odrFilePath.is_absolute()) { return odrFilePath; - } - else { + } else { // Look for the file relative the scenario file (ie root openx dir) - auto odrCatalog = std::filesystem::path(me->oscFilePath).parent_path(); + auto odrCatalog = std::filesystem::path(me->oscFilePath).parent_path(); std::filesystem::path joinedPath = odrCatalog / odrFilePath; if (!std::filesystem::exists(joinedPath)) { throw std::runtime_error("ODR file " + joinedPath.string() + " does not exist"); - } - else { + } else { RCLCPP_INFO(me->get_logger(), "Found ODR file at %s", joinedPath.string().c_str()); } return joinedPath; } - } /*! @@ -98,22 +92,24 @@ std::shared_ptr EsminiAdapter::instance() { if (me == nullptr) { me = std::shared_ptr(new EsminiAdapter()); // Start listening to connected object ids - me->connectedObjectIdsSub = ROSChannels::ConnectedObjectIds::Sub(*me,&EsminiAdapter::onConnectedObjectIdsMessage); - me->exitSub = ROSChannels::Exit::Sub(*me,&EsminiAdapter::onStaticExitMessage); - me->stateChangeSub = ROSChannels::StateChange::Sub(*me,&EsminiAdapter::onStaticStateChangeMessage); - + me->connectedObjectIdsSub = + ROSChannels::ConnectedObjectIds::Sub(*me, &EsminiAdapter::onConnectedObjectIdsMessage); + me->exitSub = ROSChannels::Exit::Sub(*me, &EsminiAdapter::onStaticExitMessage); + me->stateChangeSub = ROSChannels::StateChange::Sub(*me, &EsminiAdapter::onStaticStateChangeMessage); } return me; } /*! - * \brief This callback is executed when objects are connected, and creates Monitor subscribers for all connected objects. + * \brief This callback is executed when objects are connected, and creates Monitor subscribers for all connected + * objects. * \param msg Array of object IDs */ void EsminiAdapter::onConnectedObjectIdsMessage(const ConnectedObjectIds::message_type::SharedPtr msg) { for (uint32_t id : msg->ids) { - if (me->monrSubscribers.find(id) == me->monrSubscribers.end()){ - me->monrSubscribers[id] = std::make_shared(*me, id, std::bind(&EsminiAdapter::onMonitorMessage, me, _1, id)); + if (me->monrSubscribers.find(id) == me->monrSubscribers.end()) { + me->monrSubscribers[id] = + std::make_shared(*me, id, std::bind(&EsminiAdapter::onMonitorMessage, me, _1, id)); } } } @@ -126,56 +122,50 @@ void EsminiAdapter::onConnectedObjectIdsMessage(const ConnectedObjectIds::messag * @param msg StateChange message */ void EsminiAdapter::onStaticStateChangeMessage(const ROSChannels::StateChange::message_type::SharedPtr msg) { - int prevState = msg->prev_state; + int prevState = msg->prev_state; int currentState = msg->current_state; if (prevState == OBCState_t::OBC_STATE_ARMED && currentState == OBCState_t::OBC_STATE_RUNNING) { me->handleStartCommand(); - } - else if (currentState == OBCState_t::OBC_STATE_ABORTING) { + } else if (currentState == OBCState_t::OBC_STATE_ABORTING) { me->handleAbortCommand(); } } - void EsminiAdapter::handleAbortCommand() { SE_Close(); RCLCPP_INFO(me->get_logger(), "Esmini ScenarioEngine stopped due to Abort"); } -void EsminiAdapter::fetchOSCFilePath() -{ +void EsminiAdapter::fetchOSCFilePath() { // Get the file path of xosc file std::promise done; auto callback = [&](rclcpp::Client::SharedFuture future) { - auto response = future.get(); + auto response = future.get(); me->oscFilePath = response->path; if (response->md5hash != me->scenarioFileMd5hash) { me->scenarioFileMd5hash = response->md5hash; - me->runSimulation = true; + me->runSimulation = true; } me->scenarioFileMd5hash = response->md5hash; done.set_value(true); }; auto request = std::make_shared(); - auto future = me->oscFilePathClient_->async_send_request(request, std::move(callback)); + auto future = me->oscFilePathClient_->async_send_request(request, std::move(callback)); if (done.get_future().wait_for(250ms) == std::future_status::timeout) { RCLCPP_ERROR(me->get_logger(), "Failed to fetch open scenario file path"); } - } -void EsminiAdapter::onStaticExitMessage(const ROSChannels::Exit::message_type::SharedPtr) -{ +void EsminiAdapter::onStaticExitMessage(const ROSChannels::Exit::message_type::SharedPtr) { SE_Close(); - RCLCPP_DEBUG(me->get_logger(),"Received exit command"); + RCLCPP_DEBUG(me->get_logger(), "Received exit command"); rclcpp::shutdown(); } -void EsminiAdapter::handleStartCommand() -{ - if (SE_Init(me->oscFilePath.c_str(),0,0,0,0) < 0) { +void EsminiAdapter::handleStartCommand() { + if (SE_Init(me->oscFilePath.c_str(), 0, 0, 0, 0) < 0) { throw std::runtime_error("Failed to initialize esmini with scenario file " + me->oscFilePath.string()); } // Handle triggers and story board element changes @@ -189,21 +179,22 @@ void EsminiAdapter::handleStartCommand() * \brief Callback to be executed by esmini when story board state changes. * If story board element is an action, and the action is supported, the action is run. * \param name Name of the StoryBoardElement whose state has changed. - * \param type Possible values: STORY_BOARD = 1, STORY = 2, ACT = 3, MANEUVER_GROUP = 4, MANEUVER = 5, EVENT = 6, ACTION = 7, UNDEFINED_ELEMENT_TYPE = 0. + * \param type Possible values: STORY_BOARD = 1, STORY = 2, ACT = 3, MANEUVER_GROUP = 4, MANEUVER = 5, EVENT = 6, ACTION + * = 7, UNDEFINED_ELEMENT_TYPE = 0. * \param state new state, possible values: STANDBY = 1, RUNNING = 2, COMPLETE = 3, UNDEFINED_ELEMENT_STATE = 0. */ -void EsminiAdapter::handleStoryBoardElementChange( - const char *name, - int type, - int state, - const char *full_path) -{ - RCLCPP_INFO(me->get_logger(), "Storyboard state changed! Name: %s, Type: %d, State: %d, Full path: %s", name, type, state, full_path); +void EsminiAdapter::handleStoryBoardElementChange(const char* name, int type, int state, const char* full_path) { + RCLCPP_INFO(me->get_logger(), + "Storyboard state changed! Name: %s, Type: %d, State: %d, Full path: %s", + name, + type, + state, + full_path); atos_interfaces::msg::StoryBoardElementStateChange msg; - msg.name = name; - msg.type = type; - msg.state = state; + msg.name = name; + msg.type = type; + msg.state = state; msg.full_path = full_path; me->storyBoardElementStateChangePub.publish(msg); @@ -215,9 +206,9 @@ void EsminiAdapter::handleStoryBoardElementChange( * \param monr ROS Monitor message of an object * \param id The object ID to which the monr belongs */ -void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr monr, uint32_t esminiObjectId){ +void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr monr, uint32_t esminiObjectId) { // Conversions from ROS to Esmini - auto ori = monr->pose.pose.orientation; + auto ori = monr->pose.pose.orientation; auto quat = tf2::Quaternion(ori.x, ori.y, ori.z, ori.w); tf2::Matrix3x3 m(quat); double roll, pitch, yaw; @@ -239,14 +230,16 @@ void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr * \brief Callback for MONR messages, reports the object position to esmini and advances the simulation time * \param monr ROS Monitor message of an object * \param id The object ID to which the monr belongs -*/ + */ void EsminiAdapter::onMonitorMessage(const Monitor::message_type::SharedPtr monr, uint32_t ATOSObjectId) { if (auto idMapping = atosIdToEsminiId.find(ATOSObjectId); idMapping != atosIdToEsminiId.end()) { reportObjectPosition(monr, idMapping->second); // Report object position to esmini - SE_Step(); // Advance the "simulation world"-time - } - else { - RCLCPP_WARN(me->get_logger(), "Received MONR message for object with ATOS Object ID %d, but no such object exists in the scenario", ATOSObjectId); + SE_Step(); // Advance the "simulation world"-time + } else { + RCLCPP_WARN( + me->get_logger(), + "Received MONR message for object with ATOS Object ID %d, but no such object exists in the scenario", + ATOSObjectId); } } @@ -258,10 +251,7 @@ void EsminiAdapter::onMonitorMessage(const Monitor::message_type::SharedPtr monr * \param states A vector of object states * \return A trajectory consisting of trajectory points, one for each state. */ -ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState( - uint32_t id, - std::vector& states) -{ +ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState(uint32_t id, std::vector& states) { ATOS::Trajectory trajectory(me->get_logger()); trajectory.name = "Esmini Trajectory for object " + std::to_string(id); if (states.empty()) { @@ -287,8 +277,7 @@ ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState( if (state.timestamp != prevState.timestamp) { tp.setLongitudinalAcceleration((currLonVel - prevLonVel) / (state.timestamp - prevState.timestamp)); tp.setLateralAcceleration((currLatVel - prevLatVel) / (state.timestamp - prevState.timestamp)); - } - else { + } else { tp.setLongitudinalAcceleration(0); tp.setLateralAcceleration(0); } @@ -297,12 +286,12 @@ ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState( }; if (states.size() > 1) { - for (auto it = states.begin()+1; it != states.end(); ++it) { - if (it->x == (it-1)->x && it->y == (it-1)->y && // Nothing interesting happens within 1 timestep, skip - it->z == (it-1)->z && it->h == (it-1)->h) { + for (auto it = states.begin() + 1; it != states.end(); ++it) { + if (it->x == (it - 1)->x && it->y == (it - 1)->y && // Nothing interesting happens within 1 timestep, skip + it->z == (it - 1)->z && it->h == (it - 1)->h) { continue; } - saveTp(*it,*(it-1)); // Next timestep is different, save current one. + saveTp(*it, *(it - 1)); // Next timestep is different, save current one. } } if (trajectory.points.size() == 0) { @@ -311,7 +300,7 @@ ATOS::Trajectory EsminiAdapter::getTrajectoryFromObjectState( auto startTime = trajectory.points.front().getTime(); // Subtract start time from all timesteps - for (auto& tp : trajectory.points){ + for (auto& tp : trajectory.points) { tp.setTime(tp.getTime() - startTime); } return trajectory; @@ -326,8 +315,7 @@ std::string EsminiAdapter::projStrFromGeoReference(RM_GeoReference& geoRef) { std::string projStringFrom = "+proj="; if (strlen(geoRef.proj_) != 0) { projStringFrom += std::string(geoRef.proj_) + " "; - } - else { + } else { throw std::runtime_error("No projection found in geo reference"); } if (!std::isnan(geoRef.lat_0_)) { @@ -397,14 +385,11 @@ std::string EsminiAdapter::projStrFromGeoReference(RM_GeoReference& geoRef) { * \param states The return map of ids mapping to the respective object states at different timesteps * \return A map of object states, where the key is the object ID and the value is a vector of states */ -void EsminiAdapter::getObjectStates( - double timeStep, - std::map>& states) -{ - double accumTime = 0.0; +void EsminiAdapter::getObjectStates(double timeStep, std::map>& states) { + double accumTime = 0.0; auto pushCurrentState = [&](auto& st, auto& index) { auto id = SE_GetId(index); - //SE_SetAlignModeZ(id, 0); // Disable Z-alignment, not implemented in esmini yet + // SE_SetAlignModeZ(id, 0); // Disable Z-alignment, not implemented in esmini yet SE_ScenarioObjectState s; SE_GetObjectState(id, &s); s.timestamp = accumTime; @@ -414,13 +399,13 @@ void EsminiAdapter::getObjectStates( // Populate States map with vector for each object containing the initial state SE_StepDT(timeStep); accumTime += timeStep; - for (int j = 0; j < SE_GetNumberOfObjects(); j++){ + for (int j = 0; j < SE_GetNumberOfObjects(); j++) { states[SE_GetId(j)] = std::vector(); pushCurrentState(states, j); } constexpr double MIN_SCENARIO_TIME = 10.0; constexpr double MAX_SCENARIO_TIME = 3600.0; - bool stopSimulation = false; + bool stopSimulation = false; while (!stopSimulation) { if (SE_GetQuitFlag() != 0) { break; @@ -428,51 +413,48 @@ void EsminiAdapter::getObjectStates( SE_StepDT(timeStep); accumTime += timeStep; - for (int j = 0; j < SE_GetNumberOfObjects(); j++){ + for (int j = 0; j < SE_GetNumberOfObjects(); j++) { pushCurrentState(states, j); } - bool noMovement = std::all_of(states.begin(), states.end(), [&](auto& pair) { - return pair.second.back().speed < 0.1; - }); - bool atLeastMinTimePassed = accumTime > MIN_SCENARIO_TIME; + bool noMovement = + std::all_of(states.begin(), states.end(), [&](auto& pair) { return pair.second.back().speed < 0.1; }); + bool atLeastMinTimePassed = accumTime > MIN_SCENARIO_TIME; bool moreThanMaxTimePassed = accumTime > MAX_SCENARIO_TIME; - stopSimulation = (noMovement && atLeastMinTimePassed) || moreThanMaxTimePassed; + stopSimulation = (noMovement && atLeastMinTimePassed) || moreThanMaxTimePassed; } if (accumTime > MAX_SCENARIO_TIME) { RCLCPP_WARN(me->get_logger(), "Scenario time limit reached, stopping simulation"); - } - else if (accumTime < MIN_SCENARIO_TIME + timeStep) { - RCLCPP_WARN(me->get_logger(), "Ran scenario for the minimum time %.2f, possibly no movement in scenario", MIN_SCENARIO_TIME); + } else if (accumTime < MIN_SCENARIO_TIME + timeStep) { + RCLCPP_WARN(me->get_logger(), + "Ran scenario for the minimum time %.2f, possibly no movement in scenario", + MIN_SCENARIO_TIME); } RCLCPP_INFO(me->get_logger(), "Finished %f s simulation", accumTime); } - /*! * \brief Runs the esmini simulator with the xosc file and returns the trajectories for each object * \param timeStep Time step to use for generating the trajectories * \return A map of ids mapping to the respective trajectories */ -std::map EsminiAdapter::extractTrajectories( - double timeStep) -{ - std::map esminiObjectIdToTraj = std::map(); +std::map EsminiAdapter::extractTrajectories(double timeStep) { + std::map esminiObjectIdToTraj = std::map(); // Get object states - std::map> esminiIdToStates; + std::map> esminiIdToStates; getObjectStates(timeStep, esminiIdToStates); // Extract trajectories for (auto& os : esminiIdToStates) { - auto id = os.first; + auto id = os.first; auto objectStates = os.second; - auto traj = getTrajectoryFromObjectState(id, objectStates); + auto traj = getTrajectoryFromObjectState(id, objectStates); // Apply CRS transform if OpenDrive CRS Transformation is defined - if (me->applyTrajTransform){ + if (me->applyTrajTransform) { RCLCPP_DEBUG(me->get_logger(), "Applying CRS transformation to trajectory for object %d", id); me->crsTransformation->apply(traj.points); } - esminiObjectIdToTraj.insert(std::pair(id, traj)); + esminiObjectIdToTraj.insert(std::pair(id, traj)); } return esminiObjectIdToTraj; } @@ -481,8 +463,7 @@ std::map EsminiAdapter::extractTrajectories( * \brief Initialize the esmini simulator and perform subsequent setup tasks. * Can be called many times, each time the test is initialized. */ -void EsminiAdapter::runEsminiSimulation() -{ +void EsminiAdapter::runEsminiSimulation() { me->atosObjectIdToTraj.clear(); me->atosIdToEsminiId.clear(); me->atosIDToObjectName.clear(); @@ -495,36 +476,35 @@ void EsminiAdapter::runEsminiSimulation() RM_Close(); // Stop RoadManager in case it is running RCLCPP_INFO(me->get_logger(), "Initializing esmini with scenario file %s", me->oscFilePath.c_str()); - if (SE_Init(me->oscFilePath.c_str(),1,0,0,0) < 0) { // Disable controllers, let DefaultController be used - throw std::runtime_error("Failed to initialize esmini with scenario file " + me->oscFilePath.string() + ". For more information, see " + logFilePath + "."); + if (SE_Init(me->oscFilePath.c_str(), 1, 0, 0, 0) < 0) { // Disable controllers, let DefaultController be used + throw std::runtime_error("Failed to initialize esmini with scenario file " + me->oscFilePath.string() + + ". For more information, see " + logFilePath + "."); } auto odrFile = getOpenDriveFile(); if (RM_Init(odrFile.c_str()) < 0) { throw std::runtime_error(std::string("Failed to initialize with odr file ").append(odrFile)); } - // Call RM_GetOpenDriveGeoReference to get the RM_GeoReference struct RM_GeoReference geoRef; if (RM_GetOpenDriveGeoReference(&geoRef) == 0) { try { std::string projStringFrom = projStrFromGeoReference(geoRef); - std::string toDatum = "WGS84"; - auto llh_0 = CRSTransformation::projToLLH(projStringFrom, toDatum); + std::string toDatum = "WGS84"; + auto llh_0 = CRSTransformation::projToLLH(projStringFrom, toDatum); RCLCPP_INFO(me->get_logger(), "llh origin: %lf, %lf, %lf", llh_0[0], llh_0[1], llh_0[2]); - me->testOrigin.position.latitude = llh_0[0]; + me->testOrigin.position.latitude = llh_0[0]; me->testOrigin.position.longitude = llh_0[1]; - me->testOrigin.position.altitude = llh_0[2]; - me->testOriginSet = true; + me->testOrigin.position.altitude = llh_0[2]; + me->testOriginSet = true; std::string projStringTo = "+proj=tmerc +lat_0=" + std::to_string(llh_0[0]) + - " +lon_0=" + std::to_string(llh_0[1]) + - " +datum="+ toDatum + " +units=m +no_defs"; + " +lon_0=" + std::to_string(llh_0[1]) + " +datum=" + toDatum + + " +units=m +no_defs"; - me->crsTransformation = std::make_shared(projStringFrom, projStringTo); + me->crsTransformation = std::make_shared(projStringFrom, projStringTo); me->applyTrajTransform = true; - } - catch (std::exception& e) { + } catch (std::exception& e) { RCLCPP_ERROR(me->get_logger(), e.what()); return; } @@ -534,17 +514,18 @@ void EsminiAdapter::runEsminiSimulation() RM_Close(); auto response = std::make_shared(); - auto request = std::make_shared(); + auto request = std::make_shared(); std::promise idsFetched; - auto objectNameAndAtosIDsCallback = [&](rclcpp::Client::SharedFutureWithRequest future) { - auto response = future.get(); - for (int i = 0; i < response.second->ids.size(); i++) { - me->atosIDToObjectName[response.second->ids[i]] = response.second->names[i]; - me->objectNameToAtosId[response.second->names[i]] = response.second->ids[i]; - } - idsFetched.set_value(); - }; + auto objectNameAndAtosIDsCallback = + [&](rclcpp::Client::SharedFutureWithRequest future) { + auto response = future.get(); + for (int i = 0; i < response.second->ids.size(); i++) { + me->atosIDToObjectName[response.second->ids[i]] = response.second->names[i]; + me->objectNameToAtosId[response.second->names[i]] = response.second->ids[i]; + } + idsFetched.set_value(); + }; auto future = me->objectIdsClient_->async_send_request(request, std::move(objectNameAndAtosIDsCallback)); if (idsFetched.get_future().wait_for(250ms) == std::future_status::timeout) { @@ -553,40 +534,45 @@ void EsminiAdapter::runEsminiSimulation() } RCLCPP_INFO(me->get_logger(), "Starting extracting trajectories"); - double timeStep = me->get_parameter("timestep").as_double(); - std::map esminiIdToTraj = me->extractTrajectories(timeStep); + double timeStep = me->get_parameter("timestep").as_double(); + std::map esminiIdToTraj = me->extractTrajectories(timeStep); // Fill atosIdToTraj with the extracted trajectories for (auto& it : esminiIdToTraj) { - auto esminiId = it.first; - auto traj = it.second; + auto esminiId = it.first; + auto traj = it.second; auto objectName = SE_GetObjectName(esminiId); - if (auto idMapping = me->objectNameToAtosId.find(objectName); idMapping != me->objectNameToAtosId.end()) { - auto atos_id = idMapping->second; - me->atosObjectIdToTraj.emplace(atos_id, traj); - me->atosIdToEsminiId.emplace(atos_id, esminiId); - RCLCPP_INFO(me->get_logger(), "Extracted trajectory for object %s with size %d", objectName, traj.points.size()); - - me->pathPublishers.emplace(atos_id, ROSChannels::Path::Pub(*me, atos_id)); - me->pathPublishers.at(atos_id).publish(traj.toPath()); - std::array llh_0 = {me->testOrigin.position.latitude, me->testOrigin.position.longitude, me->testOrigin.position.altitude}; - me->gnssPathPublishers.emplace(atos_id, ROSChannels::GNSSPath::Pub(*me, atos_id)); - me->gnssPathPublishers.at(atos_id).publish(traj.toGeoJSON(llh_0)); - } - else { - RCLCPP_DEBUG(me->get_logger(), "Object %s is not an active object in the scenario", objectName); - } + if (auto idMapping = me->objectNameToAtosId.find(objectName); idMapping != me->objectNameToAtosId.end()) { + auto atos_id = idMapping->second; + me->atosObjectIdToTraj.emplace(atos_id, traj); + me->atosIdToEsminiId.emplace(atos_id, esminiId); + RCLCPP_INFO( + me->get_logger(), "Extracted trajectory for object %s with size %d", objectName, traj.points.size()); + + me->pathPublishers.emplace(atos_id, ROSChannels::Path::Pub(*me, atos_id)); + me->pathPublishers.at(atos_id).publish(traj.toPath()); + std::array llh_0 = { + me->testOrigin.position.latitude, me->testOrigin.position.longitude, me->testOrigin.position.altitude}; + me->gnssPathPublishers.emplace(atos_id, ROSChannels::GNSSPath::Pub(*me, atos_id)); + me->gnssPathPublishers.at(atos_id).publish(traj.toGeoJSON(llh_0)); + } else { + RCLCPP_DEBUG(me->get_logger(), "Object %s is not an active object in the scenario", objectName); + } - for (auto& tp : traj.points){ - RCLCPP_DEBUG(me->get_logger(), "Trajectory point: %lf, %lf, %lf, %lf, %ld", tp.getXCoord(), tp.getYCoord(), tp.getZCoord(), tp.getHeading(), tp.getTime().count()); + for (auto& tp : traj.points) { + RCLCPP_DEBUG(me->get_logger(), + "Trajectory point: %lf, %lf, %lf, %lf, %ld", + tp.getXCoord(), + tp.getYCoord(), + tp.getZCoord(), + tp.getHeading(), + tp.getTime().count()); } } SE_Close(); // Stop ScenarioEngine } -void EsminiAdapter::onRequestObjectTrajectory( - const std::shared_ptr req, - std::shared_ptr res) -{ +void EsminiAdapter::onRequestObjectTrajectory(const std::shared_ptr req, + std::shared_ptr res) { res->id; me->fetchOSCFilePath(); if (me->runSimulation || me->atosObjectIdToTraj.find(req->id) == me->atosObjectIdToTraj.end()) { @@ -596,25 +582,22 @@ void EsminiAdapter::onRequestObjectTrajectory( try { res->trajectory = me->atosObjectIdToTraj.at(req->id).toCartesianTrajectory(); - res->success = true; - } - catch (std::out_of_range& e){ + res->success = true; + } catch (std::out_of_range& e) { RCLCPP_ERROR(me->get_logger(), "Esmini trajectory service called, no trajectory found for object %d", req->id); res->success = false; } } -void EsminiAdapter::onRequestTestOrigin( - const std::shared_ptr, - std::shared_ptr res) -{ - while (me->testOriginSet == false){ +void EsminiAdapter::onRequestTestOrigin(const std::shared_ptr, + std::shared_ptr res) { + while (me->testOriginSet == false) { RCLCPP_DEBUG(me->get_logger(), "Waiting for test origin to be available"); std::this_thread::sleep_for(std::chrono::milliseconds(20)); } - res->origin.position.latitude = me->testOrigin.position.latitude; + res->origin.position.latitude = me->testOrigin.position.latitude; res->origin.position.longitude = me->testOrigin.position.longitude; - res->origin.position.altitude = me->testOrigin.position.altitude; + res->origin.position.altitude = me->testOrigin.position.altitude; } /*! @@ -626,10 +609,10 @@ void EsminiAdapter::onRequestTestOrigin( int EsminiAdapter::initializeModule() { int retval = 0; - me->objectTrajectoryService = me->create_service(ServiceNames::getObjectTrajectory, - std::bind(&EsminiAdapter::onRequestObjectTrajectory, _1, _2)); - me->testOriginService = me->create_service(ServiceNames::getTestOrigin, - std::bind(&EsminiAdapter::onRequestTestOrigin, _1, _2)); + me->objectTrajectoryService = me->create_service( + ServiceNames::getObjectTrajectory, std::bind(&EsminiAdapter::onRequestObjectTrajectory, _1, _2)); + me->testOriginService = me->create_service( + ServiceNames::getTestOrigin, std::bind(&EsminiAdapter::onRequestTestOrigin, _1, _2)); return retval; } diff --git a/atos/modules/EsminiAdapter/src/main.cpp b/atos/modules/EsminiAdapter/src/main.cpp index 947b2d4e8..04bae3e2a 100644 --- a/atos/modules/EsminiAdapter/src/main.cpp +++ b/atos/modules/EsminiAdapter/src/main.cpp @@ -5,12 +5,11 @@ */ #include "esminiadapter.hpp" - static std::shared_ptr esminiAdapter; int main(int argc, char** argv) { - rclcpp::init(argc,argv); - auto exec = std::make_shared(); + rclcpp::init(argc, argv); + auto exec = std::make_shared(); esminiAdapter = EsminiAdapter::instance(); esminiAdapter->initializeModule(); exec->add_node(esminiAdapter); diff --git a/atos/modules/IntegrationTesting/inc/integrationtesting.hpp b/atos/modules/IntegrationTesting/inc/integrationtesting.hpp index 2ddff8fae..52d9f15b3 100644 --- a/atos/modules/IntegrationTesting/inc/integrationtesting.hpp +++ b/atos/modules/IntegrationTesting/inc/integrationtesting.hpp @@ -5,34 +5,33 @@ */ #pragma once +#include "atos_interfaces/srv/get_object_control_state.hpp" #include "module.hpp" #include "roschannels/commandchannels.hpp" -#include "atos_interfaces/srv/get_object_control_state.hpp" - class IntegrationTesting : public Module { - public: - IntegrationTesting(const std::string& moduleName); - ~IntegrationTesting(); +public: + IntegrationTesting(const std::string& moduleName); + ~IntegrationTesting(); - static inline std::string const testName = "scenario_execution"; - virtual void runIntegrationTest() = 0; + static inline std::string const testName = "scenario_execution"; + virtual void runIntegrationTest() = 0; - protected: - const std::string atosNamespace = "/atos/"; - const std::string initTopic = atosNamespace + ROSChannels::Init::topicName; - const std::string connectTopic = atosNamespace + ROSChannels::Connect::topicName; - const std::string armTopic = atosNamespace + ROSChannels::Arm::topicName; - const std::string startTopic = atosNamespace + ROSChannels::Start::topicName; - std::shared_ptr> initPub; - std::shared_ptr> connectPub; - std::shared_ptr> armPub; - std::shared_ptr> startPub; - std::shared_ptr> getObjectControlStateClient; - std::vector> stateResult; +protected: + const std::string atosNamespace = "/atos/"; + const std::string initTopic = atosNamespace + ROSChannels::Init::topicName; + const std::string connectTopic = atosNamespace + ROSChannels::Connect::topicName; + const std::string armTopic = atosNamespace + ROSChannels::Arm::topicName; + const std::string startTopic = atosNamespace + ROSChannels::Start::topicName; + std::shared_ptr> initPub; + std::shared_ptr> connectPub; + std::shared_ptr> armPub; + std::shared_ptr> startPub; + std::shared_ptr> getObjectControlStateClient; + std::vector> stateResult; - virtual void printResult() = 0; - int getObjectControlState(); - void checkState(const std::string& command); -}; \ No newline at end of file + virtual void printResult() = 0; + int getObjectControlState(); + void checkState(const std::string& command); +}; diff --git a/atos/modules/IntegrationTesting/inc/integrationtestingfactory.hpp b/atos/modules/IntegrationTesting/inc/integrationtestingfactory.hpp index b861ec127..e7bc84da0 100644 --- a/atos/modules/IntegrationTesting/inc/integrationtestingfactory.hpp +++ b/atos/modules/IntegrationTesting/inc/integrationtestingfactory.hpp @@ -8,12 +8,11 @@ #include "integrationtesting.hpp" #include "scenarioexecution.hpp" - class IntegrationTestingFactory { - public: - IntegrationTestingFactory(); - ~IntegrationTestingFactory(); +public: + IntegrationTestingFactory(); + ~IntegrationTestingFactory(); - std::shared_ptr createIntegrationTestExecution(const std::string& testName); -}; \ No newline at end of file + std::shared_ptr createIntegrationTestExecution(const std::string& testName); +}; diff --git a/atos/modules/IntegrationTesting/inc/integrationtestinghandler.hpp b/atos/modules/IntegrationTesting/inc/integrationtestinghandler.hpp index 86d58ec9d..edf1e9db8 100644 --- a/atos/modules/IntegrationTesting/inc/integrationtestinghandler.hpp +++ b/atos/modules/IntegrationTesting/inc/integrationtestinghandler.hpp @@ -8,17 +8,16 @@ #include "module.hpp" #include "roschannels/commandchannels.hpp" - class IntegrationTestingHandler : public Module { - public: - IntegrationTestingHandler(); - ~IntegrationTestingHandler(); +public: + IntegrationTestingHandler(); + ~IntegrationTestingHandler(); - private: - static inline std::string const moduleName = "integration_testing_handler"; - std::map integrationTests; +private: + static inline std::string const moduleName = "integration_testing_handler"; + std::map integrationTests; - void getIntegrationTests(); - void executeIntegrationTests(); + void getIntegrationTests(); + void executeIntegrationTests(); }; diff --git a/atos/modules/IntegrationTesting/inc/scenarioexecution.hpp b/atos/modules/IntegrationTesting/inc/scenarioexecution.hpp index 86185c968..6b5c9d1fa 100644 --- a/atos/modules/IntegrationTesting/inc/scenarioexecution.hpp +++ b/atos/modules/IntegrationTesting/inc/scenarioexecution.hpp @@ -5,25 +5,24 @@ */ #pragma once -#include "integrationtesting.hpp" #include "atos_interfaces/msg/monitor.hpp" #include "atos_interfaces/srv/get_object_trajectory.hpp" - +#include "integrationtesting.hpp" class ScenarioExecution : public IntegrationTesting { - public: - ScenarioExecution(); - ~ScenarioExecution(); +public: + ScenarioExecution(); + ~ScenarioExecution(); - private: - std::shared_ptr> monitorSub; - std::shared_ptr> getObjectTrajectoryClient; - bool followedTrajectory; +private: + std::shared_ptr> monitorSub; + std::shared_ptr> getObjectTrajectoryClient; + bool followedTrajectory; - void runIntegrationTest() override; - void printResult() override; - std::vector> getTrajectoryPoints(); - void checkObjectStoppedAtLastPoint(); - void placeholderCallback(const atos_interfaces::msg::Monitor::SharedPtr msg); + void runIntegrationTest() override; + void printResult() override; + std::vector> getTrajectoryPoints(); + void checkObjectStoppedAtLastPoint(); + void placeholderCallback(const atos_interfaces::msg::Monitor::SharedPtr msg); }; diff --git a/atos/modules/IntegrationTesting/src/integrationtesting.cpp b/atos/modules/IntegrationTesting/src/integrationtesting.cpp index 7441aa36a..8e55c2e28 100644 --- a/atos/modules/IntegrationTesting/src/integrationtesting.cpp +++ b/atos/modules/IntegrationTesting/src/integrationtesting.cpp @@ -8,33 +8,32 @@ using namespace std::chrono_literals; - /** * @brief Base class for all integration tests. This class contains functionality such as creating publishers for * sending commands to change state, and getting the current state. - * - * - * @param moduleName + * + * + * @param moduleName */ -IntegrationTesting::IntegrationTesting(const std::string& moduleName) : Module(moduleName) { - initPub = this->create_publisher(initTopic, 10); - connectPub = this->create_publisher(connectTopic, 10); - armPub = this->create_publisher(armTopic, 10); - startPub = this->create_publisher(startTopic, 10); - getObjectControlStateClient = this->create_client(atosNamespace + ServiceNames::getObjectControlState); +IntegrationTesting::IntegrationTesting(const std::string& moduleName) : + Module(moduleName) { + initPub = this->create_publisher(initTopic, 10); + connectPub = this->create_publisher(connectTopic, 10); + armPub = this->create_publisher(armTopic, 10); + startPub = this->create_publisher(startTopic, 10); + getObjectControlStateClient = this->create_client( + atosNamespace + ServiceNames::getObjectControlState); } - /** * @brief Destructor. - * + * */ IntegrationTesting::~IntegrationTesting() {} - /** * @brief Get the current state of the object. - * + * * @return int Current state of the object. */ int IntegrationTesting::getObjectControlState() { @@ -43,35 +42,30 @@ int IntegrationTesting::getObjectControlState() { return int(response->state); } - /** * @brief Check if the current state of the object is the same as the expected state. * This function also saves the result for printing later. - * + * * @param command The command sent in order to change state, e.g. "/atos/init". */ void IntegrationTesting::checkState(const std::string& command) { - auto state = getObjectControlState(); + auto state = getObjectControlState(); int expectedState = OBCState_t::OBC_STATE_UNDEFINED; if (command == initTopic) { expectedState = OBCState_t::OBC_STATE_INITIALIZED; - } - else if (command == connectTopic) { + } else if (command == connectTopic) { expectedState = OBCState_t::OBC_STATE_CONNECTED; - } - else if (command == armTopic) { + } else if (command == armTopic) { expectedState = OBCState_t::OBC_STATE_ARMED; - } - else if (command == startTopic) { + } else if (command == startTopic) { expectedState = OBCState_t::OBC_STATE_RUNNING; } - if(state != expectedState) { - RCLCPP_ERROR(get_logger(), "State is not correct. State is %d, expected state %d", state, expectedState); - } - else { + if (state != expectedState) { + RCLCPP_ERROR(get_logger(), "State is not correct. State is %d, expected state %d", state, expectedState); + } else { RCLCPP_INFO(get_logger(), "State is correct. State is %d, expected state %d", state, expectedState); } stateResult.push_back(std::make_pair(state, expectedState)); -} \ No newline at end of file +} diff --git a/atos/modules/IntegrationTesting/src/integrationtestingfactory.cpp b/atos/modules/IntegrationTesting/src/integrationtestingfactory.cpp index 1936106ac..9f1ed9305 100644 --- a/atos/modules/IntegrationTesting/src/integrationtestingfactory.cpp +++ b/atos/modules/IntegrationTesting/src/integrationtestingfactory.cpp @@ -7,29 +7,27 @@ /** * @brief Factory class for creating integration tests. - * + * */ IntegrationTestingFactory::IntegrationTestingFactory() {} - /** * @brief Destructor. - * + * */ IntegrationTestingFactory::~IntegrationTestingFactory() {} - /** * @brief Create an integration test object. - * + * * @param testName Which integration test to create. * @return std::shared_ptr New integration test object. */ -std::shared_ptr IntegrationTestingFactory::createIntegrationTestExecution(const std::string& testName) { +std::shared_ptr IntegrationTestingFactory::createIntegrationTestExecution( + const std::string& testName) { if (testName == ScenarioExecution::testName) { return std::make_shared(); - } - else { + } else { throw std::runtime_error("Unknown integration test: " + testName); } -} \ No newline at end of file +} diff --git a/atos/modules/IntegrationTesting/src/integrationtestinghandler.cpp b/atos/modules/IntegrationTesting/src/integrationtestinghandler.cpp index cb092f359..0a513cf42 100644 --- a/atos/modules/IntegrationTesting/src/integrationtestinghandler.cpp +++ b/atos/modules/IntegrationTesting/src/integrationtestinghandler.cpp @@ -3,53 +3,51 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include "integrationtestingfactory.hpp" #include "integrationtestinghandler.hpp" +#include "integrationtestingfactory.hpp" #include "scenarioexecution.hpp" - /** * @brief Class for creating integration tests. The idea is to set which integrations tests to run in * params.yaml, and this class will read the params and create the integration tests. - * + * */ -IntegrationTestingHandler::IntegrationTestingHandler() : Module(moduleName) { +IntegrationTestingHandler::IntegrationTestingHandler() : + Module(moduleName) { getIntegrationTests(); executeIntegrationTests(); } /** * @brief Destructor. - * + * */ IntegrationTestingHandler::~IntegrationTestingHandler() {} - /** * @brief Get the integration tests to run from params.yaml. - * + * */ void IntegrationTestingHandler::getIntegrationTests() { declare_parameter("scenario_execution", false); get_parameter("scenario_execution", integrationTests["scenario_execution"]); } - /** * @brief Execute the selected integration tests. Each integration test is run in a separate node. - * + * */ void IntegrationTestingHandler::executeIntegrationTests() { std::this_thread::sleep_for(std::chrono::seconds(3)); // sleep thread to allow other nodes to start for (auto const& [testName, testEnabled] : integrationTests) { if (testEnabled) { - auto executor = std::make_shared(); + auto executor = std::make_shared(); auto integrationTestFactory = std::make_shared(); - auto integrationTest = integrationTestFactory->createIntegrationTestExecution(testName); + auto integrationTest = integrationTestFactory->createIntegrationTestExecution(testName); executor->add_node(integrationTest); executor->spin_once(); } } RCLCPP_INFO(get_logger(), "Finished executing integration tests"); -} \ No newline at end of file +} diff --git a/atos/modules/IntegrationTesting/src/main.cpp b/atos/modules/IntegrationTesting/src/main.cpp index f677b6d6c..377cfb4c3 100644 --- a/atos/modules/IntegrationTesting/src/main.cpp +++ b/atos/modules/IntegrationTesting/src/main.cpp @@ -5,10 +5,10 @@ */ #include "integrationtestinghandler.hpp" -int main(int argc, char **argv) { +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto integrationTestingHandlerNode = std::make_shared(); rclcpp::spin(integrationTestingHandlerNode); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/atos/modules/IntegrationTesting/src/scenarioexecution.cpp b/atos/modules/IntegrationTesting/src/scenarioexecution.cpp index cc8cc9334..61192f41a 100644 --- a/atos/modules/IntegrationTesting/src/scenarioexecution.cpp +++ b/atos/modules/IntegrationTesting/src/scenarioexecution.cpp @@ -3,36 +3,36 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include #include "scenarioexecution.hpp" #include "rclcpp/wait_for_message.hpp" +#include using namespace std::chrono_literals; /** * @brief An integration test for scenario execution. This test will run a scenario execution for one object, * and check that all states are correct and that it follows the trajectory. - * + * */ -ScenarioExecution::ScenarioExecution() : IntegrationTesting(testName) { +ScenarioExecution::ScenarioExecution() : + IntegrationTesting(testName) { RCLCPP_INFO(get_logger(), "Started integration test: %s", testName.c_str()); - getObjectTrajectoryClient = this->create_client(atosNamespace + ServiceNames::getObjectTrajectory); + getObjectTrajectoryClient = + this->create_client(atosNamespace + ServiceNames::getObjectTrajectory); runIntegrationTest(); } - /** * @brief Destructor. - * + * */ ScenarioExecution::~ScenarioExecution() {} - /** * @brief Run the integration test. This integration test will initialize a scenario, connect to object 1, * arm the object, and then start it. It will check that all states are correct, and that the object follows * the trajectory. - * + * */ void ScenarioExecution::runIntegrationTest() { RCLCPP_INFO(get_logger(), "Running scenario"); @@ -41,7 +41,7 @@ void ScenarioExecution::runIntegrationTest() { initPub->publish(msg); std::this_thread::sleep_for(1s); // sleep to allow the object to initialize checkState(initTopic); - + connectPub->publish(msg); std::this_thread::sleep_for(1s); // sleep to allow the object to connect checkState(connectTopic); @@ -56,17 +56,16 @@ void ScenarioExecution::runIntegrationTest() { checkObjectStoppedAtLastPoint(); printResult(); -} - +} /** * @brief Get the x- and y-coordinates of the trajectory points for object 1. - * + * * @return std::vector> Trajectory points. */ std::vector> ScenarioExecution::getTrajectoryPoints() { auto request = std::make_shared(); - request->id = 1; + request->id = 1; std::shared_ptr response; this->callService(1000ms, getObjectTrajectoryClient, response, request); @@ -77,43 +76,47 @@ std::vector> ScenarioExecution::getTrajectoryPoints() return trajectory; } - /** * @brief Check that the object ends up near the end point of the trajectory. - * + * */ void ScenarioExecution::checkObjectStoppedAtLastPoint() { auto trajectory = getTrajectoryPoints(); std::pair lastPoint; auto isObjectMoving = true; - while(isObjectMoving) { + while (isObjectMoving) { atos_interfaces::msg::Monitor monr; - monitorSub = this->create_subscription("/atos/object_1/object_monitor", 10, std::bind(&ScenarioExecution::placeholderCallback, this, std::placeholders::_1)); + monitorSub = this->create_subscription( + "/atos/object_1/object_monitor", + 10, + std::bind(&ScenarioExecution::placeholderCallback, this, std::placeholders::_1)); rclcpp::wait_for_message(monr, monitorSub, this->get_node_base_interface()->get_context(), 50ms); double speedThreshold = 0.1; if (abs(monr.velocity.twist.linear.x) < speedThreshold && abs(monr.velocity.twist.linear.y) < speedThreshold) { - lastPoint = std::make_pair(monr.pose.pose.position.x, monr.pose.pose.position.y); + lastPoint = std::make_pair(monr.pose.pose.position.x, monr.pose.pose.position.y); isObjectMoving = false; } } - auto distanceX = lastPoint.first - trajectory.back().first; - auto distanceY = lastPoint.second - trajectory.back().second; + auto distanceX = lastPoint.first - trajectory.back().first; + auto distanceY = lastPoint.second - trajectory.back().second; auto distanceThreshold = 1.0; if (abs(distanceX) < distanceThreshold && abs(distanceY) < distanceThreshold) { followedTrajectory = true; RCLCPP_INFO(get_logger(), "Object followed trajectory succesfully"); - } - else { + } else { followedTrajectory = false; - RCLCPP_ERROR(get_logger(), "Object did not follow trajectory succesfully. Distance from end point of trajectory is x: %f, y: %f", distanceX, distanceY); + RCLCPP_ERROR( + get_logger(), + "Object did not follow trajectory succesfully. Distance from end point of trajectory is x: %f, y: %f", + distanceX, + distanceY); } } - /** * @brief Print the result of the integration test. - * + * */ void ScenarioExecution::printResult() { std::stringstream ss; @@ -121,9 +124,8 @@ void ScenarioExecution::printResult() { ss << "\nIntegration test - Scenario Excecution results:\n"; ss << "State change results table:\n"; ss << std::setfill('-') << std::setw(width * 3) << "-" << std::endl; - ss << std::left << std::setfill(' ') << std::setw(width) << "State" - << std::setw(width) << "Expected state" - << std::setw(width) << "Pass" << std::endl; + ss << std::left << std::setfill(' ') << std::setw(width) << "State" << std::setw(width) << "Expected state" + << std::setw(width) << "Pass" << std::endl; ss << std::setfill('-') << std::setw(width * 3) << "-" << std::endl; bool allStatesCorrect = true; @@ -132,9 +134,8 @@ void ScenarioExecution::printResult() { if (pass == "NOT OK") { allStatesCorrect = false; } - ss << std::left << std::setfill(' ') << std::setw(width) << state - << std::setw(width) << expectedState - << std::setw(width) << pass << std::endl; + ss << std::left << std::setfill(' ') << std::setw(width) << state << std::setw(width) << expectedState + << std::setw(width) << pass << std::endl; } ss << std::setfill('-') << std::setw(width * 3) << "-" << std::endl; ss << std::setfill(' '); @@ -146,10 +147,9 @@ void ScenarioExecution::printResult() { RCLCPP_INFO(get_logger(), ss.str().c_str()); } - /** * @brief This is a placeholder callback function. It is needed in order to create a topic subscriber. - * + * * @param msg Message. */ -void ScenarioExecution::placeholderCallback(const atos_interfaces::msg::Monitor::SharedPtr msg) {} \ No newline at end of file +void ScenarioExecution::placeholderCallback(const atos_interfaces::msg::Monitor::SharedPtr msg) {} diff --git a/atos/modules/JournalControl/inc/journalcontrol.hpp b/atos/modules/JournalControl/inc/journalcontrol.hpp index a50104002..8d5a5a2a5 100644 --- a/atos/modules/JournalControl/inc/journalcontrol.hpp +++ b/atos/modules/JournalControl/inc/journalcontrol.hpp @@ -24,33 +24,32 @@ extern "C" { } #endif - -#include "module.hpp" #include "journalmodelcollection.hpp" +#include "module.hpp" /*------------------------------------------------------------ -- Function declarations. ------------------------------------------------------------*/ -class JournalControl : public Module -{ +class JournalControl : public Module { public: explicit JournalControl(); void initialize(); + private: static inline std::string const moduleName = "journal_control"; - std::string scenarioName; + std::string scenarioName; JournalModelCollection journals; void onArmMessage(const ROSChannels::Arm::message_type::SharedPtr) override; void onStopMessage(const ROSChannels::Stop::message_type::SharedPtr) override; void onAbortMessage(const ROSChannels::Abort::message_type::SharedPtr) override; - void onReplayMessage(const ROSChannels::Replay::message_type::SharedPtr) override; + void onReplayMessage(const ROSChannels::Replay::message_type::SharedPtr) override; - ROSChannels::Arm::Sub armSub; - ROSChannels::Stop::Sub stopSub; - ROSChannels::Abort::Sub abortSub; - ROSChannels::Replay::Sub replaySub; - ROSChannels::Exit::Sub exitSub; + ROSChannels::Arm::Sub armSub; + ROSChannels::Stop::Sub stopSub; + ROSChannels::Abort::Sub abortSub; + ROSChannels::Replay::Sub replaySub; + ROSChannels::Exit::Sub exitSub; }; #endif //__LOGGER_H_INCLUDED__ diff --git a/atos/modules/JournalControl/inc/journalmodel.hpp b/atos/modules/JournalControl/inc/journalmodel.hpp index 3234ac8a4..8d0b37bec 100644 --- a/atos/modules/JournalControl/inc/journalmodel.hpp +++ b/atos/modules/JournalControl/inc/journalmodel.hpp @@ -14,8 +14,8 @@ namespace fs = std::filesystem; namespace fs = std::experimental::filesystem; #endif -#include #include "loggable.hpp" +#include class JournalModel : public Loggable { public: @@ -23,25 +23,33 @@ class JournalModel : public Loggable { private: fs::path filePath; std::streampos filePosition; + public: - Bookmark(rclcpp::Logger log) : Loggable(log) {} + Bookmark(rclcpp::Logger log) : + Loggable(log) {} bool valid = false; - void place(const fs::path &path, const bool placeAtBeginning=false); - const std::streampos& getPosition() const { return filePosition; } - const fs::path& getFilePath() const { return filePath; } + void place(const fs::path& path, const bool placeAtBeginning = false); + const std::streampos& getPosition() const { + return filePosition; + } + const fs::path& getFilePath() const { + return filePath; + } std::string toString() const { - return valid ? getFilePath().string() + " @" + std::to_string(getPosition()) - : ""; + return valid ? getFilePath().string() + " @" + std::to_string(getPosition()) : ""; } }; - JournalModel(rclcpp::Logger log) : Loggable(log), startReference(log), stopReference(log) {} + JournalModel(rclcpp::Logger log) : + Loggable(log), + startReference(log), + stopReference(log) {} std::string moduleName; mutable Bookmark startReference; mutable Bookmark stopReference; mutable std::set containedFiles; - bool operator==(const JournalModel &other) const { + bool operator==(const JournalModel& other) const { return this->moduleName == other.moduleName; } @@ -50,11 +58,12 @@ class JournalModel : public Loggable { //! Template specialization of std::hash for JournalModel namespace std { - template <> struct hash { - size_t operator() (const JournalModel &journal) const { - return std::hash{}(journal.moduleName); - } - }; -} +template<> +struct hash { + size_t operator()(const JournalModel& journal) const { + return std::hash{}(journal.moduleName); + } +}; +} // namespace std -#endif // JOURNALMODEL_HPP \ No newline at end of file +#endif // JOURNALMODEL_HPP diff --git a/atos/modules/JournalControl/inc/journalmodelcollection.hpp b/atos/modules/JournalControl/inc/journalmodelcollection.hpp index 3c1f12e38..2ab775d10 100644 --- a/atos/modules/JournalControl/inc/journalmodelcollection.hpp +++ b/atos/modules/JournalControl/inc/journalmodelcollection.hpp @@ -6,28 +6,31 @@ #ifndef JOURNALMODELCOLLECTION_HPP #define JOURNALMODELCOLLECTION_HPP -#include +#include "journalmodel.hpp" +#include "loggable.hpp" #include #include +#include #include -#include "journalmodel.hpp" -#include "loggable.hpp" // Add a C++20 type namespace std::chrono { - typedef duration> days; +typedef duration> days; } -class JournalModelCollection : public std::unordered_set, public Loggable { +class JournalModelCollection + : public std::unordered_set + , public Loggable { public: - JournalModelCollection(rclcpp::Logger log) : Loggable(log) {} + JournalModelCollection(rclcpp::Logger log) : + Loggable(log) {} void placeStartBookmarks(); void placeStopBookmarks(); void insertNonBookmarked(); int dumpToFile(std::string filename); std::string toString() const { std::string retval = ""; - for (const auto &journal : *this) { + for (const auto& journal : *this) { retval += journal.toString() + "\n"; } if (this->size() > 0) { @@ -35,18 +38,18 @@ class JournalModelCollection : public std::unordered_set, public L } return retval; } + private: std::chrono::time_point startDay; std::chrono::time_point stopDay; - - static std::string getDateAsString(const std::chrono::system_clock::time_point &date); - static std::string getCurrentDateAsString(); - static std::vector getJournalFilesFrom(const std::chrono::system_clock::time_point &date); - static std::vector getJournalFilesFromToday() { - return getJournalFilesFrom(std::chrono::system_clock::now()); - } - static int printFilesTo(const fs::path &inputDirectory, std::ostream &outputFile); + static std::string getDateAsString(const std::chrono::system_clock::time_point& date); + static std::string getCurrentDateAsString(); + static std::vector getJournalFilesFrom(const std::chrono::system_clock::time_point& date); + static std::vector getJournalFilesFromToday() { + return getJournalFilesFrom(std::chrono::system_clock::now()); + } + static int printFilesTo(const fs::path& inputDirectory, std::ostream& outputFile); }; -#endif // JOURNALMODELCOLLECTION_HPP \ No newline at end of file +#endif // JOURNALMODELCOLLECTION_HPP diff --git a/atos/modules/JournalControl/src/journalcontrol.cpp b/atos/modules/JournalControl/src/journalcontrol.cpp index 244ff1f7e..197a1a7bb 100644 --- a/atos/modules/JournalControl/src/journalcontrol.cpp +++ b/atos/modules/JournalControl/src/journalcontrol.cpp @@ -11,15 +11,15 @@ -- Reference : ------------------------------------------------------------------------------*/ -#include -#include -#include -#include +#include +#include #include #include #include -#include -#include +#include +#include +#include +#include // GCC version 8.1 brings non-experimental support for std::filesystem #if __GNUC__ > 8 || (__GNUC__ == 8 && __GNUC_MINOR__ >= 1) #include @@ -27,8 +27,8 @@ #include #endif -#include "journalcontrol.hpp" #include "journal.hpp" +#include "journalcontrol.hpp" #if __GNUC__ > 8 || (__GNUC__ == 8 && __GNUC_MINOR__ >= 1) namespace fs = std::filesystem; @@ -40,34 +40,30 @@ using std::placeholders::_1; static void signalHandler(int signo); -JournalControl::JournalControl() - : Module(JournalControl::moduleName), - journals(get_logger()), - armSub(*this, std::bind(&JournalControl::onArmMessage, this, _1)), - stopSub(*this, std::bind(&JournalControl::onStopMessage, this, _1)), - abortSub(*this, std::bind(&JournalControl::onAbortMessage, this, _1)), - replaySub(*this, std::bind(&JournalControl::onReplayMessage, this, _1)), - exitSub(*this, std::bind(&JournalControl::onExitMessage, this, _1)) -{ +JournalControl::JournalControl() : + Module(JournalControl::moduleName), + journals(get_logger()), + armSub(*this, std::bind(&JournalControl::onArmMessage, this, _1)), + stopSub(*this, std::bind(&JournalControl::onStopMessage, this, _1)), + abortSub(*this, std::bind(&JournalControl::onAbortMessage, this, _1)), + replaySub(*this, std::bind(&JournalControl::onReplayMessage, this, _1)), + exitSub(*this, std::bind(&JournalControl::onExitMessage, this, _1)) { declare_parameter("scenario_name", "default_scenario_name"); get_parameter("scenario_name", scenarioName); initialize(); } -void JournalControl::initialize() -{ +void JournalControl::initialize() { if (std::signal(SIGINT, signalHandler) == SIG_ERR) { throw std::runtime_error("Failed to register signal handler"); } } -void JournalControl::onArmMessage(const Arm::message_type::SharedPtr) -{ +void JournalControl::onArmMessage(const Arm::message_type::SharedPtr) { journals.placeStartBookmarks(); } -void JournalControl::onStopMessage(const Stop::message_type::SharedPtr) -{ +void JournalControl::onStopMessage(const Stop::message_type::SharedPtr) { try { // Save stop references journals.placeStopBookmarks(); @@ -76,26 +72,24 @@ void JournalControl::onStopMessage(const Stop::message_type::SharedPtr) journals.insertNonBookmarked(); // Merge journals into named output journals.dumpToFile(scenarioName); - } catch (std::exception &e) { + } catch (std::exception& e) { RCLCPP_ERROR(get_logger(), "Failed to save journal: %s", e.what()); } } -void JournalControl::onAbortMessage(const Abort::message_type::SharedPtr msg) -{ +void JournalControl::onAbortMessage(const Abort::message_type::SharedPtr msg) { // Temporary: Treat ABORT as stop signal onStopMessage(msg); } -void JournalControl::onReplayMessage(const Replay::message_type::SharedPtr) -{ +void JournalControl::onReplayMessage(const Replay::message_type::SharedPtr) { RCLCPP_WARN(get_logger(), "Replay function out of date"); } -void Module::onExitMessage(const Exit::message_type::SharedPtr){ - this->quit=true; +void Module::onExitMessage(const Exit::message_type::SharedPtr) { + this->quit = true; } void signalHandler(int) { rclcpp::shutdown(); -} \ No newline at end of file +} diff --git a/atos/modules/JournalControl/src/journalmodel.cpp b/atos/modules/JournalControl/src/journalmodel.cpp index b1c2e4716..2f1d2d50a 100644 --- a/atos/modules/JournalControl/src/journalmodel.cpp +++ b/atos/modules/JournalControl/src/journalmodel.cpp @@ -8,28 +8,26 @@ #include std::string JournalModel::toString() const { - std::string retval = ""; - retval += moduleName + " journal\n\tFiles:\n"; - for (const auto &file : containedFiles) { - retval += "\t\t* " + file.string() + "\n"; - } - retval += "\tStart / end references:\n"; - retval += "\t\ts: " + startReference.toString() + "\n"; - retval += "\t\te: " + stopReference.toString(); - return retval; + std::string retval = ""; + retval += moduleName + " journal\n\tFiles:\n"; + for (const auto& file : containedFiles) { + retval += "\t\t* " + file.string() + "\n"; + } + retval += "\tStart / end references:\n"; + retval += "\t\ts: " + startReference.toString() + "\n"; + retval += "\t\te: " + stopReference.toString(); + return retval; } -void JournalModel::Bookmark::place(const fs::path &path, const bool placeAtBeginning) { - std::ifstream istrm(path, placeAtBeginning ? std::ios_base::in - : std::ios_base::ate); - if (istrm.is_open()) { - RCLCPP_DEBUG(get_logger(), "Storing bookmark to file %s", path.c_str()); - filePosition = istrm.tellg(); - filePath = path; - valid = true; - istrm.close(); - } - else { - throw std::runtime_error("Failed to open file " + path.string()); - } -} \ No newline at end of file +void JournalModel::Bookmark::place(const fs::path& path, const bool placeAtBeginning) { + std::ifstream istrm(path, placeAtBeginning ? std::ios_base::in : std::ios_base::ate); + if (istrm.is_open()) { + RCLCPP_DEBUG(get_logger(), "Storing bookmark to file %s", path.c_str()); + filePosition = istrm.tellg(); + filePath = path; + valid = true; + istrm.close(); + } else { + throw std::runtime_error("Failed to open file " + path.string()); + } +} diff --git a/atos/modules/JournalControl/src/journalmodelcollection.cpp b/atos/modules/JournalControl/src/journalmodelcollection.cpp index 6022b9b59..fef6267dc 100644 --- a/atos/modules/JournalControl/src/journalmodelcollection.cpp +++ b/atos/modules/JournalControl/src/journalmodelcollection.cpp @@ -5,12 +5,11 @@ */ #include "journalmodelcollection.hpp" #include "journal.hpp" -#include +#include #include #include #include -#include - +#include #define DATE_STRING_MAX_LEN 20 @@ -20,14 +19,14 @@ void JournalModelCollection::placeStartBookmarks() { auto journalFilesFromToday = getJournalFilesFromToday(); - auto currentDate = getCurrentDateAsString(); + auto currentDate = getCurrentDateAsString(); this->clear(); - for (const auto &journalFile : journalFilesFromToday) { + for (const auto& journalFile : journalFilesFromToday) { RCLCPP_DEBUG(get_logger(), "Storing start bookmark in file %s", journalFile.c_str()); JournalModel journal(get_logger()); - auto datePosition = journalFile.stem().string().find("-" + currentDate); + auto datePosition = journalFile.stem().string().find("-" + currentDate); journal.moduleName = journalFile.stem().string().substr(0, datePosition); journal.startReference.place(journalFile); journal.containedFiles.insert(journalFile); @@ -37,20 +36,19 @@ void JournalModelCollection::placeStartBookmarks() { this->startDay = floor(system_clock::now()); } - /*! * \brief placeStopBookmarks Stores references to the current end of file of all journals. */ void JournalModelCollection::placeStopBookmarks() { auto journalFilesFromToday = getJournalFilesFromToday(); - auto currentDate = getCurrentDateAsString(); + auto currentDate = getCurrentDateAsString(); - for (const auto &journalFile : journalFilesFromToday) { + for (const auto& journalFile : journalFilesFromToday) { RCLCPP_DEBUG(get_logger(), "Storing stop bookmark in file %s", journalFile.c_str()); // Find existing journal matching module name from file JournalModel soughtJournal(get_logger()); - auto datePosition = journalFile.stem().string().find("-" + currentDate); + auto datePosition = journalFile.stem().string().find("-" + currentDate); soughtJournal.moduleName = journalFile.stem().string().substr(0, datePosition); soughtJournal.startReference.place(journalFile, true); // In case we are emplacing, startReference at beginning // Insert checks if an equivalent element (same module) already exists @@ -88,22 +86,20 @@ void JournalModelCollection::insertNonBookmarked() { // is not specified, place it at beginning of the first file found. for (auto day = startDay; day <= stopDay; day += std::chrono::days(1)) { auto journalFilesFromDay = getJournalFilesFrom(day); - auto date = getDateAsString(day); + auto date = getDateAsString(day); - for (const auto &journalFile : journalFilesFromDay) { + for (const auto& journalFile : journalFilesFromDay) { // Find existing journal matching module name from file JournalModel soughtJournal(get_logger()); - auto datePosition = journalFile.stem().string().find("-" + date); + auto datePosition = journalFile.stem().string().find("-" + date); soughtJournal.moduleName = journalFile.stem().string().substr(0, datePosition); // Insert checks if an equivalent element (same module) already exists auto matchingJournal = insert(soughtJournal).first; if (matchingJournal->containedFiles.insert(journalFile).second) { - RCLCPP_DEBUG(get_logger(), "Inserted non-bookmarked file %s", - journalFile.c_str()); + RCLCPP_DEBUG(get_logger(), "Inserted non-bookmarked file %s", journalFile.c_str()); } if (!matchingJournal->startReference.valid) { - RCLCPP_DEBUG(get_logger(), "Storing start bookmark at beginning of file %s", - journalFile.c_str()); + RCLCPP_DEBUG(get_logger(), "Storing start bookmark at beginning of file %s", journalFile.c_str()); matchingJournal->startReference.place(journalFile, true); } } @@ -114,15 +110,14 @@ void JournalModelCollection::insertNonBookmarked() { for (auto journal : *this) { if (!journal.stopReference.valid) { for (auto day = stopDay; day >= startDay; day -= std::chrono::days(1)) { - auto date = getDateAsString(day); + auto date = getDateAsString(day); auto isFromDate = [date](const fs::path& filePath) { return filePath.filename().string().find(date) != std::string::npos; }; - auto matchingFile = std::find_if(journal.containedFiles.begin(), - journal.containedFiles.end(), isFromDate); + auto matchingFile = + std::find_if(journal.containedFiles.begin(), journal.containedFiles.end(), isFromDate); if (matchingFile != journal.containedFiles.end()) { - RCLCPP_DEBUG(get_logger(), "Storing end bookmark at end of file %s", - matchingFile->c_str()); + RCLCPP_DEBUG(get_logger(), "Storing end bookmark at end of file %s", matchingFile->c_str()); journal.stopReference.place(*matchingFile); break; } @@ -131,7 +126,6 @@ void JournalModelCollection::insertNonBookmarked() { } } - /*! * \brief dumpToFile Generates a merged file based on input journals * with ascending timestamps. The output journal file is created by @@ -140,14 +134,14 @@ void JournalModelCollection::insertNonBookmarked() { */ int JournalModelCollection::dumpToFile(std::string fileName) { - int retval = 0; + int retval = 0; char journalDir[PATH_MAX] = {'\0'}; - //TODO: Create a function for this. - UtilGetJournalDirectoryPath(journalDir, sizeof (journalDir)); + // TODO: Create a function for this. + UtilGetJournalDirectoryPath(journalDir, sizeof(journalDir)); // If a filename with the same name exists, add a number to the end of the filename int maxnum = 0; - for (const auto & entry : fs::directory_iterator(std::string(journalDir))) { + for (const auto& entry : fs::directory_iterator(std::string(journalDir))) { auto entryFileName = entry.path().filename().string(); // Find the file with maximum number if (entryFileName.find(fileName) != std::string::npos) { @@ -183,9 +177,9 @@ int JournalModelCollection::dumpToFile(std::string fileName) { std::streampos beg; //!< First character index of relevant section std::streampos end; //!< Last character index of relevant section std::string lastRead; //!< Last read string from ::istrm member - unsigned int nReadRows = 0; //!< Number of rows read from ::istrm member + unsigned int nReadRows = 0; //!< Number of rows read from ::istrm member //! The < operator tells which of two is oldest at its last read line - bool operator< (const JournalFileSection &other) const { + bool operator<(const JournalFileSection& other) const { std::istringstream strThis(lastRead), strOther(other.lastRead); double timeThis = 0.0, timeOther = 0.0; strThis >> timeThis; @@ -200,51 +194,48 @@ int JournalModelCollection::dumpToFile(std::string fileName) { // After this, the vector contains opened streams positioned // for reading at the line closest to the start time. std::vector inputFiles; - for (const auto &journal : *this) { - for (const fs::path &file : journal.containedFiles) { - JournalFileSection §ion = inputFiles.emplace_back(); - section.path = file; + for (const auto& journal : *this) { + for (const fs::path& file : journal.containedFiles) { + JournalFileSection& section = inputFiles.emplace_back(); + section.path = file; section.istrm.open(file); if (section.istrm.is_open()) { RCLCPP_DEBUG(get_logger(), "Opened file %s", file.c_str()); if (file == journal.startReference.getFilePath()) { section.beg = journal.startReference.getPosition(); - } - else { + } else { section.istrm.seekg(0, section.istrm.beg); section.beg = section.istrm.tellg(); } if (file == journal.stopReference.getFilePath()) { section.end = journal.stopReference.getPosition(); - } - else { + } else { section.istrm.seekg(0, section.istrm.end); section.end = section.istrm.tellg(); } section.istrm.seekg(section.beg); if (section.end - section.beg < 0) { - RCLCPP_ERROR(get_logger(), "End precedes beginning in file %s: beg @%ld, end @%ld", - file.c_str(), (long int)section.beg, (long int)section.end); + RCLCPP_ERROR(get_logger(), + "End precedes beginning in file %s: beg @%ld, end @%ld", + file.c_str(), + (long int)section.beg, + (long int)section.end); section.istrm.close(); inputFiles.pop_back(); - } - else if (section.beg == section.end) { + } else if (section.beg == section.end) { RCLCPP_DEBUG(get_logger(), "No data found"); section.istrm.close(); inputFiles.pop_back(); - } - else if (!std::getline(section.istrm, section.lastRead)) { + } else if (!std::getline(section.istrm, section.lastRead)) { RCLCPP_DEBUG(get_logger(), "Failed to read line"); section.istrm.close(); inputFiles.pop_back(); - } - else { + } else { section.nReadRows++; } - } - else { + } else { RCLCPP_ERROR(get_logger(), "Unable to open %s for reading", file.c_str()); inputFiles.pop_back(); retval = -1; @@ -253,22 +244,22 @@ int JournalModelCollection::dumpToFile(std::string fileName) { } // Each iteration, transfer the line starting with the oldest timestamp to the output file and read the next - while(!inputFiles.empty()) { + while (!inputFiles.empty()) { std::vector::iterator oldestFile = std::min_element(inputFiles.begin(), inputFiles.end()); ostrm << oldestFile->lastRead << std::endl; if (!std::getline(oldestFile->istrm, oldestFile->lastRead)) { - RCLCPP_DEBUG(get_logger(), "Reached end of journal file %s, read %u rows", - oldestFile->path.c_str(), oldestFile->nReadRows); + RCLCPP_DEBUG(get_logger(), + "Reached end of journal file %s, read %u rows", + oldestFile->path.c_str(), + oldestFile->nReadRows); oldestFile->istrm.close(); inputFiles.erase(oldestFile); - } - else if (oldestFile->nReadRows > oldestFile->end - oldestFile->beg) { - RCLCPP_DEBUG(get_logger(), "Read %u rows from journal file %s", - oldestFile->nReadRows, oldestFile->path.c_str()); + } else if (oldestFile->nReadRows > oldestFile->end - oldestFile->beg) { + RCLCPP_DEBUG( + get_logger(), "Read %u rows from journal file %s", oldestFile->nReadRows, oldestFile->path.c_str()); oldestFile->istrm.close(); inputFiles.erase(oldestFile); - } - else { + } else { oldestFile->nReadRows++; } } @@ -291,11 +282,11 @@ std::string JournalModelCollection::getCurrentDateAsString() { * \param date Timestamp for which date string is to be extracted. * \return A std::string containing the date representation */ -std::string JournalModelCollection::getDateAsString(const std::chrono::system_clock::time_point &date) { - using Clock = std::chrono::system_clock; +std::string JournalModelCollection::getDateAsString(const std::chrono::system_clock::time_point& date) { + using Clock = std::chrono::system_clock; char dateString[DATE_STRING_MAX_LEN] = {'\0'}; - auto dateRaw = Clock::to_time_t(date); - auto dateStructPtr = std::localtime(&dateRaw); + auto dateRaw = Clock::to_time_t(date); + auto dateStructPtr = std::localtime(&dateRaw); std::strftime(dateString, DATE_STRING_MAX_LEN, "%Y-%m-%d", dateStructPtr); return dateString; } @@ -306,13 +297,12 @@ std::string JournalModelCollection::getDateAsString(const std::chrono::system_cl * \param date Date for which journals are to be fetched * \return A std::vector containing file paths */ -std::vector JournalModelCollection::getJournalFilesFrom(const std::chrono::system_clock::time_point &date) { +std::vector JournalModelCollection::getJournalFilesFrom(const std::chrono::system_clock::time_point& date) { std::vector journalsFromDate; std::vector buffer(PATH_MAX, '\0'); UtilGetJournalDirectoryPath(buffer.data(), buffer.size()); - buffer.erase(std::find(buffer.begin(), buffer.end(), '\0'), - buffer.end()); + buffer.erase(std::find(buffer.begin(), buffer.end(), '\0'), buffer.end()); fs::path journalDirPath(buffer.begin(), buffer.end()); if (!exists(journalDirPath)) { @@ -321,11 +311,11 @@ std::vector JournalModelCollection::getJournalFilesFrom(const std::chr auto dateString = getDateAsString(date); - for (const auto &dirEntry : fs::directory_iterator(journalDirPath)) { - if (fs::is_regular_file(dirEntry.status()) - && dirEntry.path().extension().string().find(JOURNAL_FILE_ENDING) != std::string::npos) { + for (const auto& dirEntry : fs::directory_iterator(journalDirPath)) { + if (fs::is_regular_file(dirEntry.status()) && + dirEntry.path().extension().string().find(JOURNAL_FILE_ENDING) != std::string::npos) { // Check if file contains current date - size_t pos = dirEntry.path().string().find(dateString); + size_t pos = dirEntry.path().string().find(dateString); if (pos != std::string::npos) { journalsFromDate.push_back(dirEntry.path()); } @@ -341,12 +331,12 @@ std::vector JournalModelCollection::getJournalFilesFrom(const std::chr * \param outputFile Stream to which contents are to be printed * \return 0 on success, -1 otherwise */ -int JournalModelCollection::printFilesTo(const fs::path &inputDirectory, std::ostream &outputFile) { +int JournalModelCollection::printFilesTo(const fs::path& inputDirectory, std::ostream& outputFile) { if (!exists(inputDirectory)) { throw std::runtime_error("Unable to find directory " + inputDirectory.string()); } - for (const auto &dirEntry : fs::directory_iterator(inputDirectory)) { + for (const auto& dirEntry : fs::directory_iterator(inputDirectory)) { if (fs::is_regular_file(dirEntry.status())) { const auto inputFile = dirEntry.path(); std::ifstream istrm(inputFile.string()); diff --git a/atos/modules/JournalControl/src/main.cpp b/atos/modules/JournalControl/src/main.cpp index 4d317052b..3fc7f3fce 100644 --- a/atos/modules/JournalControl/src/main.cpp +++ b/atos/modules/JournalControl/src/main.cpp @@ -3,13 +3,13 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include "util.h" -#include "rclcpp/rclcpp.hpp" #include "journalcontrol.hpp" +#include "rclcpp/rclcpp.hpp" +#include "util.h" -int main(int argc, char **argv) { - rclcpp::init(argc,argv); +int main(int argc, char** argv) { + rclcpp::init(argc, argv); auto node = std::make_shared(); rclcpp::spin(node); return 0; -} \ No newline at end of file +} diff --git a/atos/modules/MQTTBridge/inc/Imqtt2ros.hpp b/atos/modules/MQTTBridge/inc/Imqtt2ros.hpp index 17927c8b2..8a52e9f7b 100644 --- a/atos/modules/MQTTBridge/inc/Imqtt2ros.hpp +++ b/atos/modules/MQTTBridge/inc/Imqtt2ros.hpp @@ -7,16 +7,14 @@ * is the MQTT topic name. */ struct Mqtt2RosInterface { - struct { - int qos = 0; ///< MQTT QoS value - } mqtt; ///< MQTT-related variables - struct { - std::string topic; ///< ROS topic - std::string msg_type; ///< message type of publisher - rclcpp::Publisher::SharedPtr - publisher; ///< ROS publisher - int queue_size = 1; ///< ROS publisher queue size - bool is_stale = - false; ///< whether a new generic publisher/subscriber is required - } ros; ///< ROS-related variables -}; \ No newline at end of file + struct { + int qos = 0; ///< MQTT QoS value + } mqtt; ///< MQTT-related variables + struct { + std::string topic; ///< ROS topic + std::string msg_type; ///< message type of publisher + rclcpp::Publisher::SharedPtr publisher; ///< ROS publisher + int queue_size = 1; ///< ROS publisher queue size + bool is_stale = false; ///< whether a new generic publisher/subscriber is required + } ros; ///< ROS-related variables +}; diff --git a/atos/modules/MQTTBridge/inc/Iros2mqtt.hpp b/atos/modules/MQTTBridge/inc/Iros2mqtt.hpp index 295af0ce6..56ad38a04 100644 --- a/atos/modules/MQTTBridge/inc/Iros2mqtt.hpp +++ b/atos/modules/MQTTBridge/inc/Iros2mqtt.hpp @@ -5,16 +5,15 @@ * @brief Struct containing variables related to a MQTT2ROS connection. */ struct Ros2MqttInterface { - struct { - std::string topic; ///< MQTT topic - int qos = 0; ///< MQTT QoS value - } mqtt; ///< MQTT-related variables - struct { - std::string topic; ///< ROS topic - std::string msg_type; ///< message type of subscriber - rclcpp::GenericSubscription::SharedPtr subscriber; ///< ROS subscriber - int queue_size = 1; ///< ROS subscriber queue size - bool is_stale = - false; ///< whether a new generic publisher/subscriber is required - } ros; ///< ROS-related variables -}; \ No newline at end of file + struct { + std::string topic; ///< MQTT topic + int qos = 0; ///< MQTT QoS value + } mqtt; ///< MQTT-related variables + struct { + std::string topic; ///< ROS topic + std::string msg_type; ///< message type of subscriber + rclcpp::GenericSubscription::SharedPtr subscriber; ///< ROS subscriber + int queue_size = 1; ///< ROS subscriber queue size + bool is_stale = false; ///< whether a new generic publisher/subscriber is required + } ros; ///< ROS-related variables +}; diff --git a/atos/modules/MQTTBridge/inc/mqttbridge.hpp b/atos/modules/MQTTBridge/inc/mqttbridge.hpp index b03d001e2..802d384fc 100644 --- a/atos/modules/MQTTBridge/inc/mqttbridge.hpp +++ b/atos/modules/MQTTBridge/inc/mqttbridge.hpp @@ -24,152 +24,135 @@ using json = nlohmann::json; * publisher */ -class MqttBridge : public Module, - public virtual mqtt::callback, - public virtual mqtt::iaction_listener { +class MqttBridge + : public Module + , public virtual mqtt::callback + , public virtual mqtt::iaction_listener { public: - MqttBridge(); - void initialize(); + MqttBridge(); + void initialize(); protected: - /** - * @brief Loads ROS parameters from parameter server. - */ - void loadParameters(); + /** + * @brief Loads ROS parameters from parameter server. + */ + void loadParameters(); - void connect(); + void connect(); - void newMqtt2RosBridge( - const std::shared_ptr - request, - std::shared_ptr - response); + void newMqtt2RosBridge(const std::shared_ptr request, + std::shared_ptr response); - void newRos2MqttBridge( - const std::shared_ptr - request, - std::shared_ptr - response); + void newRos2MqttBridge(const std::shared_ptr request, + std::shared_ptr response); private: - /** - * @brief MQTT2ROS connection variables sorted by MQTT topic - */ - std::map mqtt2ros_; - - /** - * @brief ROS2MQTT connection variables sorted by ROS topic - */ - std::map ros2mqtt_; - - /** @brief ROS Service server for providing dynamic MQTT to ROS mappings. - */ - rclcpp::Service::SharedPtr - new_mqtt2ros_bridge_service_; - - /** @brief ROS Service server for providing dynamic ROS to MQTT mappings. - */ - rclcpp::Service::SharedPtr - new_ros2mqtt_bridge_service_; - - /** - * @brief MQTT client variable - */ - std::shared_ptr client_; - - /** - * @brief MQTT client connection options - */ - mqtt::connect_options connect_options_; - - /** - * @brief Callback for when the client receives a MQTT message from the - * broker. - * - * Overrides mqtt::callback::message_arrived(mqtt::const_message_ptr). - * Publishes any empty ROS message to the corresponding ROS topic. - * - * @param mqtt_msg MQTT message - */ - void message_arrived(mqtt::const_message_ptr mqtt_msg) override; - - /** - * @brief Checks all active ROS topics in order to set up generic subscribers. - */ - void setupSubscriptions(); - - /** - * @brief Publishes a ROS message received via MQTT to ROS. - * - * @param mqtt_msg MQTT message - * @param arrival_stamp arrival timestamp used for latency computation - */ - void mqtt2ros(mqtt::const_message_ptr mqtt_msg, - const rclcpp::Time &arrival_stamp); - - /** - * @brief Publishes a MQTT message received via ROS to the MQTT broker. - * - * @param serialized_msg generic serialized ROS message - * @param ros_topic ROS topic where the message was published - */ - void - ros2mqtt(const std::shared_ptr &serialized_msg, - const std::string &ros_topic); - - /** - * @brief Callback for when a MQTT action succeeds. - * - * Overrides mqtt::iaction_listener::on_success(const mqtt::token&). - * Does nothing. - * - * @param token token tracking the action - */ - void on_success(const mqtt::token &token) override; - - /** - * @brief Callback for when a MQTT action fails. - * - * Overrides mqtt::iaction_listener::on_failure(const mqtt::token&). - * Logs error. - * - * @param token token tracking the action - */ - void on_failure(const mqtt::token &token) override; - - /** - * @brief Status variable keeping track of connection status to broker - */ - bool is_connected_ = false; - - static inline std::string const moduleName = "mqtt_bridge"; - constexpr static std::chrono::milliseconds SEND_INTERVAL = - std::chrono::milliseconds(5000); - std::string brokerIP; - int port; - std::string clientId; - std::string username; - std::string password; - std::string topic_prefix; - - ROSChannels::CustomCommandAction::Sub - customCommandActionMsgSub; //!< Subscriber to v2x messages requests - ROSChannels::StateChange::Sub - obcStateChangeSub; //!< Subscriber to object state change requests - - void setupClient(); - void setupMqtt2RosBridge(); - void setupRos2MqttBridge(); - void onCustomCommandActionMsg( - const ROSChannels::CustomCommandAction::message_type::SharedPtr); - void - onObcStateChangeMsg(const ROSChannels::StateChange::message_type::SharedPtr); - - template - void onMessage(T msg, std::string mqtt_topic, - std::function convertFunc); - - static json v2xToJson(const std::string v2x_msg); - static json obcStateChangeToJson( - const ROSChannels::StateChange::message_type::SharedPtr obc_msg); -}; \ No newline at end of file + /** + * @brief MQTT2ROS connection variables sorted by MQTT topic + */ + std::map mqtt2ros_; + + /** + * @brief ROS2MQTT connection variables sorted by ROS topic + */ + std::map ros2mqtt_; + + /** @brief ROS Service server for providing dynamic MQTT to ROS mappings. + */ + rclcpp::Service::SharedPtr new_mqtt2ros_bridge_service_; + + /** @brief ROS Service server for providing dynamic ROS to MQTT mappings. + */ + rclcpp::Service::SharedPtr new_ros2mqtt_bridge_service_; + + /** + * @brief MQTT client variable + */ + std::shared_ptr client_; + + /** + * @brief MQTT client connection options + */ + mqtt::connect_options connect_options_; + + /** + * @brief Callback for when the client receives a MQTT message from the + * broker. + * + * Overrides mqtt::callback::message_arrived(mqtt::const_message_ptr). + * Publishes any empty ROS message to the corresponding ROS topic. + * + * @param mqtt_msg MQTT message + */ + void message_arrived(mqtt::const_message_ptr mqtt_msg) override; + + /** + * @brief Checks all active ROS topics in order to set up generic subscribers. + */ + void setupSubscriptions(); + + /** + * @brief Publishes a ROS message received via MQTT to ROS. + * + * @param mqtt_msg MQTT message + * @param arrival_stamp arrival timestamp used for latency computation + */ + void mqtt2ros(mqtt::const_message_ptr mqtt_msg, const rclcpp::Time& arrival_stamp); + + /** + * @brief Publishes a MQTT message received via ROS to the MQTT broker. + * + * @param serialized_msg generic serialized ROS message + * @param ros_topic ROS topic where the message was published + */ + void ros2mqtt(const std::shared_ptr& serialized_msg, const std::string& ros_topic); + + /** + * @brief Callback for when a MQTT action succeeds. + * + * Overrides mqtt::iaction_listener::on_success(const mqtt::token&). + * Does nothing. + * + * @param token token tracking the action + */ + void on_success(const mqtt::token& token) override; + + /** + * @brief Callback for when a MQTT action fails. + * + * Overrides mqtt::iaction_listener::on_failure(const mqtt::token&). + * Logs error. + * + * @param token token tracking the action + */ + void on_failure(const mqtt::token& token) override; + + /** + * @brief Status variable keeping track of connection status to broker + */ + bool is_connected_ = false; + + static inline std::string const moduleName = "mqtt_bridge"; + constexpr static std::chrono::milliseconds SEND_INTERVAL = std::chrono::milliseconds(5000); + std::string brokerIP; + int port; + std::string clientId; + std::string username; + std::string password; + std::string topic_prefix; + + ROSChannels::CustomCommandAction::Sub customCommandActionMsgSub; //!< Subscriber to v2x messages requests + ROSChannels::StateChange::Sub obcStateChangeSub; //!< Subscriber to object state change requests + + void setupClient(); + void setupMqtt2RosBridge(); + void setupRos2MqttBridge(); + void onCustomCommandActionMsg(const ROSChannels::CustomCommandAction::message_type::SharedPtr); + void onObcStateChangeMsg(const ROSChannels::StateChange::message_type::SharedPtr); + + template + void onMessage(T msg, std::string mqtt_topic, std::function convertFunc); + + static json v2xToJson(const std::string v2x_msg); + static json obcStateChangeToJson(const ROSChannels::StateChange::message_type::SharedPtr obc_msg); +}; diff --git a/atos/modules/MQTTBridge/src/main.cpp b/atos/modules/MQTTBridge/src/main.cpp index ef66d96ee..4ab08f92f 100644 --- a/atos/modules/MQTTBridge/src/main.cpp +++ b/atos/modules/MQTTBridge/src/main.cpp @@ -7,10 +7,10 @@ #include "mqttbridge.hpp" int main(int argc, char** argv) { - rclcpp::init(argc,argv); + rclcpp::init(argc, argv); auto mb = std::make_shared(); - if(rclcpp::ok()) { + if (rclcpp::ok()) { rclcpp::spin(mb); } rclcpp::shutdown(); diff --git a/atos/modules/MQTTBridge/src/mqttbridge.cpp b/atos/modules/MQTTBridge/src/mqttbridge.cpp index 782a0a71a..7edd331d5 100644 --- a/atos/modules/MQTTBridge/src/mqttbridge.cpp +++ b/atos/modules/MQTTBridge/src/mqttbridge.cpp @@ -10,140 +10,134 @@ using namespace ROSChannels; using std::placeholders::_1; -MqttBridge::MqttBridge() - : Module(MqttBridge::moduleName), - customCommandActionMsgSub( - *this, std::bind(&MqttBridge::onCustomCommandActionMsg, this, _1)), - obcStateChangeSub(*this, - std::bind(&MqttBridge::onObcStateChangeMsg, this, _1)) { - this->loadParameters(); - this->initialize(); +MqttBridge::MqttBridge() : + Module(MqttBridge::moduleName), + customCommandActionMsgSub(*this, std::bind(&MqttBridge::onCustomCommandActionMsg, this, _1)), + obcStateChangeSub(*this, std::bind(&MqttBridge::onObcStateChangeMsg, this, _1)) { + this->loadParameters(); + this->initialize(); } void MqttBridge::loadParameters() { - declare_parameter("broker.host", ""); - declare_parameter("broker.port", 1883); - declare_parameter("client.username", ""); - declare_parameter("client.password", ""); - declare_parameter("client.id", ""); - declare_parameter("topic_prefix", "atos"); - - get_parameter("broker.host", brokerIP); - get_parameter("broker.port", port); - get_parameter("client.username", username); - get_parameter("client.password", password); - get_parameter("client.id", clientId); - get_parameter("topic_prefix", topic_prefix); - - rcl_interfaces::msg::ParameterDescriptor param_desc; - param_desc.description = "The list of topics to bridge from MQTT to ROS"; - const auto mqtt2ros_mqtt_topics = declare_parameter>( - "mqtt2ros.mqtt_topics", std::vector(), param_desc); - for (const auto &mqtt_topic : mqtt2ros_mqtt_topics) { - param_desc.description = - "ROS topic on which corresponding MQTT messages are published"; - declare_parameter(fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), - rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "MQTT QoS value"; - declare_parameter(fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "ROS publisher queue size"; - declare_parameter( - fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - } - - param_desc.description = "The list of topics to bridge from ROS to MQTT"; - const auto ros2mqtt_ros_topics = declare_parameter>( - "ros2mqtt.ros_topics", std::vector(), param_desc); - for (const auto &ros_topic : ros2mqtt_ros_topics) { - param_desc.description = "MQTT topic on which the corresponding ROS " - "messages are sent to the broker"; - declare_parameter(fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), - rclcpp::ParameterType::PARAMETER_STRING, param_desc); - param_desc.description = "ROS subscriber queue size"; - declare_parameter( - fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - param_desc.description = "MQTT QoS value"; - declare_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), - rclcpp::ParameterType::PARAMETER_INTEGER, param_desc); - } - - // mqtt2ros - for (const auto &mqtt_topic : mqtt2ros_mqtt_topics) { - - rclcpp::Parameter ros_topic_param; - if (get_parameter(fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), - ros_topic_param)) { - - // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic - const std::string ros_topic = ros_topic_param.as_string(); - Mqtt2RosInterface &mqtt2ros = mqtt2ros_[mqtt_topic]; - mqtt2ros.ros.topic = ros_topic; - - // mqtt2ros[k]/advanced/mqtt/qos - rclcpp::Parameter qos_param; - if (get_parameter( - fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), - qos_param)) { - mqtt2ros.mqtt.qos = qos_param.as_int(); - } - - // mqtt2ros[k]/advanced/ros/queue_size - rclcpp::Parameter queue_size_param; - if (get_parameter( - fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), - queue_size_param)) { - mqtt2ros.ros.queue_size = queue_size_param.as_int(); - } - - RCLCPP_INFO(get_logger(), "Bridging MQTT topic '%s' to ROS topic '%s'", - mqtt_topic.c_str(), mqtt2ros.ros.topic.c_str()); - } else { - RCLCPP_WARN(get_logger(), - fmt::format("Parameter 'ros2mqtt.{}' is missing subparameter " - "'ros_topic', will be ignored", - mqtt_topic) - .c_str()); - } - } - - // ros2mqtt - for (const auto &ros_topic : ros2mqtt_ros_topics) { - - rclcpp::Parameter mqtt_topic_param; - if (get_parameter(fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), - mqtt_topic_param)) { - - // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic - const std::string mqtt_topic = mqtt_topic_param.as_string(); - Ros2MqttInterface &ros2mqtt = ros2mqtt_[ros_topic]; - ros2mqtt.mqtt.topic = mqtt_topic; - - // ros2mqtt[k]/advanced/ros/queue_size - rclcpp::Parameter queue_size_param; - if (get_parameter( - fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), - queue_size_param)) - ros2mqtt.ros.queue_size = queue_size_param.as_int(); - - // ros2mqtt[k]/advanced/mqtt/qos - rclcpp::Parameter qos_param; - if (get_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), - qos_param)) - ros2mqtt.mqtt.qos = qos_param.as_int(); - - RCLCPP_INFO(get_logger(), "Bridging ROS topic '%s' to MQTT topic '%s'", - ros_topic.c_str(), ros2mqtt.mqtt.topic.c_str()); - } else { - RCLCPP_WARN(get_logger(), - fmt::format("Parameter 'ros2mqtt.{}' is missing subparameter " - "'mqtt_topic', will be ignored", - ros_topic) - .c_str()); - } - } + declare_parameter("broker.host", ""); + declare_parameter("broker.port", 1883); + declare_parameter("client.username", ""); + declare_parameter("client.password", ""); + declare_parameter("client.id", ""); + declare_parameter("topic_prefix", "atos"); + + get_parameter("broker.host", brokerIP); + get_parameter("broker.port", port); + get_parameter("client.username", username); + get_parameter("client.password", password); + get_parameter("client.id", clientId); + get_parameter("topic_prefix", topic_prefix); + + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.description = "The list of topics to bridge from MQTT to ROS"; + const auto mqtt2ros_mqtt_topics = + declare_parameter>("mqtt2ros.mqtt_topics", std::vector(), param_desc); + for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) { + param_desc.description = "ROS topic on which corresponding MQTT messages are published"; + declare_parameter( + fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "MQTT QoS value"; + declare_parameter(fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, + param_desc); + param_desc.description = "ROS publisher queue size"; + declare_parameter(fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, + param_desc); + } + + param_desc.description = "The list of topics to bridge from ROS to MQTT"; + const auto ros2mqtt_ros_topics = + declare_parameter>("ros2mqtt.ros_topics", std::vector(), param_desc); + for (const auto& ros_topic : ros2mqtt_ros_topics) { + param_desc.description = "MQTT topic on which the corresponding ROS " + "messages are sent to the broker"; + declare_parameter( + fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), rclcpp::ParameterType::PARAMETER_STRING, param_desc); + param_desc.description = "ROS subscriber queue size"; + declare_parameter(fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, + param_desc); + param_desc.description = "MQTT QoS value"; + declare_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), + rclcpp::ParameterType::PARAMETER_INTEGER, + param_desc); + } + + // mqtt2ros + for (const auto& mqtt_topic : mqtt2ros_mqtt_topics) { + + rclcpp::Parameter ros_topic_param; + if (get_parameter(fmt::format("mqtt2ros.{}.ros_topic", mqtt_topic), ros_topic_param)) { + + // mqtt2ros[k]/mqtt_topic and mqtt2ros[k]/ros_topic + const std::string ros_topic = ros_topic_param.as_string(); + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; + mqtt2ros.ros.topic = ros_topic; + + // mqtt2ros[k]/advanced/mqtt/qos + rclcpp::Parameter qos_param; + if (get_parameter(fmt::format("mqtt2ros.{}.advanced.mqtt.qos", mqtt_topic), qos_param)) { + mqtt2ros.mqtt.qos = qos_param.as_int(); + } + + // mqtt2ros[k]/advanced/ros/queue_size + rclcpp::Parameter queue_size_param; + if (get_parameter(fmt::format("mqtt2ros.{}.advanced.ros.queue_size", mqtt_topic), queue_size_param)) { + mqtt2ros.ros.queue_size = queue_size_param.as_int(); + } + + RCLCPP_INFO(get_logger(), + "Bridging MQTT topic '%s' to ROS topic '%s'", + mqtt_topic.c_str(), + mqtt2ros.ros.topic.c_str()); + } else { + RCLCPP_WARN(get_logger(), + fmt::format("Parameter 'ros2mqtt.{}' is missing subparameter " + "'ros_topic', will be ignored", + mqtt_topic) + .c_str()); + } + } + + // ros2mqtt + for (const auto& ros_topic : ros2mqtt_ros_topics) { + + rclcpp::Parameter mqtt_topic_param; + if (get_parameter(fmt::format("ros2mqtt.{}.mqtt_topic", ros_topic), mqtt_topic_param)) { + + // ros2mqtt[k]/ros_topic and ros2mqtt[k]/mqtt_topic + const std::string mqtt_topic = mqtt_topic_param.as_string(); + Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; + ros2mqtt.mqtt.topic = mqtt_topic; + + // ros2mqtt[k]/advanced/ros/queue_size + rclcpp::Parameter queue_size_param; + if (get_parameter(fmt::format("ros2mqtt.{}.advanced.ros.queue_size", ros_topic), queue_size_param)) + ros2mqtt.ros.queue_size = queue_size_param.as_int(); + + // ros2mqtt[k]/advanced/mqtt/qos + rclcpp::Parameter qos_param; + if (get_parameter(fmt::format("ros2mqtt.{}.advanced.mqtt.qos", ros_topic), qos_param)) + ros2mqtt.mqtt.qos = qos_param.as_int(); + + RCLCPP_INFO(get_logger(), + "Bridging ROS topic '%s' to MQTT topic '%s'", + ros_topic.c_str(), + ros2mqtt.mqtt.topic.c_str()); + } else { + RCLCPP_WARN(get_logger(), + fmt::format("Parameter 'ros2mqtt.{}' is missing subparameter " + "'mqtt_topic', will be ignored", + ros_topic) + .c_str()); + } + } } /*! @@ -151,322 +145,293 @@ void MqttBridge::loadParameters() { * with ros parameter settings. */ void MqttBridge::initialize() { - if (this->brokerIP.empty()) { - RCLCPP_INFO(this->get_logger(), - "No Broker IP provided in configuration. Shutting down..."); - rclcpp::shutdown(); - } else { - this->setupClient(); - this->connect(); - // Wait for all ros topics to be announced - rclcpp::sleep_for(std::chrono::seconds(1)); - this->setupMqtt2RosBridge(); - this->setupRos2MqttBridge(); - } + if (this->brokerIP.empty()) { + RCLCPP_INFO(this->get_logger(), "No Broker IP provided in configuration. Shutting down..."); + rclcpp::shutdown(); + } else { + this->setupClient(); + this->connect(); + // Wait for all ros topics to be announced + rclcpp::sleep_for(std::chrono::seconds(1)); + this->setupMqtt2RosBridge(); + this->setupRos2MqttBridge(); + } } void MqttBridge::setupMqtt2RosBridge() { - new_mqtt2ros_bridge_service_ = - create_service( - "~/new_mqtt2ros_bridge", - std::bind(&MqttBridge::newMqtt2RosBridge, this, std::placeholders::_1, - std::placeholders::_2)); - - // Create a request and response for each mqtt2ros bridge and put into a - // map - auto serviceCallMap = std::map< - std::shared_ptr, - std::shared_ptr>(); - for (const auto &mqtt2ros : mqtt2ros_) { - atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request = - std::make_shared(); - request->mqtt_topic = mqtt2ros.first; - request->ros_topic = mqtt2ros.second.ros.topic; - request->mqtt_qos = mqtt2ros.second.mqtt.qos; - request->ros_queue_size = mqtt2ros.second.ros.queue_size; - atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response = - std::make_shared(); - serviceCallMap[response] = request; - } - - // Loop through the map until all response objects are true - while (std::any_of(serviceCallMap.begin(), serviceCallMap.end(), - [](const auto &pair) { return !pair.first->success; })) { - // Get all unsuccesful responses - auto failedResponses = std::vector< - std::shared_ptr>(); - for (const auto &pair : serviceCallMap) { - if (!pair.first->success) { - failedResponses.push_back(pair.first); - } - } - // Retry all failed responses - for (const auto &response : failedResponses) { - newMqtt2RosBridge(serviceCallMap[response], response); - } - // Wait for a second before retrying - rclcpp::sleep_for(std::chrono::seconds(1)); - } + new_mqtt2ros_bridge_service_ = create_service( + "~/new_mqtt2ros_bridge", + std::bind(&MqttBridge::newMqtt2RosBridge, this, std::placeholders::_1, std::placeholders::_2)); + + // Create a request and response for each mqtt2ros bridge and put into a + // map + auto serviceCallMap = std::map, + std::shared_ptr>(); + for (const auto& mqtt2ros : mqtt2ros_) { + atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request = + std::make_shared(); + request->mqtt_topic = mqtt2ros.first; + request->ros_topic = mqtt2ros.second.ros.topic; + request->mqtt_qos = mqtt2ros.second.mqtt.qos; + request->ros_queue_size = mqtt2ros.second.ros.queue_size; + atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response = + std::make_shared(); + serviceCallMap[response] = request; + } + + // Loop through the map until all response objects are true + while (std::any_of( + serviceCallMap.begin(), serviceCallMap.end(), [](const auto& pair) { return !pair.first->success; })) { + // Get all unsuccesful responses + auto failedResponses = std::vector>(); + for (const auto& pair : serviceCallMap) { + if (!pair.first->success) { + failedResponses.push_back(pair.first); + } + } + // Retry all failed responses + for (const auto& response : failedResponses) { + newMqtt2RosBridge(serviceCallMap[response], response); + } + // Wait for a second before retrying + rclcpp::sleep_for(std::chrono::seconds(1)); + } } void MqttBridge::setupRos2MqttBridge() { - new_ros2mqtt_bridge_service_ = - create_service( - "~/new_ros2mqtt_bridge", - std::bind(&MqttBridge::newRos2MqttBridge, this, std::placeholders::_1, - std::placeholders::_2)); - - // setup subscribers - this->setupSubscriptions(); + new_ros2mqtt_bridge_service_ = create_service( + "~/new_ros2mqtt_bridge", + std::bind(&MqttBridge::newRos2MqttBridge, this, std::placeholders::_1, std::placeholders::_2)); + + // setup subscribers + this->setupSubscriptions(); } void MqttBridge::setupSubscriptions() { - // get info of all topics - const auto all_topics_and_types = get_topic_names_and_types(); - // check for ros2mqtt topics - for (auto &[ros_topic, ros2mqtt] : ros2mqtt_) { - if (all_topics_and_types.count(ros_topic)) { - // check if message type has changed or if mapping is stale - const std::string &msg_type = all_topics_and_types.at(ros_topic)[0]; - if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale) - continue; - ros2mqtt.ros.is_stale = false; - ros2mqtt.ros.msg_type = msg_type; - - // create new generic subscription, if message type has changed - std::function msg)> - bound_callback_func = std::bind(&MqttBridge::ros2mqtt, this, - std::placeholders::_1, ros_topic); - try { - ros2mqtt.ros.subscriber = create_generic_subscription( - ros_topic, msg_type, ros2mqtt.ros.queue_size, bound_callback_func); - } catch (rclcpp::exceptions::RCLError &e) { - RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", - e.what()); - return; - } - RCLCPP_INFO(get_logger(), "Subscribed to ROS topic '%s' of type '%s'", - ros_topic.c_str(), msg_type.c_str()); - } - } + // get info of all topics + const auto all_topics_and_types = get_topic_names_and_types(); + // check for ros2mqtt topics + for (auto& [ros_topic, ros2mqtt] : ros2mqtt_) { + if (all_topics_and_types.count(ros_topic)) { + // check if message type has changed or if mapping is stale + const std::string& msg_type = all_topics_and_types.at(ros_topic)[0]; + if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale) + continue; + ros2mqtt.ros.is_stale = false; + ros2mqtt.ros.msg_type = msg_type; + + // create new generic subscription, if message type has changed + std::function msg)> bound_callback_func = + std::bind(&MqttBridge::ros2mqtt, this, std::placeholders::_1, ros_topic); + try { + ros2mqtt.ros.subscriber = + create_generic_subscription(ros_topic, msg_type, ros2mqtt.ros.queue_size, bound_callback_func); + } catch (rclcpp::exceptions::RCLError& e) { + RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s", e.what()); + return; + } + RCLCPP_INFO(get_logger(), "Subscribed to ROS topic '%s' of type '%s'", ros_topic.c_str(), msg_type.c_str()); + } + } } -void MqttBridge::ros2mqtt( - const std::shared_ptr &serialized_msg, - const std::string &ros_topic) { - - Ros2MqttInterface &ros2mqtt = ros2mqtt_[ros_topic]; - std::string mqtt_topic = ros2mqtt.mqtt.topic; - std::vector payload_buffer; - - RCLCPP_INFO(get_logger(), "Received ROS message on topic '%s'", - ros_topic.c_str()); - - try { - mqtt::message_ptr mqtt_msg = - mqtt::make_message(mqtt_topic, payload_buffer.data(), - payload_buffer.size(), ros2mqtt.mqtt.qos, true); - client_->publish(mqtt_msg); - RCLCPP_INFO(get_logger(), "Published ROS message to MQTT topic '%s'", - mqtt_topic.c_str()); - } catch (const mqtt::exception &e) { - RCLCPP_WARN( - get_logger(), - "Publishing ROS message type information to MQTT topic '%s' failed: %s", - mqtt_topic.c_str(), e.what()); - } +void MqttBridge::ros2mqtt(const std::shared_ptr& serialized_msg, + const std::string& ros_topic) { + + Ros2MqttInterface& ros2mqtt = ros2mqtt_[ros_topic]; + std::string mqtt_topic = ros2mqtt.mqtt.topic; + std::vector payload_buffer; + + RCLCPP_INFO(get_logger(), "Received ROS message on topic '%s'", ros_topic.c_str()); + + try { + mqtt::message_ptr mqtt_msg = + mqtt::make_message(mqtt_topic, payload_buffer.data(), payload_buffer.size(), ros2mqtt.mqtt.qos, true); + client_->publish(mqtt_msg); + RCLCPP_INFO(get_logger(), "Published ROS message to MQTT topic '%s'", mqtt_topic.c_str()); + } catch (const mqtt::exception& e) { + RCLCPP_WARN(get_logger(), + "Publishing ROS message type information to MQTT topic '%s' failed: %s", + mqtt_topic.c_str(), + e.what()); + } } void MqttBridge::setupClient() { - RCLCPP_INFO(this->get_logger(), - "Setting up connection with clientID: %s, and broker IP: %s", - clientId.c_str(), brokerIP.c_str()); - - // basic client connection options - connect_options_.set_automatic_reconnect(true); - connect_options_.set_clean_session(true); - connect_options_.set_keep_alive_interval(5); - - // user authentication - if (!username.empty()) { - connect_options_.set_user_name(username); - connect_options_.set_password(password); - } - - auto timeNanoseconds = - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - .count(); - auto id = clientId + "_" + std::to_string(timeNanoseconds); - - const std::string uri = fmt::format("{}://{}:{}", "tcp", brokerIP, port); - try { - client_ = - std::shared_ptr(new mqtt::async_client(uri, id)); - } catch (const mqtt::exception &e) { - RCLCPP_ERROR(get_logger(), "Client could not be initialized: %s", e.what()); - exit(EXIT_FAILURE); - } - - // setup MQTT callbacks - client_->set_callback(*this); + RCLCPP_INFO(this->get_logger(), + "Setting up connection with clientID: %s, and broker IP: %s", + clientId.c_str(), + brokerIP.c_str()); + + // basic client connection options + connect_options_.set_automatic_reconnect(true); + connect_options_.set_clean_session(true); + connect_options_.set_keep_alive_interval(5); + + // user authentication + if (!username.empty()) { + connect_options_.set_user_name(username); + connect_options_.set_password(password); + } + + auto timeNanoseconds = + std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + auto id = clientId + "_" + std::to_string(timeNanoseconds); + + const std::string uri = fmt::format("{}://{}:{}", "tcp", brokerIP, port); + try { + client_ = std::shared_ptr(new mqtt::async_client(uri, id)); + } catch (const mqtt::exception& e) { + RCLCPP_ERROR(get_logger(), "Client could not be initialized: %s", e.what()); + exit(EXIT_FAILURE); + } + + // setup MQTT callbacks + client_->set_callback(*this); } void MqttBridge::connect() { - std::string as_client = - clientId.empty() ? "" - : std::string(" as '") + clientId + std::string("'"); - RCLCPP_INFO(get_logger(), "Connecting to broker at '%s'%s ...", - client_->get_server_uri().c_str(), as_client.c_str()); - - try { - client_->connect(connect_options_, nullptr, *this); - } catch (const mqtt::exception &e) { - RCLCPP_ERROR(get_logger(), "Connection to broker failed: %s", e.what()); - exit(EXIT_FAILURE); - } + std::string as_client = clientId.empty() ? "" : std::string(" as '") + clientId + std::string("'"); + RCLCPP_INFO( + get_logger(), "Connecting to broker at '%s'%s ...", client_->get_server_uri().c_str(), as_client.c_str()); + + try { + client_->connect(connect_options_, nullptr, *this); + } catch (const mqtt::exception& e) { + RCLCPP_ERROR(get_logger(), "Connection to broker failed: %s", e.what()); + exit(EXIT_FAILURE); + } } -void MqttBridge::newMqtt2RosBridge( - atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, - atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response) { - - // add mapping definition to mqtt2ros_ - Mqtt2RosInterface &mqtt2ros = mqtt2ros_[request->mqtt_topic]; - mqtt2ros.ros.is_stale = true; - mqtt2ros.ros.topic = request->ros_topic; - mqtt2ros.mqtt.qos = request->mqtt_qos; - mqtt2ros.ros.publisher = this->create_publisher( - mqtt2ros.ros.topic, request->ros_queue_size); - mqtt2ros.ros.queue_size = request->ros_queue_size; - - RCLCPP_DEBUG(get_logger(), "Bridging MQTT topic '%s' to ROS topic '%s'", - request->mqtt_topic.c_str(), mqtt2ros.ros.topic.c_str()); - - // subscribe to the MQTT topic - std::string mqtt_topic_to_subscribe = request->mqtt_topic; - try { - client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); - RCLCPP_INFO(get_logger(), "Subscribed to MQTT topic '%s'", - mqtt_topic_to_subscribe.c_str()); - response->success = true; - } - // Catch exception if the topic is already subscribed - catch (const mqtt::exception &e) { - RCLCPP_WARN(get_logger(), "Failed to subscribe MQTT topic '%s': %s", - mqtt_topic_to_subscribe.c_str(), e.what()); - response->success = false; - } +void MqttBridge::newMqtt2RosBridge(atos_interfaces::srv::NewMqtt2RosBridge::Request::SharedPtr request, + atos_interfaces::srv::NewMqtt2RosBridge::Response::SharedPtr response) { + + // add mapping definition to mqtt2ros_ + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[request->mqtt_topic]; + mqtt2ros.ros.is_stale = true; + mqtt2ros.ros.topic = request->ros_topic; + mqtt2ros.mqtt.qos = request->mqtt_qos; + mqtt2ros.ros.publisher = this->create_publisher(mqtt2ros.ros.topic, request->ros_queue_size); + mqtt2ros.ros.queue_size = request->ros_queue_size; + + RCLCPP_DEBUG(get_logger(), + "Bridging MQTT topic '%s' to ROS topic '%s'", + request->mqtt_topic.c_str(), + mqtt2ros.ros.topic.c_str()); + + // subscribe to the MQTT topic + std::string mqtt_topic_to_subscribe = request->mqtt_topic; + try { + client_->subscribe(mqtt_topic_to_subscribe, mqtt2ros.mqtt.qos); + RCLCPP_INFO(get_logger(), "Subscribed to MQTT topic '%s'", mqtt_topic_to_subscribe.c_str()); + response->success = true; + } + // Catch exception if the topic is already subscribed + catch (const mqtt::exception& e) { + RCLCPP_WARN(get_logger(), "Failed to subscribe MQTT topic '%s': %s", mqtt_topic_to_subscribe.c_str(), e.what()); + response->success = false; + } } -void MqttBridge::newRos2MqttBridge( - atos_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, - atos_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response) { +void MqttBridge::newRos2MqttBridge(atos_interfaces::srv::NewRos2MqttBridge::Request::SharedPtr request, + atos_interfaces::srv::NewRos2MqttBridge::Response::SharedPtr response) { - // add mapping definition to ros2mqtt_ - Ros2MqttInterface &ros2mqtt = ros2mqtt_[request->ros_topic]; - ros2mqtt.ros.is_stale = true; - ros2mqtt.ros.topic = request->ros_topic; - ros2mqtt.mqtt.topic = request->mqtt_topic; - ros2mqtt.mqtt.qos = request->mqtt_qos; + // add mapping definition to ros2mqtt_ + Ros2MqttInterface& ros2mqtt = ros2mqtt_[request->ros_topic]; + ros2mqtt.ros.is_stale = true; + ros2mqtt.ros.topic = request->ros_topic; + ros2mqtt.mqtt.topic = request->mqtt_topic; + ros2mqtt.mqtt.qos = request->mqtt_qos; - RCLCPP_DEBUG(get_logger(), "Bridging ROS topic '%s' to MQTT topic '%s'", - ros2mqtt.ros.topic.c_str(), ros2mqtt.mqtt.topic.c_str()); + RCLCPP_DEBUG(get_logger(), + "Bridging ROS topic '%s' to MQTT topic '%s'", + ros2mqtt.ros.topic.c_str(), + ros2mqtt.mqtt.topic.c_str()); - // setup ROS subscriptions - setupSubscriptions(); + // setup ROS subscriptions + setupSubscriptions(); - response->success = true; + response->success = true; } void MqttBridge::onCustomCommandActionMsg( - const CustomCommandAction::message_type::SharedPtr - custom_command_action_msg) { - if (custom_command_action_msg->type == "v2x") { - RCLCPP_INFO(this->get_logger(), "Received V2X message on %s topic", - CustomCommandAction::topicName.c_str()); - this->onMessage(custom_command_action_msg->content, "v2x", - v2xToJson); - } + const CustomCommandAction::message_type::SharedPtr custom_command_action_msg) { + if (custom_command_action_msg->type == "v2x") { + RCLCPP_INFO(this->get_logger(), "Received V2X message on %s topic", CustomCommandAction::topicName.c_str()); + this->onMessage(custom_command_action_msg->content, "v2x", v2xToJson); + } } -void MqttBridge::onObcStateChangeMsg( - const StateChange::message_type::SharedPtr obc_msg) { - this->onMessage(obc_msg, "state", - obcStateChangeToJson); +void MqttBridge::onObcStateChangeMsg(const StateChange::message_type::SharedPtr obc_msg) { + this->onMessage(obc_msg, "state", obcStateChangeToJson); } void MqttBridge::message_arrived(mqtt::const_message_ptr mqtt_msg) { - std::string mqtt_topic = mqtt_msg->get_topic(); - RCLCPP_DEBUG(get_logger(), "Received MQTT message on topic '%s'", - mqtt_topic.c_str()); - Mqtt2RosInterface &mqtt2ros = mqtt2ros_[mqtt_topic]; - - // Publish empty message to ROS topic - mqtt2ros.ros.publisher->publish(std_msgs::msg::Empty()); - RCLCPP_DEBUG(get_logger(), "Published empty message to ROS topic '%s'", - mqtt2ros.ros.topic.c_str()); + std::string mqtt_topic = mqtt_msg->get_topic(); + RCLCPP_DEBUG(get_logger(), "Received MQTT message on topic '%s'", mqtt_topic.c_str()); + Mqtt2RosInterface& mqtt2ros = mqtt2ros_[mqtt_topic]; + + // Publish empty message to ROS topic + mqtt2ros.ros.publisher->publish(std_msgs::msg::Empty()); + RCLCPP_DEBUG(get_logger(), "Published empty message to ROS topic '%s'", mqtt2ros.ros.topic.c_str()); } -template -void MqttBridge::onMessage(T msg, std::string mqtt_topic, - std::function convertFunc) { - json payload = convertFunc(msg); - try { - RCLCPP_DEBUG(this->get_logger(), "Publishing MQTT msg to broker %s", - payload.dump().c_str()); - client_->publish(mqtt_topic, payload.dump().c_str(), payload.dump().size()); - } catch (std::runtime_error &) { - RCLCPP_ERROR(this->get_logger(), "Failed to publish MQTT message"); - } +template +void MqttBridge::onMessage(T msg, std::string mqtt_topic, std::function convertFunc) { + json payload = convertFunc(msg); + try { + RCLCPP_DEBUG(this->get_logger(), "Publishing MQTT msg to broker %s", payload.dump().c_str()); + client_->publish(mqtt_topic, payload.dump().c_str(), payload.dump().size()); + } catch (std::runtime_error&) { + RCLCPP_ERROR(this->get_logger(), "Failed to publish MQTT message"); + } } json MqttBridge::v2xToJson(std::string msg_content) { - std::replace(msg_content.begin(), msg_content.end(), '\'', - '"'); // Replace ROS single quotes string with double quotes to - // make it a valid JSON string - json j; - // Parse the string to a json object - try { - j = json::parse(msg_content); - } catch (json::parse_error &e) { - std::cerr << "Failed to parse JSON object: " << e.what() << std::endl; - } - - if (j.find("message_type") == j.end()) { - std::cerr << "No message_type field in JSON object" << std::endl; - return j; - } - if (j["message_type"] == "DENM") { - j["detection_time"] = - std::chrono::system_clock::now().time_since_epoch().count(); - } - return j; + std::replace(msg_content.begin(), + msg_content.end(), + '\'', + '"'); // Replace ROS single quotes string with double quotes to + // make it a valid JSON string + json j; + // Parse the string to a json object + try { + j = json::parse(msg_content); + } catch (json::parse_error& e) { + std::cerr << "Failed to parse JSON object: " << e.what() << std::endl; + } + + if (j.find("message_type") == j.end()) { + std::cerr << "No message_type field in JSON object" << std::endl; + return j; + } + if (j["message_type"] == "DENM") { + j["detection_time"] = std::chrono::system_clock::now().time_since_epoch().count(); + } + return j; } -json MqttBridge::obcStateChangeToJson( - const StateChange::message_type::SharedPtr obc_msg) { - json j; - j["current_state"] = obc_msg->current_state; - j["prev_state"] = obc_msg->prev_state; - return j; +json MqttBridge::obcStateChangeToJson(const StateChange::message_type::SharedPtr obc_msg) { + json j; + j["current_state"] = obc_msg->current_state; + j["prev_state"] = obc_msg->prev_state; + return j; } -void MqttBridge::on_success(const mqtt::token &token) { +void MqttBridge::on_success(const mqtt::token& token) { - (void)token; // Avoid compiler warning for unused parameter. - RCLCPP_INFO(get_logger(), "Connected to broker successfully"); - is_connected_ = true; + (void)token; // Avoid compiler warning for unused parameter. + RCLCPP_INFO(get_logger(), "Connected to broker successfully"); + is_connected_ = true; } -void MqttBridge::on_failure(const mqtt::token &token) { +void MqttBridge::on_failure(const mqtt::token& token) { - RCLCPP_ERROR( - get_logger(), - "Connection to broker failed (return code %d), will automatically " - "retry...", - token.get_return_code()); - is_connected_ = false; -} \ No newline at end of file + RCLCPP_ERROR(get_logger(), + "Connection to broker failed (return code %d), will automatically " + "retry...", + token.get_return_code()); + is_connected_ = false; +} diff --git a/atos/modules/OSIAdapter/inc/osiadapter.hpp b/atos/modules/OSIAdapter/inc/osiadapter.hpp index 129c7e5be..9ba78e79d 100644 --- a/atos/modules/OSIAdapter/inc/osiadapter.hpp +++ b/atos/modules/OSIAdapter/inc/osiadapter.hpp @@ -6,45 +6,43 @@ #pragma once #include "module.hpp" +#include "osi_handler.hpp" #include "roschannels/commandchannels.hpp" #include "roschannels/monitorchannel.hpp" -#include "osi_handler.hpp" -#include "unordered_map" #include "serverfactory.hpp" +#include "unordered_map" #include -class OSIAdapter : public Module -{ - public: - void initializeServer(); - OSIAdapter(); - ~OSIAdapter(); - - - private: - using TimeUnit = std::chrono::milliseconds; - std::string address; - uint16_t port; - std::string protocol; - uint16_t frequency; - static inline std::string const moduleName = "osi_adapter"; - - void getParameters(); - void sendOSIData(); - std::vector makeOSIMessage(const std::vector& osiData); - const OsiHandler::GlobalObjectGroundTruth_t makeOSIData(ROSChannels::Monitor::message_type& monr); - - std::unique_ptr server; - rclcpp::TimerBase::SharedPtr timer; - ROSChannels::ConnectedObjectIds::Sub connectedObjectIdsSub; //!< Publisher to report connected objects - - std::unordered_map lastMonitors; - std::unordered_map lastMonitorTimes; - std::unordered_map> monrSubscribers; - - void onConnectedObjectIdsMessage(const ROSChannels::ConnectedObjectIds::message_type::SharedPtr msg); - void onMonitorMessage(const ROSChannels::Monitor::message_type::SharedPtr msg, uint32_t id); - - inline double linPosPrediction(const double position, const double velocity, const TimeUnit dt); - void extrapolateMONR(ROSChannels::Monitor::message_type& monr, const TimeUnit dt); +class OSIAdapter : public Module { +public: + void initializeServer(); + OSIAdapter(); + ~OSIAdapter(); + +private: + using TimeUnit = std::chrono::milliseconds; + std::string address; + uint16_t port; + std::string protocol; + uint16_t frequency; + static inline std::string const moduleName = "osi_adapter"; + + void getParameters(); + void sendOSIData(); + std::vector makeOSIMessage(const std::vector& osiData); + const OsiHandler::GlobalObjectGroundTruth_t makeOSIData(ROSChannels::Monitor::message_type& monr); + + std::unique_ptr server; + rclcpp::TimerBase::SharedPtr timer; + ROSChannels::ConnectedObjectIds::Sub connectedObjectIdsSub; //!< Publisher to report connected objects + + std::unordered_map lastMonitors; + std::unordered_map lastMonitorTimes; + std::unordered_map> monrSubscribers; + + void onConnectedObjectIdsMessage(const ROSChannels::ConnectedObjectIds::message_type::SharedPtr msg); + void onMonitorMessage(const ROSChannels::Monitor::message_type::SharedPtr msg, uint32_t id); + + inline double linPosPrediction(const double position, const double velocity, const TimeUnit dt); + void extrapolateMONR(ROSChannels::Monitor::message_type& monr, const TimeUnit dt); }; diff --git a/atos/modules/OSIAdapter/inc/serverfactory.hpp b/atos/modules/OSIAdapter/inc/serverfactory.hpp index db36b1c80..e25f59012 100644 --- a/atos/modules/OSIAdapter/inc/serverfactory.hpp +++ b/atos/modules/OSIAdapter/inc/serverfactory.hpp @@ -5,29 +5,27 @@ */ #pragma once -#include #include "server.hpp" - +#include class ServerFactory { - public: - ServerFactory(const std::string& address, const uint16_t& port, const std::string protocol); - ~ServerFactory(); - - void createServer(); - void setupServer(); - void destroyServer(); - void resetServer(); - void sendData(const std::vector& data); +public: + ServerFactory(const std::string& address, const uint16_t& port, const std::string protocol); + ~ServerFactory(); + void createServer(); + void setupServer(); + void destroyServer(); + void resetServer(); + void sendData(const std::vector& data); - private: - std::string address; - uint16_t port; - std::string protocol; - std::unique_ptr tcpServer; - std::unique_ptr udpServer; - std::shared_ptr socket; - BasicSocket::HostInfo hostInfo; +private: + std::string address; + uint16_t port; + std::string protocol; + std::unique_ptr tcpServer; + std::unique_ptr udpServer; + std::shared_ptr socket; + BasicSocket::HostInfo hostInfo; }; diff --git a/atos/modules/OSIAdapter/src/main.cpp b/atos/modules/OSIAdapter/src/main.cpp index b60a8dd25..b3344e2bc 100644 --- a/atos/modules/OSIAdapter/src/main.cpp +++ b/atos/modules/OSIAdapter/src/main.cpp @@ -3,16 +3,15 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include #include "osiadapter.hpp" - +#include int main(int argc, char** argv) { - rclcpp::init(argc, argv); - auto OSIAdapterNode = std::make_shared(); - rclcpp::spin(OSIAdapterNode); - rclcpp::shutdown(); + rclcpp::init(argc, argv); + auto OSIAdapterNode = std::make_shared(); + rclcpp::spin(OSIAdapterNode); + rclcpp::shutdown(); - return 0; -} \ No newline at end of file + return 0; +} diff --git a/atos/modules/OSIAdapter/src/osiadapter.cpp b/atos/modules/OSIAdapter/src/osiadapter.cpp index 1feffbcb7..263841e5d 100644 --- a/atos/modules/OSIAdapter/src/osiadapter.cpp +++ b/atos/modules/OSIAdapter/src/osiadapter.cpp @@ -3,195 +3,183 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include -#include #include +#include +#include #include "osi_handler.hpp" #include "osiadapter.hpp" - using namespace ROSChannels; using namespace std::chrono; using std::placeholders::_1; - /** * @brief Module for sending OSI-data from MONR-messages over a server. - * + * */ OSIAdapter::OSIAdapter() : Module(OSIAdapter::moduleName), - connectedObjectIdsSub(*this,std::bind(&OSIAdapter::onConnectedObjectIdsMessage, this, _1)) - { - getParameters(); - initializeServer(); - - std::chrono::duration sendInterval = std::chrono::milliseconds(1000 / frequency); - timer = this->create_wall_timer(std::chrono::milliseconds(sendInterval), std::bind(&OSIAdapter::sendOSIData, this)); - }; + connectedObjectIdsSub(*this, std::bind(&OSIAdapter::onConnectedObjectIdsMessage, this, _1)) { + getParameters(); + initializeServer(); + std::chrono::duration sendInterval = std::chrono::milliseconds(1000 / frequency); + timer = this->create_wall_timer(std::chrono::milliseconds(sendInterval), std::bind(&OSIAdapter::sendOSIData, this)); +}; /** * @brief Destroy the OSIAdapter::OSIAdapter object. - * + * */ OSIAdapter::~OSIAdapter() { - server->destroyServer(); - } - + server->destroyServer(); +} /** * @brief Get parameters from parameter file. - * + * */ void OSIAdapter::getParameters() { - declare_parameter("address","0.0.0.0"); - declare_parameter("port",10); - declare_parameter("protocol","tcp"); - declare_parameter("frequency",10); - - get_parameter("address", address); - get_parameter("port", port); - get_parameter("protocol", protocol); - get_parameter("frequency", frequency); - + declare_parameter("address", "0.0.0.0"); + declare_parameter("port", 10); + declare_parameter("protocol", "tcp"); + declare_parameter("frequency", 10); + + get_parameter("address", address); + get_parameter("port", port); + get_parameter("protocol", protocol); + get_parameter("frequency", frequency); } - /** * @brief Initializes a server, using either TCP or UDP. - * + * * @param address IP-address * @param port Port */ void OSIAdapter::initializeServer() { - server = std::make_unique(address, port, protocol); - server->createServer(); - server->setupServer(); + server = std::make_unique(address, port, protocol); + server->createServer(); + server->setupServer(); } - /** * @brief Send OSI-data to the receiver at other end of socket. * writing to a socket that has been closed by the other end will result in error code 32 (broken pipe), * therefore, the connection is reset and OSIAdapter waits until a client connects again. - * + * */ void OSIAdapter::sendOSIData() { - // Extrapolate monr data and create a sensorView containing the objects - std::for_each(lastMonitors.begin(),lastMonitors.end(),[&](auto pair){ OSIAdapter::extrapolateMONR(pair.second,lastMonitorTimes.at(pair.first));}); - std::vector sensorView(lastMonitors.size()); - std::transform(lastMonitors.begin(),lastMonitors.end(), sensorView.begin(), [&](auto pair) {return OSIAdapter::makeOSIData(pair.second);}); - - std::vector data = OSIAdapter::makeOSIMessage(sensorView); - - try { - server->sendData(data); - } - catch (const SocketErrors::DisconnectedError& e) { - RCLCPP_INFO(get_logger(), "Client disconnected: %s. Restarting server", e.what()); - server->resetServer(); - } - catch (const SocketErrors::SocketSendError& e) { - RCLCPP_INFO(get_logger(), "Client disconnected: %s. Restarting server" , e.what()); - server->resetServer(); - } + // Extrapolate monr data and create a sensorView containing the objects + std::for_each(lastMonitors.begin(), lastMonitors.end(), [&](auto pair) { + OSIAdapter::extrapolateMONR(pair.second, lastMonitorTimes.at(pair.first)); + }); + std::vector sensorView(lastMonitors.size()); + std::transform(lastMonitors.begin(), lastMonitors.end(), sensorView.begin(), [&](auto pair) { + return OSIAdapter::makeOSIData(pair.second); + }); + + std::vector data = OSIAdapter::makeOSIMessage(sensorView); + + try { + server->sendData(data); + } catch (const SocketErrors::DisconnectedError& e) { + RCLCPP_INFO(get_logger(), "Client disconnected: %s. Restarting server", e.what()); + server->resetServer(); + } catch (const SocketErrors::SocketSendError& e) { + RCLCPP_INFO(get_logger(), "Client disconnected: %s. Restarting server", e.what()); + server->resetServer(); + } } - /** * @brief Encodes SvGt message and returns vector to use for sending message in socket. - * + * * @param osiData OSI-data * @return std::vector Vector from SvGt encoding */ std::vector OSIAdapter::makeOSIMessage(const std::vector& osiData) { - OsiHandler osi; - std::chrono::system_clock::time_point timestamp = std::chrono::system_clock::now(); - auto projStr = ""; - auto rawData = osi.encodeSvGtMessage(osiData, timestamp, projStr, false); + OsiHandler osi; + std::chrono::system_clock::time_point timestamp = std::chrono::system_clock::now(); + auto projStr = ""; + auto rawData = osi.encodeSvGtMessage(osiData, timestamp, projStr, false); - std::vector vec(rawData.length()); - std::copy(rawData.begin(), rawData.end(), vec.begin()); - - return vec; -} + std::vector vec(rawData.length()); + std::copy(rawData.begin(), rawData.end(), vec.begin()); + return vec; +} /** * @brief Create OSI-data from MONR. - * - * @return const OsiHandler::GlobalObjectGroundTruth_t OSI-data + * + * @return const OsiHandler::GlobalObjectGroundTruth_t OSI-data */ const OsiHandler::GlobalObjectGroundTruth_t OSIAdapter::makeOSIData(ROSChannels::Monitor::message_type& monr) { - - OsiHandler::GlobalObjectGroundTruth_t osiData; - osiData.id = monr.atos_header.object_id; - osiData.pos_m.x = monr.pose.pose.position.x; - osiData.pos_m.y = monr.pose.pose.position.y; - osiData.pos_m.z = monr.pose.pose.position.z; + OsiHandler::GlobalObjectGroundTruth_t osiData; + osiData.id = monr.atos_header.object_id; - osiData.acc_m_s2.x = monr.acceleration.accel.linear.x; - osiData.acc_m_s2.y = monr.acceleration.accel.linear.y; - osiData.acc_m_s2.z = monr.acceleration.accel.linear.z; + osiData.pos_m.x = monr.pose.pose.position.x; + osiData.pos_m.y = monr.pose.pose.position.y; + osiData.pos_m.z = monr.pose.pose.position.z; - osiData.vel_m_s.x = monr.velocity.twist.linear.x; - osiData.vel_m_s.y = monr.velocity.twist.linear.y; - osiData.vel_m_s.z = monr.velocity.twist.linear.z; + osiData.acc_m_s2.x = monr.acceleration.accel.linear.x; + osiData.acc_m_s2.y = monr.acceleration.accel.linear.y; + osiData.acc_m_s2.z = monr.acceleration.accel.linear.z; - osiData.orientation_rad.yaw = 2 * acos(monr.pose.pose.orientation.w); + osiData.vel_m_s.x = monr.velocity.twist.linear.x; + osiData.vel_m_s.y = monr.velocity.twist.linear.y; + osiData.vel_m_s.z = monr.velocity.twist.linear.z; - return osiData; + osiData.orientation_rad.yaw = 2 * acos(monr.pose.pose.orientation.w); + return osiData; } - /** * @brief Linear extrapolation for a position. - * + * * @param position The current position * @param velocity The current velocity * @param dt Time forward * @return double extrapolated position, in dt milliseconds */ double OSIAdapter::linPosPrediction(const double position, const double velocity, const TimeUnit dt) { - return position + velocity * duration(dt).count(); + return position + velocity * duration(dt).count(); } - /** * @brief Linear extrapolation for a MONR-message. - * + * * @param monr MONR-message to extrapolate * @param dt Time forward */ -void OSIAdapter::extrapolateMONR(Monitor::message_type& monr, const TimeUnit dt) { - monr.pose.pose.position.x = linPosPrediction(monr.pose.pose.position.x, monr.velocity.twist.linear.x, dt); - monr.pose.pose.position.y = linPosPrediction(monr.pose.pose.position.y, monr.velocity.twist.linear.y, dt); - monr.pose.pose.position.z = linPosPrediction(monr.pose.pose.position.z, monr.velocity.twist.linear.z, dt); +void OSIAdapter::extrapolateMONR(Monitor::message_type& monr, const TimeUnit dt) { + monr.pose.pose.position.x = linPosPrediction(monr.pose.pose.position.x, monr.velocity.twist.linear.x, dt); + monr.pose.pose.position.y = linPosPrediction(monr.pose.pose.position.y, monr.velocity.twist.linear.y, dt); + monr.pose.pose.position.z = linPosPrediction(monr.pose.pose.position.z, monr.velocity.twist.linear.z, dt); } - void OSIAdapter::onConnectedObjectIdsMessage(const ConnectedObjectIds::message_type::SharedPtr msg) { - for (uint32_t id : msg->ids) { - if (monrSubscribers.find(id) == monrSubscribers.end()){ - monrSubscribers[id] = std::make_shared(*this, id, std::bind(&OSIAdapter::onMonitorMessage, this, _1, id)); - } - } + for (uint32_t id : msg->ids) { + if (monrSubscribers.find(id) == monrSubscribers.end()) { + monrSubscribers[id] = + std::make_shared(*this, id, std::bind(&OSIAdapter::onMonitorMessage, this, _1, id)); + } + } } - void OSIAdapter::onMonitorMessage(const Monitor::message_type::SharedPtr msg, uint32_t id) { - if (lastMonitors.find(id) == lastMonitors.end()){ // Do not extrapolate first message - lastMonitorTimes[id] = TimeUnit(0); - } - else{ // Otherwise take diff between last two messages - auto newTime = seconds(msg->atos_header.header.stamp.sec) + nanoseconds(msg->atos_header.header.stamp.nanosec); - auto oldTime = seconds(lastMonitors[id].atos_header.header.stamp.sec) + nanoseconds(lastMonitors[id].atos_header.header.stamp.nanosec); - lastMonitorTimes[id] = duration_cast(newTime-oldTime); - } - lastMonitors[id] = *msg; + if (lastMonitors.find(id) == lastMonitors.end()) { // Do not extrapolate first message + lastMonitorTimes[id] = TimeUnit(0); + } else { // Otherwise take diff between last two messages + auto newTime = seconds(msg->atos_header.header.stamp.sec) + nanoseconds(msg->atos_header.header.stamp.nanosec); + auto oldTime = seconds(lastMonitors[id].atos_header.header.stamp.sec) + + nanoseconds(lastMonitors[id].atos_header.header.stamp.nanosec); + lastMonitorTimes[id] = duration_cast(newTime - oldTime); + } + lastMonitors[id] = *msg; } diff --git a/atos/modules/OSIAdapter/src/serverfactory.cpp b/atos/modules/OSIAdapter/src/serverfactory.cpp index 818a1f15a..20b8f7ec6 100644 --- a/atos/modules/OSIAdapter/src/serverfactory.cpp +++ b/atos/modules/OSIAdapter/src/serverfactory.cpp @@ -5,99 +5,87 @@ */ #include "serverfactory.hpp" - /** * @brief Factory for creating TCP and UDP servers. - * + * * @param address Address * @param port Port * @param protocol Either "tcp" or "udp" */ ServerFactory::ServerFactory(const std::string& address, const uint16_t& port, const std::string protocol) { - this->address = address; - this->port = port; - this->protocol = protocol; + this->address = address; + this->port = port; + this->protocol = protocol; } - /** * @brief Destroy the Server Factory:: Server Factory object - * + * */ ServerFactory::~ServerFactory() { - destroyServer(); + destroyServer(); } - /** * @brief Creates either a TCPServer or a UDPServer. - * + * * @param protocol Which protocol to use. Either tcp or udp * @return std::unique_ptr Server object */ void ServerFactory::createServer() { - if (protocol == "tcp") { - tcpServer = std::make_unique(this->address, this->port); - } - else if (protocol == "udp") { - udpServer = std::make_unique(this->address, this->port); - } - else { - throw std::invalid_argument("Protocol must be either tcp or udp"); - } + if (protocol == "tcp") { + tcpServer = std::make_unique(this->address, this->port); + } else if (protocol == "udp") { + udpServer = std::make_unique(this->address, this->port); + } else { + throw std::invalid_argument("Protocol must be either tcp or udp"); + } } - /** * @brief Setup the server. TCP: Wait for a connection on address and port. UDP: Wait for someone to connect * in order to get address and port to send to, - * + * */ void ServerFactory::setupServer() { - if (protocol == "tcp") { - socket = std::make_shared(tcpServer->await(address, port)); - } - else if (protocol == "udp") { - auto pair = udpServer->recvfrom(); - hostInfo = pair.second; - } + if (protocol == "tcp") { + socket = std::make_shared(tcpServer->await(address, port)); + } else if (protocol == "udp") { + auto pair = udpServer->recvfrom(); + hostInfo = pair.second; + } } - /** * @brief Close the server. - * + * */ void ServerFactory::destroyServer() { - if (protocol == "tcp") { - tcpServer->close(); - } - else if (protocol == "udp") { - udpServer->close(); - } + if (protocol == "tcp") { + tcpServer->close(); + } else if (protocol == "udp") { + udpServer->close(); + } } - /** * @brief Reset the server. - * + * */ void ServerFactory::resetServer() { - destroyServer(); - setupServer(); + destroyServer(); + setupServer(); } - /** * @brief Send data to the client. - * + * * @param data The data to be sent */ void ServerFactory::sendData(const std::vector& data) { - if (protocol == "tcp") { - socket->send(data, Socket::NOSIGNAL); - } - else if (protocol == "udp") { - udpServer->sendto(std::make_pair(data, hostInfo)); - } -} \ No newline at end of file + if (protocol == "tcp") { + socket->send(data, Socket::NOSIGNAL); + } else if (protocol == "udp") { + udpServer->sendto(std::make_pair(data, hostInfo)); + } +} diff --git a/atos/modules/ObjectControl/inc/objectconnection.hpp b/atos/modules/ObjectControl/inc/objectconnection.hpp index e5b059f7e..f811a497f 100644 --- a/atos/modules/ObjectControl/inc/objectconnection.hpp +++ b/atos/modules/ObjectControl/inc/objectconnection.hpp @@ -5,12 +5,12 @@ */ #pragma once -#include -#include -#include +#include "channel.hpp" #include "iso22133.h" #include "loggable.hpp" -#include "channel.hpp" +#include +#include +#include /*! * \brief The ObjectConnection class holds network connection data for @@ -22,17 +22,16 @@ class ObjectConnection : public Loggable { Channel cmd; Channel mntr; - ObjectConnection(rclcpp::Logger log, int id) - : Loggable(log), - cmd(SOCK_STREAM, log, id), - mntr(SOCK_DGRAM, log, id) { - pipe(interruptionPipeFds); - } + ObjectConnection(rclcpp::Logger log, int id) : + Loggable(log), + cmd(SOCK_STREAM, log, id), + mntr(SOCK_DGRAM, log, id) { + pipe(interruptionPipeFds); + } bool isValid() const; bool isConnected() const; - void connect(std::shared_future stopRequest, - const std::chrono::milliseconds retryPeriod); + void connect(std::shared_future stopRequest, const std::chrono::milliseconds retryPeriod); void disconnect(); ISOMessageID pendingMessageType(bool awaitNext = false); void interruptSocket() { @@ -40,6 +39,7 @@ class ObjectConnection : public Loggable { write(interruptionPipeFds[1], &i, sizeof(i)); close(interruptionPipeFds[1]); } + private: int interruptionPipeFds[2]; -}; \ No newline at end of file +}; diff --git a/atos/modules/ObjectControl/inc/objectlistener.hpp b/atos/modules/ObjectControl/inc/objectlistener.hpp index c73f34955..5b1163d50 100644 --- a/atos/modules/ObjectControl/inc/objectlistener.hpp +++ b/atos/modules/ObjectControl/inc/objectlistener.hpp @@ -5,23 +5,19 @@ */ #pragma once +#include "loggable.hpp" #include "objectcontrol.hpp" #include "testobject.hpp" -#include "loggable.hpp" #include class ObjectControl; class ObjectControlState; -class ObjectListener : public Loggable -{ +class ObjectListener : public Loggable { public: - ObjectListener( - ObjectControl*, - std::shared_ptr, - rclcpp::Logger - ); + ObjectListener(ObjectControl*, std::shared_ptr, rclcpp::Logger); ~ObjectListener(); + private: std::shared_ptr obj; ObjectControl* handler; @@ -30,4 +26,3 @@ class ObjectListener : public Loggable void listen(); bool quit = false; }; - diff --git a/atos/modules/ObjectControl/inc/relativeanchor.hpp b/atos/modules/ObjectControl/inc/relativeanchor.hpp index 9b1d54a7d..ac75d09fd 100644 --- a/atos/modules/ObjectControl/inc/relativeanchor.hpp +++ b/atos/modules/ObjectControl/inc/relativeanchor.hpp @@ -15,10 +15,10 @@ class RelativeAnchor : public TestObject { RelativeAnchor(RelativeAnchor&&); RelativeAnchor& operator=(const RelativeAnchor&) = delete; - RelativeAnchor& operator=(RelativeAnchor&&) = default; + RelativeAnchor& operator=(RelativeAnchor&&) = default; - virtual void publishMonr(const ROSChannels::Monitor::message_type) override; + virtual void publishMonr(const ROSChannels::Monitor::message_type) override; private: - ROSChannels::Monitor::AnchorPub anchorPub; + ROSChannels::Monitor::AnchorPub anchorPub; }; diff --git a/atos/modules/ObjectControl/inc/relativetestobject.hpp b/atos/modules/ObjectControl/inc/relativetestobject.hpp index 2fd6a746f..44d3bd1ff 100644 --- a/atos/modules/ObjectControl/inc/relativetestobject.hpp +++ b/atos/modules/ObjectControl/inc/relativetestobject.hpp @@ -5,9 +5,9 @@ */ #pragma once -#include "testobject.hpp" -#include "roschannels/monitorchannel.hpp" #include "positioning.h" +#include "roschannels/monitorchannel.hpp" +#include "testobject.hpp" class RelativeTestObject : public TestObject { public: @@ -16,15 +16,15 @@ class RelativeTestObject : public TestObject { RelativeTestObject(RelativeTestObject&&); RelativeTestObject& operator=(const RelativeTestObject&) = delete; - RelativeTestObject& operator=(RelativeTestObject&&) = default; + RelativeTestObject& operator=(RelativeTestObject&&) = default; private: - virtual ObjectMonitorType transformCoordinate(const ObjectMonitorType& point, - const ObjectMonitorType& anchor, - const bool debug); - virtual MonitorMessage readMonitorMessage() override; - - ROSChannels::Monitor::AnchorSub anchorSub; - ObjectMonitorType lastAnchorMonr; - void updateAnchor(const ROSChannels::Monitor::message_type::SharedPtr); + virtual ObjectMonitorType transformCoordinate(const ObjectMonitorType& point, + const ObjectMonitorType& anchor, + const bool debug); + virtual MonitorMessage readMonitorMessage() override; + + ROSChannels::Monitor::AnchorSub anchorSub; + ObjectMonitorType lastAnchorMonr; + void updateAnchor(const ROSChannels::Monitor::message_type::SharedPtr); }; diff --git a/atos/modules/ObjectControl/src/dump_state_machine.cpp b/atos/modules/ObjectControl/src/dump_state_machine.cpp index 1705b7a73..0f9a79c53 100644 --- a/atos/modules/ObjectControl/src/dump_state_machine.cpp +++ b/atos/modules/ObjectControl/src/dump_state_machine.cpp @@ -10,20 +10,22 @@ namespace sml = boost::sml; -inline void do_indent(std::ostream& out, unsigned int indent) { out << std::string(indent, ' '); } +inline void do_indent(std::ostream& out, unsigned int indent) { + out << std::string(indent, ' '); +} // use this to track initialization for orthogonal states -bool state_initialized = false; // NOLINT(misc-definitions-in-headers) +bool state_initialized = false; // NOLINT(misc-definitions-in-headers) // use this to track which submachines have already been dumped so they don't get dumped twice -std::vector completed_submachines; // NOLINT(misc-definitions-in-headers) +std::vector completed_submachines; // NOLINT(misc-definitions-in-headers) /** allows for checking if the type is sml::front::seq_ * This type is used by sml when there are lists of actions. */ -template -struct is_seq_ : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) -template -struct is_seq_> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) +template +struct is_seq_ : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) +template +struct is_seq_> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) /** allows for checking if the type is sml::front::not_ * This type is used by sml inside of guards, when the guard value is negated with ! @@ -33,111 +35,112 @@ struct is_seq_> : sml::aux::true_type {}; // NOLINT(rea * If the type passed doesn't match sml::front::not_, it'll match the generic is_not_ which inherits * from sml::aux::false_type, giving it a member called "value" that is set to false. */ -template -struct is_not_ : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) -template -struct is_not_> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) +template +struct is_not_ : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) +template +struct is_not_> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) /** provides access to the template parameter type of an sml::front::not_ */ -template +template struct strip_not_ { - using type = T; -}; // NOLINT(readability-identifier-naming) -template + using type = T; +}; // NOLINT(readability-identifier-naming) +template struct strip_not_> { - using type = T; -}; // NOLINT(readability-identifier-naming) + using type = T; +}; // NOLINT(readability-identifier-naming) /** allows for checking if the type is sml::front::and_ * This type is used by sml inside of guards when two guard functions are combined with && */ -template -struct is_and_ : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) -template -struct is_and_> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) +template +struct is_and_ : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) +template +struct is_and_> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) /** allows for checking if the type is sml::front::or_ * This type is used by sml inside of guards when two guard functions are combined with || */ -template -struct is_or_ : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) -template -struct is_or_> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) +template +struct is_or_ : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) +template +struct is_or_> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) /** uses std::tuple_element and std::tuple to access the Nth type in a parameter pack */ -template +template using NthTypeOf = typename std::tuple_element>::type; /** gets the size of a parameter pack * this isn't really necessary, sizeof...(Ts) can be used directly instead */ -template -struct count { // NOLINT(readability-identifier-naming) - static const std::size_t value = sizeof...(Ts); // NOLINT(readability-identifier-naming) +template +struct count { // NOLINT(readability-identifier-naming) + static const std::size_t value = sizeof...(Ts); // NOLINT(readability-identifier-naming) }; /** allows for checking if the type is sml::aux::zero_wrapper * sml puts this around types inside of guards and event sequences */ -template -struct is_zero_wrapper : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) -template -struct is_zero_wrapper> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) +template +struct is_zero_wrapper : sml::aux::false_type {}; // NOLINT(readability-identifier-naming) +template +struct is_zero_wrapper> : sml::aux::true_type {}; // NOLINT(readability-identifier-naming) /** if T is a zero wrapper, ::type will be the inner type. if not, it will be T. */ -template +template struct strip_zero_wrapper { - using type = T; -}; // NOLINT(readability-identifier-naming) -template + using type = T; +}; // NOLINT(readability-identifier-naming) +template struct strip_zero_wrapper> { - using type = T; -}; // NOLINT(readability-identifier-naming) + using type = T; +}; // NOLINT(readability-identifier-naming) /** accesses the type of a state-machine, sml::back::sm */ -template +template struct submachine_type { - using type = T; -}; // NOLINT(readability-identifier-naming) -template + using type = T; +}; // NOLINT(readability-identifier-naming) +template struct submachine_type> { - using type = typename T::sm; -}; // NOLINT(readability-identifier-naming) + using type = typename T::sm; +}; // NOLINT(readability-identifier-naming) /** print the types inside a sml::front::seq_ * These types came from a list of actions. */ -template -struct print_seq_types { // NOLINT(readability-identifier-naming) - template - static void func(std::ostream& out) { - constexpr auto param_pack_empty = (sizeof...(Ts) == I); - if constexpr (!param_pack_empty) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) - using current_type = NthTypeOf; - if constexpr (is_seq_::value) { // NOLINT(readability-braces-around-statements) - // handle nested seq_ types, these happen when there are 3 or more actions - print_seq_types::template func<0>(out); - } else { // NOLINT(readability-misleading-indentation) - // print this param directly - out << sml::aux::string::type>{}.c_str(); - } - if constexpr (I + 1 < sizeof...(Ts)) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) - out << ",\\n "; - } - print_seq_types::template func(out); - } - } +template +struct print_seq_types { // NOLINT(readability-identifier-naming) + template + static void func(std::ostream& out) { + constexpr auto param_pack_empty = (sizeof...(Ts) == I); + if constexpr (!param_pack_empty) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) + using current_type = NthTypeOf; + if constexpr (is_seq_::value) { // NOLINT(readability-braces-around-statements) + // handle nested seq_ types, these happen when there are 3 or more actions + print_seq_types::template func<0>(out); + } else { // NOLINT(readability-misleading-indentation) + // print this param directly + out << sml::aux::string::type>{}.c_str(); + } + if constexpr (I + 1 < + sizeof...(Ts)) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) + out << ",\\n "; + } + print_seq_types::template func(out); + } + } }; -template -struct print_seq_types> { // NOLINT(readability-identifier-naming) - template - static void func(std::ostream& out) { - print_seq_types::template func<0>(out); - } +template +struct print_seq_types> { // NOLINT(readability-identifier-naming) + template + static void func(std::ostream& out) { + print_seq_types::template func<0>(out); + } }; /** print the types inside a guard @@ -145,224 +148,227 @@ struct print_seq_types> { // NOLINT(readability-identif * this one more complicated. They also involve the zero_wrapper. * The various partial specializations handle all of the possible types. */ -template -struct print_guard { // NOLINT(readability-identifier-naming) - template - static void func(std::ostream& out, const std::string& sep = "") { - constexpr auto param_pack_empty = (sizeof...(Ts) == I); - if constexpr (!param_pack_empty) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) - using current_type = NthTypeOf; - if constexpr (is_zero_wrapper< - current_type>::value) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) - // unwrap the zero_wrapper and put it back into the recursion, it could be anything - print_guard::type>::template func<0>(out); - } else { // NOLINT(readability-misleading-indentation) - // it's just a functor, print it - out << sml::aux::string{}.c_str(); - } - - // if we're not at the end, print the separator - if constexpr (I + 1 < sizeof...(Ts)) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) - if (!sep.empty()) { - out << sep; - } - } - - // keep the recursion going, call for the next type in the parameter pack - print_guard::template func(out, sep); - } - } +template +struct print_guard { // NOLINT(readability-identifier-naming) + template + static void func(std::ostream& out, const std::string& sep = "") { + constexpr auto param_pack_empty = (sizeof...(Ts) == I); + if constexpr (!param_pack_empty) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) + using current_type = NthTypeOf; + if constexpr (is_zero_wrapper:: + value) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) + // unwrap the zero_wrapper and put it back into the recursion, it could be anything + print_guard::type>::template func<0>(out); + } else { // NOLINT(readability-misleading-indentation) + // it's just a functor, print it + out << sml::aux::string{}.c_str(); + } + + // if we're not at the end, print the separator + if constexpr (I + 1 < + sizeof...(Ts)) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) + if (!sep.empty()) { + out << sep; + } + } + + // keep the recursion going, call for the next type in the parameter pack + print_guard::template func(out, sep); + } + } }; -template -struct print_guard> { // NOLINT(readability-identifier-naming) - template - static void func(std::ostream& out, const std::string& /*sep*/ = "") { - out << "!" << sml::aux::string::type>{}.c_str(); - } +template +struct print_guard> { // NOLINT(readability-identifier-naming) + template + static void func(std::ostream& out, const std::string& /*sep*/ = "") { + out << "!" << sml::aux::string::type>{}.c_str(); + } }; -template -struct print_guard> { // NOLINT(readability-identifier-naming) - template - static void func(std::ostream& out, const std::string& /*sep*/ = "") { - constexpr auto param_pack_empty = (sizeof...(Ts) == I); - if constexpr (!param_pack_empty) { - print_guard::template func(out, " &&\\n "); - } - } +template +struct print_guard> { // NOLINT(readability-identifier-naming) + template + static void func(std::ostream& out, const std::string& /*sep*/ = "") { + constexpr auto param_pack_empty = (sizeof...(Ts) == I); + if constexpr (!param_pack_empty) { + print_guard::template func(out, " &&\\n "); + } + } }; -template -struct print_guard> { // NOLINT(readability-identifier-naming) - template - static void func(std::ostream& out, const std::string& /*sep*/ = "") { - constexpr auto param_pack_empty = (sizeof...(Ts) == I); - if constexpr (!param_pack_empty) { - print_guard::template func(out, " ||\\n "); - } - } +template +struct print_guard> { // NOLINT(readability-identifier-naming) + template + static void func(std::ostream& out, const std::string& /*sep*/ = "") { + constexpr auto param_pack_empty = (sizeof...(Ts) == I); + if constexpr (!param_pack_empty) { + print_guard::template func(out, " ||\\n "); + } + } }; // forward declaration of dump_transitions -template +template struct dump_transitions; -template +template void dump_transition(std::ostream& out) noexcept { - constexpr auto src_is_sub_sm = - !sml::aux::is_same, sml::back::get_sub_sms>::value; - constexpr auto dst_is_sub_sm = - !sml::aux::is_same, sml::back::get_sub_sms>::value; - - std::string src_state, dst_state; - // NOLINTNEXTLINE(readability-braces-around-statements) - if constexpr (src_is_sub_sm) { - src_state = std::string{sml::aux::string::type>{}.c_str()}; - } else { // NOLINT(readability-misleading-indentation) - src_state = std::string{sml::aux::string{}.c_str()}; - } - - // NOLINTNEXTLINE(readability-braces-around-statements) - if constexpr (dst_is_sub_sm) { - dst_state = std::string{sml::aux::string::type>{}.c_str()}; - } else { // NOLINT(readability-misleading-indentation) - dst_state = std::string{sml::aux::string{}.c_str()}; - } - - const auto dst_internal = sml::aux::is_same::value; - - const auto has_event = !sml::aux::is_same::value; - const auto has_guard = !sml::aux::is_same::value; - const auto has_action = !sml::aux::is_same::value; - - if (has_event && has_action && sml::aux::is_same::value) { - do_indent(out, N); - out << src_state << " : " << boost::sml::aux::get_type_name() << " / defer\n"; - return; - } - - if (dst_state == "terminate") { - dst_state = "[*]"; - } - - if (T::initial) { - if (state_initialized) { // create an orthogonal section - do_indent(out, N); - out << "--\n"; - } - - state_initialized = true; - do_indent(out, N); - out << "[*] --> " << src_state << "\n"; - } - - // NOLINTNEXTLINE(readability-braces-around-statements, bugprone-suspicious-semicolon) - if constexpr (src_is_sub_sm) { - auto already_in = - std::find(completed_submachines.begin(), completed_submachines.end(), src_state) != completed_submachines.end(); - if (!already_in) { - completed_submachines.push_back(src_state); - constexpr int indent = N + 2; - do_indent(out, N); - out << "state " << src_state << " {\n"; - bool prev_state = state_initialized; - state_initialized = false; - dump_transitions::template func(out); - do_indent(out, N); - out << "}\n"; - state_initialized = prev_state; - } - } - - do_indent(out, N); - out << src_state; - if (!dst_internal) { - out << " --> " << dst_state; - } - - if (has_event || has_guard || has_action) { - out << " :"; - } - - if (has_event) { - out << " " << std::string{sml::aux::string{}.c_str()}; - } - - if (has_guard) { - out << "\\n ["; - print_guard::template func<0>(out); - out << "]"; - } - - if (has_action) { - out << " /\\n "; - - if constexpr (is_seq_::value) { // NOLINT(readability-braces-around-statements) - out << "("; - print_seq_types::template func<0>(out); - out << ")"; - } else { // NOLINT(readability-misleading-indentation) - out << sml::aux::string{}.c_str(); - } - } - - out << "\n"; - - // NOLINTNEXTLINE(readability-braces-around-statements, bugprone-suspicious-semicolon) - if constexpr (dst_is_sub_sm) { - auto already_in = - std::find(completed_submachines.begin(), completed_submachines.end(), dst_state) != completed_submachines.end(); - if (!already_in) { - completed_submachines.push_back(dst_state); - constexpr int indent = N + 2; - do_indent(out, N); - out << "state " << dst_state << " {\n"; - bool prev_state = state_initialized; - state_initialized = false; - dump_transitions::template func(out); - do_indent(out, N); - out << "}\n"; - state_initialized = prev_state; - } - } + constexpr auto src_is_sub_sm = + !sml::aux::is_same, sml::back::get_sub_sms>::value; + constexpr auto dst_is_sub_sm = + !sml::aux::is_same, sml::back::get_sub_sms>::value; + + std::string src_state, dst_state; + // NOLINTNEXTLINE(readability-braces-around-statements) + if constexpr (src_is_sub_sm) { + src_state = std::string{sml::aux::string::type>{}.c_str()}; + } else { // NOLINT(readability-misleading-indentation) + src_state = std::string{sml::aux::string{}.c_str()}; + } + + // NOLINTNEXTLINE(readability-braces-around-statements) + if constexpr (dst_is_sub_sm) { + dst_state = std::string{sml::aux::string::type>{}.c_str()}; + } else { // NOLINT(readability-misleading-indentation) + dst_state = std::string{sml::aux::string{}.c_str()}; + } + + const auto dst_internal = sml::aux::is_same::value; + + const auto has_event = !sml::aux::is_same::value; + const auto has_guard = !sml::aux::is_same::value; + const auto has_action = !sml::aux::is_same::value; + + if (has_event && has_action && sml::aux::is_same::value) { + do_indent(out, N); + out << src_state << " : " << boost::sml::aux::get_type_name() << " / defer\n"; + return; + } + + if (dst_state == "terminate") { + dst_state = "[*]"; + } + + if (T::initial) { + if (state_initialized) { // create an orthogonal section + do_indent(out, N); + out << "--\n"; + } + + state_initialized = true; + do_indent(out, N); + out << "[*] --> " << src_state << "\n"; + } + + // NOLINTNEXTLINE(readability-braces-around-statements, bugprone-suspicious-semicolon) + if constexpr (src_is_sub_sm) { + auto already_in = std::find(completed_submachines.begin(), completed_submachines.end(), src_state) != + completed_submachines.end(); + if (!already_in) { + completed_submachines.push_back(src_state); + constexpr int indent = N + 2; + do_indent(out, N); + out << "state " << src_state << " {\n"; + bool prev_state = state_initialized; + state_initialized = false; + dump_transitions::template func(out); + do_indent(out, N); + out << "}\n"; + state_initialized = prev_state; + } + } + + do_indent(out, N); + out << src_state; + if (!dst_internal) { + out << " --> " << dst_state; + } + + if (has_event || has_guard || has_action) { + out << " :"; + } + + if (has_event) { + out << " " << std::string{sml::aux::string{}.c_str()}; + } + + if (has_guard) { + out << "\\n ["; + print_guard::template func<0>(out); + out << "]"; + } + + if (has_action) { + out << " /\\n "; + + if constexpr (is_seq_::value) { // NOLINT(readability-braces-around-statements) + out << "("; + print_seq_types::template func<0>(out); + out << ")"; + } else { // NOLINT(readability-misleading-indentation) + out << sml::aux::string{}.c_str(); + } + } + + out << "\n"; + + // NOLINTNEXTLINE(readability-braces-around-statements, bugprone-suspicious-semicolon) + if constexpr (dst_is_sub_sm) { + auto already_in = std::find(completed_submachines.begin(), completed_submachines.end(), dst_state) != + completed_submachines.end(); + if (!already_in) { + completed_submachines.push_back(dst_state); + constexpr int indent = N + 2; + do_indent(out, N); + out << "state " << dst_state << " {\n"; + bool prev_state = state_initialized; + state_initialized = false; + dump_transitions::template func(out); + do_indent(out, N); + out << "}\n"; + state_initialized = prev_state; + } + } } // this template allows iterating through the types in the parameter pack Ts... // I is the counter // INDENT is the current indentation level (for the state machine or sub-state machine) -template +template void apply_dump_transition(std::ostream& out) { - // iteration is finished when I == the size of the parameter pack - constexpr auto param_pack_empty = (sizeof...(Ts) == I); - if constexpr (!param_pack_empty) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) - // run the dump_transition function to print this sml::front::transition type - dump_transition>(out); - // iteration isn't finished, keep going - apply_dump_transition(out); - } + // iteration is finished when I == the size of the parameter pack + constexpr auto param_pack_empty = (sizeof...(Ts) == I); + if constexpr (!param_pack_empty) { // NOLINT(readability-braces-around-statements,bugprone-suspicious-semicolon) + // run the dump_transition function to print this sml::front::transition type + dump_transition>(out); + // iteration isn't finished, keep going + apply_dump_transition(out); + } } // SFINAE type -template -struct dump_transitions { // NOLINT(readability-identifier-naming) - template - static void func(std::ostream&) {} +template +struct dump_transitions { // NOLINT(readability-identifier-naming) + template + static void func(std::ostream&) {} }; // Partial specialization for sml::aux::type_list. This grants access to the // types inside the type list, which are sml::front::transition types, so they can // be passed to apply_dump_transition. -template -struct dump_transitions> { // NOLINT(readability-identifier-naming) - template - static void func(std::ostream& out) { - apply_dump_transition(out); - } +template +struct dump_transitions> { // NOLINT(readability-identifier-naming) + template + static void func(std::ostream& out) { + apply_dump_transition(out); + } }; -template +template void dump(std::ostream& out) noexcept { - out << "@startuml\n\n"; - dump_transitions::transitions>::template func<0>(out); - out << "\n@enduml\n"; + out << "@startuml\n\n"; + dump_transitions::transitions>::template func<0>(out); + out << "\n@enduml\n"; } -int main() { dump(std::cout); } +int main() { + dump(std::cout); +} diff --git a/atos/modules/ObjectControl/src/main.cpp b/atos/modules/ObjectControl/src/main.cpp index b496a07a1..4220fa5b4 100644 --- a/atos/modules/ObjectControl/src/main.cpp +++ b/atos/modules/ObjectControl/src/main.cpp @@ -6,10 +6,10 @@ #include "objectcontrol.hpp" #include -int main(int argc, char **argv) { - rclcpp::init(argc,argv); +int main(int argc, char** argv) { + rclcpp::init(argc, argv); auto exec = std::make_shared(); - auto obc = std::make_shared(exec); + auto obc = std::make_shared(exec); exec->add_node(obc); exec->spin(); rclcpp::shutdown(); diff --git a/atos/modules/ObjectControl/src/objectconnection.cpp b/atos/modules/ObjectControl/src/objectconnection.cpp index 311eed5c6..417b00c57 100644 --- a/atos/modules/ObjectControl/src/objectconnection.cpp +++ b/atos/modules/ObjectControl/src/objectconnection.cpp @@ -5,9 +5,7 @@ */ #include "objectconnection.hpp" -void ObjectConnection::connect( - std::shared_future stopRequest, - const std::chrono::milliseconds retryPeriod) { +void ObjectConnection::connect(std::shared_future stopRequest, const std::chrono::milliseconds retryPeriod) { try { this->cmd.connect(stopRequest, retryPeriod); this->mntr.connect(stopRequest, retryPeriod); @@ -24,9 +22,9 @@ bool ObjectConnection::isConnected() const { return false; } pollfd fds[2]; - fds[0].fd = mntr.socket; + fds[0].fd = mntr.socket; fds[0].events = POLLIN | POLLOUT; - fds[1].fd = cmd.socket; + fds[1].fd = cmd.socket; fds[1].events = POLLIN | POLLOUT; return poll(fds, 2, 0) >= 0; } @@ -52,28 +50,24 @@ ISOMessageID ObjectConnection::pendingMessageType(bool awaitNext) { FD_SET(interruptionPipeFds[0], &fds); FD_SET(mntr.socket, &fds); FD_SET(cmd.socket, &fds); - auto result = select(std::max({mntr.socket,cmd.socket,interruptionPipeFds[0]})+1, - &fds, nullptr, nullptr, nullptr); + auto result = + select(std::max({mntr.socket, cmd.socket, interruptionPipeFds[0]}) + 1, &fds, nullptr, nullptr, nullptr); if (result < 0) { - throw std::runtime_error(std::string("Failed socket operation (select: ") + strerror(errno) + ")"); // TODO clearer - } - else if (!isValid()) { + throw std::runtime_error(std::string("Failed socket operation (select: ") + strerror(errno) + + ")"); // TODO clearer + } else if (!isValid()) { throw std::invalid_argument("Connection invalidated during select call"); - } - else if (FD_ISSET(interruptionPipeFds[0], &fds)){ + } else if (FD_ISSET(interruptionPipeFds[0], &fds)) { close(interruptionPipeFds[0]); throw std::range_error("Select call was interrupted"); - } - else if (FD_ISSET(mntr.socket, &fds)) { + } else if (FD_ISSET(mntr.socket, &fds)) { return this->mntr.pendingMessageType(); - } - else if (FD_ISSET(cmd.socket, &fds)) { + } else if (FD_ISSET(cmd.socket, &fds)) { return this->cmd.pendingMessageType(); } throw std::logic_error("Call to select returned unexpectedly: " + std::to_string(result)); - } - else { + } else { auto retval = this->mntr.pendingMessageType(); return retval != MESSAGE_ID_INVALID ? retval : this->cmd.pendingMessageType(); } -} \ No newline at end of file +} diff --git a/atos/modules/ObjectControl/src/objectlistener.cpp b/atos/modules/ObjectControl/src/objectlistener.cpp index 96e695d5d..69426a3b9 100644 --- a/atos/modules/ObjectControl/src/objectlistener.cpp +++ b/atos/modules/ObjectControl/src/objectlistener.cpp @@ -6,10 +6,10 @@ #include "objectlistener.hpp" #include "objectcontrol.hpp" -ObjectListener::ObjectListener(ObjectControl* sh, std::shared_ptr ob, rclcpp::Logger log) : - Loggable(log), - obj(ob), - handler(sh) { +ObjectListener::ObjectListener(ObjectControl* sh, std::shared_ptr ob, rclcpp::Logger log) : + Loggable(log), + obj(ob), + handler(sh) { if (!obj->isConnected()) { throw std::invalid_argument("Attempted to start listener for disconnected object"); @@ -31,12 +31,12 @@ ObjectListener::~ObjectListener() { void ObjectListener::listen() { try { while (!this->quit) { - //handle incoming iso22133 messages + // handle incoming iso22133 messages obj->handleISOMessage(true); } } catch (std::invalid_argument& e) { RCLCPP_ERROR(get_logger(), e.what()); // TODO: add comment explaining this case.. - } catch (std::range_error& e){ + } catch (std::range_error& e) { RCLCPP_DEBUG(get_logger(), e.what()); // Socket was interrupted intentionally, exit gracefully } catch (std::runtime_error& e) { RCLCPP_ERROR(get_logger(), e.what()); diff --git a/atos/modules/OpenScenarioGateway/openscenariogateway.py b/atos/modules/OpenScenarioGateway/openscenariogateway.py index ada9f9dec..e4851e430 100755 --- a/atos/modules/OpenScenarioGateway/openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/openscenariogateway.py @@ -1,20 +1,20 @@ #!/usr/bin/env python3 import hashlib -import json import time -import atos_interfaces.srv from os import path +from typing import List + import rclpy import rclpy.logging -import atos_interfaces.msg +from modules.OpenScenarioGateway.storyboard_handler import StoryBoardHandler from rcl_interfaces.msg import SetParametersResult from rclpy.node import Node -from std_msgs.msg import Empty from scenariogeneration import xosc -from modules.OpenScenarioGateway.storyboard_handler import StoryBoardHandler -from typing import List +from std_msgs.msg import Empty +import atos_interfaces.msg +import atos_interfaces.srv ROOT_FOLDER_PATH_PARAMETER = "root_folder_path" ACTIVE_OBJECT_NAME_PARAMETER = "active_object_names" @@ -34,7 +34,6 @@ def __init__(self, name: str, catalog_ref: xosc.CatalogReference): class OpenScenarioGateway(Node): - def __init__(self): super().__init__("open_scenario_gateway") @@ -201,8 +200,7 @@ def update_active_scenario_objects(self, active_objects_name: List[str]): def get_all_objects_in_scenario(self, scenario_file) -> List[ScenarioObject]: scenario = xosc.ParseOpenScenario(scenario_file) scenario_objects = { - id - + 1: ScenarioObject( + id + 1: ScenarioObject( name=scenario_object.name, catalog_ref=xosc.CatalogReference( scenario_object.entityobject.catalogname, diff --git a/atos/modules/OpenScenarioGateway/storyboard_handler.py b/atos/modules/OpenScenarioGateway/storyboard_handler.py index 116c55a84..3acf5ce7b 100644 --- a/atos/modules/OpenScenarioGateway/storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/storyboard_handler.py @@ -1,10 +1,10 @@ import sys -from scenariogeneration import xosc + from modules.OpenScenarioGateway.custom_command_action import CustomCommandAction +from scenariogeneration import xosc class StoryBoardHandler: - def __init__(self, scenario_file: str): self.scenario_file = scenario_file self.xosc = xosc.ParseOpenScenario(scenario_file) diff --git a/atos/modules/OpenScenarioGateway/tests/test_openscenariogateway.py b/atos/modules/OpenScenarioGateway/tests/test_openscenariogateway.py index b5ffdcca3..db914aeb1 100644 --- a/atos/modules/OpenScenarioGateway/tests/test_openscenariogateway.py +++ b/atos/modules/OpenScenarioGateway/tests/test_openscenariogateway.py @@ -1,10 +1,11 @@ import os + +import modules.OpenScenarioGateway.openscenariogateway as openxgw import pytest import rclpy +import rclpy.parameter -import modules.OpenScenarioGateway.openscenariogateway as openxgw import atos_interfaces.srv as srv -import rclpy.parameter @pytest.fixture(scope="module") diff --git a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py b/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py index 7ff44f70b..436924768 100644 --- a/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py +++ b/atos/modules/OpenScenarioGateway/tests/test_storyboard_handler.py @@ -1,5 +1,5 @@ import os -import pytest + import modules.OpenScenarioGateway.storyboard_handler as sh diff --git a/atos/modules/PointcloudPublisher/inc/pointcloudpublisher.hpp b/atos/modules/PointcloudPublisher/inc/pointcloudpublisher.hpp index b54906366..bfcb57a58 100644 --- a/atos/modules/PointcloudPublisher/inc/pointcloudpublisher.hpp +++ b/atos/modules/PointcloudPublisher/inc/pointcloudpublisher.hpp @@ -6,29 +6,29 @@ #pragma once #include "module.hpp" -#include "roschannels/pointcloudchannel.hpp" #include "roschannels/commandchannels.hpp" +#include "roschannels/pointcloudchannel.hpp" #include #include class PointcloudPublisher : public Module { public: - PointcloudPublisher(); - ~PointcloudPublisher(); + PointcloudPublisher(); + ~PointcloudPublisher(); private: - static inline std::string const moduleName = "pointcloud_publisher"; - ROSChannels::Init::Sub initSub; - std::map> pointcloudPubs; + static inline std::string const moduleName = "pointcloud_publisher"; + ROSChannels::Init::Sub initSub; + std::map> pointcloudPubs; - std::vector pointcloudFiles; - std::map>> pointclouds; + std::vector pointcloudFiles; + std::map>> pointclouds; - void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) override; - void initialize(); - void readPointcloudParams(); - void loadPointClouds(); - void createPublishers(); - std::string getPublisherTopicName(const std::string &path) const; -}; \ No newline at end of file + void onInitMessage(const ROSChannels::Init::message_type::SharedPtr) override; + void initialize(); + void readPointcloudParams(); + void loadPointClouds(); + void createPublishers(); + std::string getPublisherTopicName(const std::string& path) const; +}; diff --git a/atos/modules/PointcloudPublisher/src/main.cpp b/atos/modules/PointcloudPublisher/src/main.cpp index 7ed846c7c..04dfd8e07 100644 --- a/atos/modules/PointcloudPublisher/src/main.cpp +++ b/atos/modules/PointcloudPublisher/src/main.cpp @@ -3,15 +3,15 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include #include "pointcloudpublisher.hpp" +#include -int main(int argc, char **argv){ +int main(int argc, char** argv) { - rclcpp::init(argc, argv); - auto pointcloudPublisherNode = std::make_shared(); - rclcpp::spin(pointcloudPublisherNode); - rclcpp::shutdown(); + rclcpp::init(argc, argv); + auto pointcloudPublisherNode = std::make_shared(); + rclcpp::spin(pointcloudPublisherNode); + rclcpp::shutdown(); - return 0; -} \ No newline at end of file + return 0; +} diff --git a/atos/modules/PointcloudPublisher/src/pointcloudpublisher.cpp b/atos/modules/PointcloudPublisher/src/pointcloudpublisher.cpp index dd23667e9..9c05a5378 100644 --- a/atos/modules/PointcloudPublisher/src/pointcloudpublisher.cpp +++ b/atos/modules/PointcloudPublisher/src/pointcloudpublisher.cpp @@ -5,17 +5,17 @@ */ #include "pointcloudpublisher.hpp" -#include #include +#include #include /** * @brief PointcloudPublisher constructor. * */ -PointcloudPublisher::PointcloudPublisher() : Module(PointcloudPublisher::moduleName), - initSub(*this, std::bind(&PointcloudPublisher::onInitMessage, this, std::placeholders::_1)) -{ +PointcloudPublisher::PointcloudPublisher() : + Module(PointcloudPublisher::moduleName), + initSub(*this, std::bind(&PointcloudPublisher::onInitMessage, this, std::placeholders::_1)) { std::vector default_files = {""}; declare_parameter("pointcloud_files", default_files); } @@ -47,8 +47,7 @@ void PointcloudPublisher::initialize() { void PointcloudPublisher::readPointcloudParams() { get_parameter("pointcloud_files", pointcloudFiles); const std::string homeDir = getenv("HOME"); - for (auto &pointcloudFile : pointcloudFiles) - { + for (auto& pointcloudFile : pointcloudFiles) { pointcloudFile = homeDir + "/.astazero/ATOS/pointclouds/" + pointcloudFile; } } @@ -58,17 +57,16 @@ void PointcloudPublisher::readPointcloudParams() { * */ void PointcloudPublisher::loadPointClouds() { - for (auto &pointcloudFile : pointcloudFiles) - { + for (auto& pointcloudFile : pointcloudFiles) { auto pointcloud = std::make_shared>(); - if (pcl::io::loadPCDFile(pointcloudFile, *pointcloud) == -1) - { + if (pcl::io::loadPCDFile(pointcloudFile, *pointcloud) == -1) { RCLCPP_ERROR(get_logger(), "Could not read file %s", pointcloudFile.c_str()); - } - else - { + } else { pointclouds[pointcloudFile] = pointcloud; - RCLCPP_INFO(get_logger(), "Loaded pointcloud %s with %d points", pointcloudFile.c_str(), pointcloud->width * pointcloud->height); + RCLCPP_INFO(get_logger(), + "Loaded pointcloud %s with %d points", + pointcloudFile.c_str(), + pointcloud->width * pointcloud->height); } } } @@ -78,9 +76,9 @@ void PointcloudPublisher::loadPointClouds() { * */ void PointcloudPublisher::createPublishers() { - for (auto &pointcloudFile : pointcloudFiles) - { - auto pointcloudPub = std::make_shared(*this, getPublisherTopicName(pointcloudFile)); + for (auto& pointcloudFile : pointcloudFiles) { + auto pointcloudPub = + std::make_shared(*this, getPublisherTopicName(pointcloudFile)); pointcloudPubs[pointcloudFile] = pointcloudPub; } } @@ -91,7 +89,7 @@ void PointcloudPublisher::createPublishers() { * @param path Path to make topicn ame from * @return std::string The topic name */ -std::string PointcloudPublisher::getPublisherTopicName(const std::string &path) const { +std::string PointcloudPublisher::getPublisherTopicName(const std::string& path) const { auto str = path.substr(path.rfind('/') + 1); return str.substr(0, str.length() - 4); } @@ -103,13 +101,12 @@ std::string PointcloudPublisher::getPublisherTopicName(const std::string &path) void PointcloudPublisher::onInitMessage(const ROSChannels::Init::message_type::SharedPtr) { initialize(); - for (auto &pointcloudFile : pointcloudFiles) - { + for (auto& pointcloudFile : pointcloudFiles) { sensor_msgs::msg::PointCloud2 msg; auto pointcloud = pointclouds[pointcloudFile]; pcl::toROSMsg(*pointcloud, msg); msg.header.frame_id = "map"; - msg.header.stamp = this->get_clock()->now(); + msg.header.stamp = this->get_clock()->now(); auto pointcloudPub = pointcloudPubs[pointcloudFile]; pointcloudPub->publish(msg); diff --git a/atos/modules/RESTBridge/inc/restbridge.hpp b/atos/modules/RESTBridge/inc/restbridge.hpp index 7f7bfb565..8ea0c5928 100644 --- a/atos/modules/RESTBridge/inc/restbridge.hpp +++ b/atos/modules/RESTBridge/inc/restbridge.hpp @@ -19,20 +19,18 @@ using json = nlohmann::json; */ class RESTBridge : public Module { public: - static inline std::string const moduleName = "rest_bridge"; - RESTBridge(); - ~RESTBridge(); + static inline std::string const moduleName = "rest_bridge"; + RESTBridge(); + ~RESTBridge(); protected: - void onCustomCommandAction( - const atos_interfaces::msg::CustomCommandAction::SharedPtr msg); + void onCustomCommandAction(const atos_interfaces::msg::CustomCommandAction::SharedPtr msg); private: - ROSChannels::CustomCommandAction::Sub - customCommandActionMsgSub; //!< Subscriber to icdc messages requests + ROSChannels::CustomCommandAction::Sub customCommandActionMsgSub; //!< Subscriber to icdc messages requests - json parseJsonData(std::string &msg); - void POST(const std::string &endpoint, const json &data); + json parseJsonData(std::string& msg); + void POST(const std::string& endpoint, const json& data); - CURL *curl_handle; + CURL* curl_handle; }; diff --git a/atos/modules/RESTBridge/src/main.cpp b/atos/modules/RESTBridge/src/main.cpp index 1915d8cd1..11b4593f3 100644 --- a/atos/modules/RESTBridge/src/main.cpp +++ b/atos/modules/RESTBridge/src/main.cpp @@ -1,10 +1,10 @@ #include "rclcpp/rclcpp.hpp" #include "restbridge.hpp" -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto rb = std::make_shared(); - rclcpp::spin(rb); - rclcpp::shutdown(); - return 0; +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto rb = std::make_shared(); + rclcpp::spin(rb); + rclcpp::shutdown(); + return 0; } diff --git a/atos/modules/SampleModule/inc/samplemodule.hpp b/atos/modules/SampleModule/inc/samplemodule.hpp index 69b65d729..f9628138b 100644 --- a/atos/modules/SampleModule/inc/samplemodule.hpp +++ b/atos/modules/SampleModule/inc/samplemodule.hpp @@ -6,21 +6,23 @@ #pragma once -#include -#include -#include "roschannels/test_channels.hpp" #include "module.hpp" +#include "roschannels/test_channels.hpp" #include "server.hpp" +#include +#include /*! - * \brief The SampleModule is a ros2 node that demonstrates how to use the Module class + * \brief The SampleModule is a ros2 node that demonstrates how to use the Module class */ -class SampleModule : public Module{ +class SampleModule : public Module { public: static inline std::string const moduleName = "sample_module"; SampleModule(); std::vector getObjectIds(); - bool getAborting() const { return aborting_; } + bool getAborting() const { + return aborting_; + } private: ROSChannels::Init::Sub initSub; @@ -32,8 +34,8 @@ class SampleModule : public Module{ void onInitMessage(ROSChannels::Init::message_type::SharedPtr) override; void onAbortMessage(ROSChannels::Abort::message_type::SharedPtr) override; void onAllClearMessage(ROSChannels::AllClear::message_type::SharedPtr) override; - void OnCallbackSetBool(const std::shared_ptr request, - std::shared_ptr response); + void OnCallbackSetBool(const std::shared_ptr request, + std::shared_ptr response); std::vector objectIds; bool aborting_ = false; diff --git a/atos/modules/SampleModule/src/main.cpp b/atos/modules/SampleModule/src/main.cpp index fcdbc8f07..81223d352 100644 --- a/atos/modules/SampleModule/src/main.cpp +++ b/atos/modules/SampleModule/src/main.cpp @@ -1,9 +1,8 @@ #include "rclcpp/rclcpp.hpp" #include "samplemodule.hpp" - int main(int argc, char** argv) { - rclcpp::init(argc,argv); + rclcpp::init(argc, argv); auto sm = std::make_shared(); rclcpp::spin(sm); rclcpp::shutdown(); diff --git a/atos/modules/SampleModule/src/samplemodule.cpp b/atos/modules/SampleModule/src/samplemodule.cpp index 254c63361..45f5e99ef 100644 --- a/atos/modules/SampleModule/src/samplemodule.cpp +++ b/atos/modules/SampleModule/src/samplemodule.cpp @@ -1,20 +1,19 @@ #include "samplemodule.hpp" -#include "roschannels/commandchannels.hpp" #include "rclcpp/wait_for_message.hpp" +#include "roschannels/commandchannels.hpp" using namespace ROSChannels; using namespace std::chrono_literals; using namespace std::placeholders; SampleModule::SampleModule() : - Module(moduleName), - initSub(*this, std::bind(&SampleModule::onInitMessage, this, _1)), - abortSub(*this, std::bind(&SampleModule::onAbortMessage, this, _1)), - allClearSub(*this, std::bind(&SampleModule::onAllClearMessage, this, _1)), - smOnInitResponsePub(*this) -{ - service_server = create_service( - "/sample_module_test_service", std::bind(&SampleModule::OnCallbackSetBool, this, _1, _2)); + Module(moduleName), + initSub(*this, std::bind(&SampleModule::onInitMessage, this, _1)), + abortSub(*this, std::bind(&SampleModule::onAbortMessage, this, _1)), + allClearSub(*this, std::bind(&SampleModule::onAllClearMessage, this, _1)), + smOnInitResponsePub(*this) { + service_server = create_service("/sample_module_test_service", + std::bind(&SampleModule::OnCallbackSetBool, this, _1, _2)); } //! Message queue callbacks @@ -24,7 +23,8 @@ void SampleModule::onInitMessage(const Init::message_type::SharedPtr) { // Get the object id of the objects in the scenario ConnectedObjectIds::message_type connectedObjectIds; // Wait for a single message containing a vector of ids, at most 1000ms - rclcpp::wait_for_message(connectedObjectIds, shared_from_this(), std::string(get_namespace()) + "connected_object_ids", 1000ms); + rclcpp::wait_for_message( + connectedObjectIds, shared_from_this(), std::string(get_namespace()) + "connected_object_ids", 1000ms); // Populate a list of object IDs for (auto id : connectedObjectIds.ids) { objectIds.push_back(id); @@ -38,17 +38,15 @@ void SampleModule::onAbortMessage(const Abort::message_type::SharedPtr) { void SampleModule::onAllClearMessage(const AllClear::message_type::SharedPtr) {} - void SampleModule::OnCallbackSetBool(const std::shared_ptr, - std::shared_ptr response) -{ + std::shared_ptr response) { response->message = "succeeded"; response->success = true; } /*! \brief returns the object ids of the objects in the scenario * \return a vector of object ids -*/ + */ std::vector SampleModule::getObjectIds() { return objectIds; -} \ No newline at end of file +} diff --git a/atos/modules/SampleModule/test/main.cpp b/atos/modules/SampleModule/test/main.cpp index 92cbc41f1..83cca6f1a 100644 --- a/atos/modules/SampleModule/test/main.cpp +++ b/atos/modules/SampleModule/test/main.cpp @@ -1,7 +1,6 @@ #include "gtest/gtest.h" -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/atos/modules/SampleModule/test/test_samplemodule.cpp b/atos/modules/SampleModule/test/test_samplemodule.cpp index 83b1b4b5d..0ec1dfdff 100644 --- a/atos/modules/SampleModule/test/test_samplemodule.cpp +++ b/atos/modules/SampleModule/test/test_samplemodule.cpp @@ -1,110 +1,108 @@ -#include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "testUtils/testUtils.hpp" #include "samplemodule.hpp" +#include "testUtils/testUtils.hpp" +#include "gtest/gtest.h" class SampleModuleTest : public ::testing::Test { public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - protected: - - std::shared_ptr sampleModule; - std::shared_ptr helperNode; - rclcpp::executors::SingleThreadedExecutor executor; - - void SetUp() override - { - helperNode = rclcpp::Node::make_shared("SampleModuleTestHelper_node"); - sampleModule = std::make_shared(); - executor.add_node(helperNode); - executor.add_node(sampleModule); - } - - void TearDown() override - { - executor.remove_node(helperNode); - executor.remove_node(sampleModule); - helperNode.reset(); - sampleModule.reset(); - } + static void SetUpTestCase() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr sampleModule; + std::shared_ptr helperNode; + rclcpp::executors::SingleThreadedExecutor executor; + + void SetUp() override { + helperNode = rclcpp::Node::make_shared("SampleModuleTestHelper_node"); + sampleModule = std::make_shared(); + executor.add_node(helperNode); + executor.add_node(sampleModule); + } + + void TearDown() override { + executor.remove_node(helperNode); + executor.remove_node(sampleModule); + helperNode.reset(); + sampleModule.reset(); + } }; -TEST_F(SampleModuleTest, testModuleDoesNotAbortOnStartup){ - ASSERT_EQ(sampleModule->getAborting(), false); +TEST_F(SampleModuleTest, testModuleDoesNotAbortOnStartup) { + ASSERT_EQ(sampleModule->getAborting(), false); } -TEST_F(SampleModuleTest, testObjectIdsEmptyOnStartup){ - std::vector objectIds = sampleModule->getObjectIds(); - ASSERT_EQ((int)objectIds.size(), 0); +TEST_F(SampleModuleTest, testObjectIdsEmptyOnStartup) { + std::vector objectIds = sampleModule->getObjectIds(); + ASSERT_EQ((int)objectIds.size(), 0); } -TEST_F(SampleModuleTest, testSetsAbortingWhenAbortMessagePublished){ - // Setup - auto abortPublisher = ROSChannels::Abort::Pub(*helperNode); - - // Act - abortPublisher.publish(ROSChannels::Abort::message_type()); - executor.spin_some(std::chrono::milliseconds(300)); +TEST_F(SampleModuleTest, testSetsAbortingWhenAbortMessagePublished) { + // Setup + auto abortPublisher = ROSChannels::Abort::Pub(*helperNode); + + // Act + abortPublisher.publish(ROSChannels::Abort::message_type()); + executor.spin_some(std::chrono::milliseconds(300)); - // Assert - ASSERT_EQ(sampleModule->getAborting(), true); + // Assert + ASSERT_EQ(sampleModule->getAborting(), true); } -TEST_F(SampleModuleTest, testPublishesOnInitResponseWhenInitIsPublished){ - // Setup - std::promise subCalled; - std::shared_future subCalledFuture(subCalled.get_future()); - auto failAfterTimeout = std::chrono::milliseconds(5000); - - bool receivedMsg = false; - auto smOnInitResponseCallback = [&receivedMsg, &subCalled](const ROSChannels::SampleModuleTestForInitResponse::message_type::SharedPtr) { - receivedMsg = true; - subCalled.set_value(); - }; - - std::string topicname = std::string(sampleModule->get_namespace()) + ROSChannels::SampleModuleTestForInitResponse::topicName; - auto sub = ROSChannels::SampleModuleTestForInitResponse::Sub(*helperNode, smOnInitResponseCallback); - - // Act - auto initPublisher = ROSChannels::Init::Pub(*helperNode); - initPublisher.publish(ROSChannels::Init::message_type()); - // wait for discovery and the subscriber to connect - testUtils::waitForSubscriber(helperNode, topicname); - testUtils::waitForFuture(executor, subCalledFuture, failAfterTimeout); - - // Assert - ASSERT_EQ(receivedMsg, true); +TEST_F(SampleModuleTest, testPublishesOnInitResponseWhenInitIsPublished) { + // Setup + std::promise subCalled; + std::shared_future subCalledFuture(subCalled.get_future()); + auto failAfterTimeout = std::chrono::milliseconds(5000); + + bool receivedMsg = false; + auto smOnInitResponseCallback = + [&receivedMsg, &subCalled](const ROSChannels::SampleModuleTestForInitResponse::message_type::SharedPtr) { + receivedMsg = true; + subCalled.set_value(); + }; + + std::string topicname = + std::string(sampleModule->get_namespace()) + ROSChannels::SampleModuleTestForInitResponse::topicName; + auto sub = ROSChannels::SampleModuleTestForInitResponse::Sub(*helperNode, smOnInitResponseCallback); + + // Act + auto initPublisher = ROSChannels::Init::Pub(*helperNode); + initPublisher.publish(ROSChannels::Init::message_type()); + // wait for discovery and the subscriber to connect + testUtils::waitForSubscriber(helperNode, topicname); + testUtils::waitForFuture(executor, subCalledFuture, failAfterTimeout); + + // Assert + ASSERT_EQ(receivedMsg, true); } -TEST_F(SampleModuleTest, testThatServiceResponseIsSetToSuccessWhenServiceIsCalled){ - // Setup - std::promise serviceCalled; - std::shared_future serviceCalledFuture(serviceCalled.get_future()); - auto failAfterTimeout = std::chrono::milliseconds(1000); - bool receivedReq = false; - - auto getObjectIdsClient = helperNode->create_client("/sample_module_test_service"); - auto getObjectIdsRequest = std::make_shared(); - - auto testServiceCalledCallback = [&receivedReq, &serviceCalled](const rclcpp::Client::SharedFuture response) { - receivedReq = response.get()->success; - serviceCalled.set_value(); - }; - - // Act - getObjectIdsClient->async_send_request(getObjectIdsRequest, testServiceCalledCallback); - // wait for the service to be available and for it to process the request - testUtils::waitForService(getObjectIdsClient, std::chrono::milliseconds(1000)); - testUtils::waitForFuture(executor, serviceCalledFuture, failAfterTimeout); - - // Assert - ASSERT_EQ(receivedReq, true); -} \ No newline at end of file +TEST_F(SampleModuleTest, testThatServiceResponseIsSetToSuccessWhenServiceIsCalled) { + // Setup + std::promise serviceCalled; + std::shared_future serviceCalledFuture(serviceCalled.get_future()); + auto failAfterTimeout = std::chrono::milliseconds(1000); + bool receivedReq = false; + + auto getObjectIdsClient = helperNode->create_client("/sample_module_test_service"); + auto getObjectIdsRequest = std::make_shared(); + + auto testServiceCalledCallback = + [&receivedReq, &serviceCalled](const rclcpp::Client::SharedFuture response) { + receivedReq = response.get()->success; + serviceCalled.set_value(); + }; + + // Act + getObjectIdsClient->async_send_request(getObjectIdsRequest, testServiceCalledCallback); + // wait for the service to be available and for it to process the request + testUtils::waitForService(getObjectIdsClient, std::chrono::milliseconds(1000)); + testUtils::waitForFuture(executor, serviceCalledFuture, failAfterTimeout); + + // Assert + ASSERT_EQ(receivedReq, true); +} diff --git a/atos/modules/SystemControl/inc/systemcontrol.hpp b/atos/modules/SystemControl/inc/systemcontrol.hpp index 8a51c0423..a953d11ba 100644 --- a/atos/modules/SystemControl/inc/systemcontrol.hpp +++ b/atos/modules/SystemControl/inc/systemcontrol.hpp @@ -5,38 +5,35 @@ */ #include "module.hpp" -#include -#include -#include -#include +#include "atosTime.h" +#include "datadictionary.h" +#include "roschannels/commandchannels.hpp" +#include "roschannels/remotecontrolchannels.hpp" +#include "util.h" +#include #include -#include -#include -#include #include -#include -#include +#include +#include +#include +#include #include -#include +#include +#include +#include +#include #include +#include +#include +#include #include -#include -#include -#include -#include +#include #include -#include -#include "atosTime.h" -#include "datadictionary.h" -#include "util.h" -#include "roschannels/commandchannels.hpp" -#include "roschannels/remotecontrolchannels.hpp" #include #include -class SystemControl : public Module -{ +class SystemControl : public Module { public: SystemControl(); const int64_t getQueueEmptyPollPeriod(); @@ -51,37 +48,37 @@ class SystemControl : public Module private: static inline std::string const module_name = "system_control"; - //const std::string module_name = std::string("SystemControl"); + // const std::string module_name = std::string("SystemControl"); /* constants and datatypes */ const int64_t QUEUE_EMPTY_POLL_PERIOD_NS = 10000000; - static const int SYSTEM_CONTROL_RESPONSE_CODE_OK = 0x0001; - static const int SYSTEM_CONTROL_RESPONSE_CODE_ERROR = 0x0F10; + static const int SYSTEM_CONTROL_RESPONSE_CODE_OK = 0x0001; + static const int SYSTEM_CONTROL_RESPONSE_CODE_ERROR = 0x0F10; static const int SYSTEM_CONTROL_RESPONSE_CODE_FUNCTION_NOT_AVAILABLE = 0x0F20; - static const int SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE = 0x0F25; + static const int SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE = 0x0F25; - static const int SYSTEM_CONTROL_RESPONSE_CODE_INVALID_LENGTH = 0x0F30; - static const int SYSTEM_CONTROL_RESPONSE_CODE_BUSY = 0x0F40; - static const int SYSTEM_CONTROL_RESPONSE_CODE_INVALID_SCRIPT = 0x0F50; + static const int SYSTEM_CONTROL_RESPONSE_CODE_INVALID_LENGTH = 0x0F30; + static const int SYSTEM_CONTROL_RESPONSE_CODE_BUSY = 0x0F40; + static const int SYSTEM_CONTROL_RESPONSE_CODE_INVALID_SCRIPT = 0x0F50; static const int SYSTEM_CONTROL_RESPONSE_CODE_INVALID_ENCRYPTION_CODE = 0x0F60; - static const int SYSTEM_CONTROL_RESPONSE_CODE_DECRYPTION_ERROR = 0x0F61; - static const int SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA = 0x0F62; + static const int SYSTEM_CONTROL_RESPONSE_CODE_DECRYPTION_ERROR = 0x0F61; + static const int SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA = 0x0F62; - static const int SMALL_BUFFER_SIZE_6 = 6; - static const int SMALL_BUFFER_SIZE_16 = 16; - static const int SMALL_BUFFER_SIZE_20 = 20; - static const int SMALL_BUFFER_SIZE_128 = 128; + static const int SMALL_BUFFER_SIZE_6 = 6; + static const int SMALL_BUFFER_SIZE_16 = 16; + static const int SMALL_BUFFER_SIZE_20 = 20; + static const int SMALL_BUFFER_SIZE_128 = 128; static const int SMALL_BUFFER_SIZE_1024 = 1024; - static const int SYSTEM_CONTROL_ARG_CHAR_COUNT = 2; - static const int SYSTEM_CONTROL_COMMAND_MAX_LENGTH = 32; - static const int SYSTEM_CONTROL_ARG_MAX_COUNT = 6; + static const int SYSTEM_CONTROL_ARG_CHAR_COUNT = 2; + static const int SYSTEM_CONTROL_COMMAND_MAX_LENGTH = 32; + static const int SYSTEM_CONTROL_ARG_MAX_COUNT = 6; static const int SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH = 32; static const int SYSTEM_CONTROL_RVSS_DATA_BUFFER = 128; static const int TCP_RECV_BUFFER_SIZE = 2048; - + static const int SYSTEM_CONTROL_SEND_BUFFER_SIZE = 1024; static const int SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE = 64; @@ -90,7 +87,7 @@ class SystemControl : public Module static const int SYSTEM_CONTROL_RX_PACKET_SIZE = 1280; static const int SYSTEM_CONTROL_TX_PACKET_SIZE = SYSTEM_CONTROL_RX_PACKET_SIZE; - static const int IPC_BUFFER_SIZE = SYSTEM_CONTROL_RX_PACKET_SIZE; + static const int IPC_BUFFER_SIZE = SYSTEM_CONTROL_RX_PACKET_SIZE; typedef enum { SERVER_STATE_UNDEFINED, @@ -103,29 +100,55 @@ class SystemControl : public Module } ServerState_t; typedef enum { - Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, - InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, - SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, - GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, + Idle_0, + GetServerStatus_0, + ArmScenario_0, + DisarmScenario_0, + StartScenario_1, + stop_0, + AbortScenario_0, + InitializeScenario_0, + ConnectObject_0, + DisconnectObject_0, + GetServerParameterList_0, + SetServerParameter_2, + GetServerParameter_1, + DownloadFile_1, + UploadFile_4, + CheckFileDirectoryExist_1, + GetRootDirectoryContent_0, + GetDirectoryContent_1, + DeleteTrajectory_1, + DeleteGeofence_1, DeleteFileDirectory_1, - ClearTrajectories_0, ClearGeofences_0, ClearObjects_0, RemoteControl_1, RemoteControlManoeuvre_2, + ClearTrajectories_0, + ClearGeofences_0, + ClearObjects_0, + RemoteControl_1, + RemoteControlManoeuvre_2, SetObjectEnableStatus_2, GetObjectEnableStatus_1, - CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, - start_ext_trigg_1, ClearAllScenario_0 , DownloadDirectoryContent_1, DownloadTrajFiles_0, nocommand + CreateDirectory_1, + GetTestOrigin_0, + replay_1, + control_0, + Exit_0, + start_ext_trigg_1, + ClearAllScenario_0, + DownloadDirectoryContent_1, + DownloadTrajFiles_0, + nocommand } SystemControlCommand_t; struct content_dir_info { int exist; int fd; - char *info_buffer; + char* info_buffer; int size; }; - typedef enum { - MSCP_BACK_TO_START = 3 - } MSCPRemoteControlCommand; - + typedef enum { MSCP_BACK_TO_START = 3 } MSCPRemoteControlCommand; + /* callbacks */ void onGetStatusResponse(const ROSChannels::GetStatusResponse::message_type::SharedPtr) override; void onFailureMessage(const ROSChannels::Failure::message_type::SharedPtr) override; @@ -134,116 +157,157 @@ class SystemControl : public Module void onAllClearMessage(const ROSChannels::AllClear::message_type::SharedPtr) override; /* methods */ - SystemControlCommand_t SystemControlFindCommand(const char *CommandBuffer, - SystemControlCommand_t * CurrentCommand, int *ArgCount); - I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct in_addr *ip_addr); - I32 SystemControlConnectServer(int *sockfd, const char *name, const uint32_t port); - void SystemControlSendBytes(const char *data, int length, int *sockfd, int debug); - void SystemControlSendControlResponse(U16 ResponseStatus, const char * ResponseString, const char * ResponseData, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug); - I32 SystemControlBuildControlResponse(U16 ResponseStatus, char * ResponseString, char * ResponseData, - I32 ResponseDataLength, U8 Debug); - void SystemControlFileDownloadResponse(U16 ResponseStatus, const char * ResponseString, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug); - void SystemControlSendLog(const char * LogString, I32 * Sockfd, U8 Debug); - void SystemControlSendMONR(const char * LogString, I32 * Sockfd, U8 Debug); - void SystemControlCreateProcessChannel(const char * name, const U32 port, I32 * sockfd, - struct sockaddr_in *addr); - //I32 SystemControlSendUDPData(I32 *sockfd, struct sockaddr_in* addr, char *SendData, I32 Length, U8 debug); - I32 SystemControlReadServerParameterList(char * ParameterList, U8 debug); - I32 SystemControlGetServerParameter(char * ParameterName, char * ReturnValue, U32 BufferLength, - U8 Debug); - I32 SystemControlSetServerParameter(char * ParameterName, char * NewValue, U8 Debug); - I32 SystemControlCheckFileDirectoryExist(char * ParameterName, char * ReturnValue, U8 Debug); - I32 SystemControlUploadFile(const char * Filename, const char * FileSize, const char * PacketSize, const char * FileType, char * ReturnValue, - char * CompleteFilePath, U8 Debug); - I32 SystemControlReceiveRxData(I32 * sockfd, const char * Path, const char * FileSize, const char * PacketSize, char * ReturnValue, + SystemControlCommand_t SystemControlFindCommand(const char* CommandBuffer, + SystemControlCommand_t* CurrentCommand, + int* ArgCount); + I32 SystemControlInitServer(int* ClientSocket, int* ServerHandle, struct in_addr* ip_addr); + I32 SystemControlConnectServer(int* sockfd, const char* name, const uint32_t port); + void SystemControlSendBytes(const char* data, int length, int* sockfd, int debug); + void SystemControlSendControlResponse(U16 ResponseStatus, + const char* ResponseString, + const char* ResponseData, + I32 ResponseDataLength, + I32* Sockfd, + U8 Debug); + I32 SystemControlBuildControlResponse(U16 ResponseStatus, + char* ResponseString, + char* ResponseData, + I32 ResponseDataLength, + U8 Debug); + void SystemControlFileDownloadResponse(U16 ResponseStatus, + const char* ResponseString, + I32 ResponseDataLength, + I32* Sockfd, + U8 Debug); + void SystemControlSendLog(const char* LogString, I32* Sockfd, U8 Debug); + void SystemControlSendMONR(const char* LogString, I32* Sockfd, U8 Debug); + void SystemControlCreateProcessChannel(const char* name, const U32 port, I32* sockfd, struct sockaddr_in* addr); + // I32 SystemControlSendUDPData(I32 *sockfd, struct sockaddr_in* addr, char *SendData, I32 Length, U8 debug); + I32 SystemControlReadServerParameterList(char* ParameterList, U8 debug); + I32 SystemControlGetServerParameter(char* ParameterName, char* ReturnValue, U32 BufferLength, U8 Debug); + I32 SystemControlSetServerParameter(char* ParameterName, char* NewValue, U8 Debug); + I32 SystemControlCheckFileDirectoryExist(char* ParameterName, char* ReturnValue, U8 Debug); + I32 SystemControlUploadFile(const char* Filename, + const char* FileSize, + const char* PacketSize, + const char* FileType, + char* ReturnValue, + char* CompleteFilePath, U8 Debug); - char SystemControlDeleteTrajectory(const char * trajectoryName, const size_t nameLen); - char SystemControlDeleteGeofence(const char * geofenceName, const size_t nameLen); - char SystemControlDeleteGenericFile(const char * filePath, const size_t nameLen); + I32 SystemControlReceiveRxData(I32* sockfd, + const char* Path, + const char* FileSize, + const char* PacketSize, + char* ReturnValue, + U8 Debug); + char SystemControlDeleteTrajectory(const char* trajectoryName, const size_t nameLen); + char SystemControlDeleteGeofence(const char* geofenceName, const size_t nameLen); + char SystemControlDeleteGenericFile(const char* filePath, const size_t nameLen); char SystemControlClearTrajectories(void); char SystemControlClearGeofences(void); char SystemControlClearObjects(void); - I32 SystemControlDeleteFileDirectory(const char * Path, char * ReturnValue, U8 Debug); - I32 SystemControlBuildFileContentInfo(const char * Path, U8 Debug); - I32 SystemControlDestroyFileContentInfo(const char * Path, U8 RemoveFile); - I32 SystemControlSendFileContent(I32 * sockfd, const char * Path, const char * PacketSize, char * ReturnValue, U8 Remove, - U8 Debug); - I32 SystemControlCreateDirectory(const char * Path, char * ReturnValue, U8 Debug); - I32 SystemControlBuildRVSSTimeChannelMessage(char * RVSSData, U32 * RVSSDataLengthU32, - U8 Debug); - I32 SystemControlBuildRVSSATOSChannelMessage(char * RVSSData, U32 * RVSSDataLengthU32, - U8 SysCtrlState, U8 Debug); - I32 SystemControlBuildRVSSAspChannelMessage(char * RVSSData, U32 * RVSSDataLengthU32, U8 Debug); - int32_t SystemControlSendRVSSMonitorChannelMessages(int *socket, struct sockaddr_in *addr); - void SystemControlUpdateRVSSSendTime(struct timeval *currentRVSSSendTime, uint8_t RVSSRate_Hz); - - I32 SystemControlGetStatusMessage(const char *respondingModule, U8 debug); - - ssize_t SystemControlReceiveUserControlData(I32 socket, char * dataBuffer, size_t dataBufferLength); - char SystemControlVerifyHostAddress(char *ip); - - void appendSysInfoString(char *ControlResponseBuffer, const size_t bufferSize); + I32 SystemControlDeleteFileDirectory(const char* Path, char* ReturnValue, U8 Debug); + I32 SystemControlBuildFileContentInfo(const char* Path, U8 Debug); + I32 SystemControlDestroyFileContentInfo(const char* Path, U8 RemoveFile); + I32 SystemControlSendFileContent(I32* sockfd, + const char* Path, + const char* PacketSize, + char* ReturnValue, + U8 Remove, + U8 Debug); + I32 SystemControlCreateDirectory(const char* Path, char* ReturnValue, U8 Debug); + I32 SystemControlBuildRVSSTimeChannelMessage(char* RVSSData, U32* RVSSDataLengthU32, U8 Debug); + I32 SystemControlBuildRVSSATOSChannelMessage(char* RVSSData, U32* RVSSDataLengthU32, U8 SysCtrlState, U8 Debug); + I32 SystemControlBuildRVSSAspChannelMessage(char* RVSSData, U32* RVSSDataLengthU32, U8 Debug); + int32_t SystemControlSendRVSSMonitorChannelMessages(int* socket, struct sockaddr_in* addr); + void SystemControlUpdateRVSSSendTime(struct timeval* currentRVSSSendTime, uint8_t RVSSRate_Hz); + + I32 SystemControlGetStatusMessage(const char* respondingModule, U8 debug); + + ssize_t SystemControlReceiveUserControlData(I32 socket, char* dataBuffer, size_t dataBufferLength); + char SystemControlVerifyHostAddress(char* ip); + + void appendSysInfoString(char* ControlResponseBuffer, const size_t bufferSize); /* variables */ - const char *SystemControlStatesArr[7] = - { "UNDEFINED", "INITIALIZED", "IDLE", "READY", "RUNNING", "INWORK", "ERROR" }; - const char *SystemControlOBCStatesArr[8] = - { "UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "REMOTECONTROL", "ERROR" }; - const char *POSTRequestMandatoryContent[3] = { "POST", "HTTP/1.1", "\r\n\r\n" }; - const char *SystemControlCommandsArr[37] = { - "Idle_0", "GetServerStatus_0", "ArmScenario_0", "DisarmScenario_0", "StartScenario_1", "stop_0", - "AbortScenario_0", "InitializeScenario_0", - "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", - "GetServerParameter_1", "DownloadFile_1", "UploadFile_4", "CheckFileDirectoryExist_1", - "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteTrajectory_1", "DeleteGeofence_1", - "DeleteFileDirectory_1", - "ClearTrajectories_0", "ClearGeofences_0", "ClearObjects_0", "RemoteControl_1", - "RemoteControlManoeuvre_2", - "SetObjectEnableStatus_2", - "GetObjectEnableStatus_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", - "control_0", - "Exit_0", "start_ext_trigg_1", "ClearAllScenario_0", "DownloadDirectoryContent_1", "DownloadTrajFiles_0" - }; + const char* SystemControlStatesArr[7] = {"UNDEFINED", "INITIALIZED", "IDLE", "READY", "RUNNING", "INWORK", "ERROR"}; + const char* SystemControlOBCStatesArr[8] = + {"UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "REMOTECONTROL", "ERROR"}; + const char* POSTRequestMandatoryContent[3] = {"POST", "HTTP/1.1", "\r\n\r\n"}; + const char* SystemControlCommandsArr[37] = {"Idle_0", + "GetServerStatus_0", + "ArmScenario_0", + "DisarmScenario_0", + "StartScenario_1", + "stop_0", + "AbortScenario_0", + "InitializeScenario_0", + "ConnectObject_0", + "DisconnectObject_0", + "GetServerParameterList_0", + "SetServerParameter_2", + "GetServerParameter_1", + "DownloadFile_1", + "UploadFile_4", + "CheckFileDirectoryExist_1", + "GetRootDirectoryContent_0", + "GetDirectoryContent_1", + "DeleteTrajectory_1", + "DeleteGeofence_1", + "DeleteFileDirectory_1", + "ClearTrajectories_0", + "ClearGeofences_0", + "ClearObjects_0", + "RemoteControl_1", + "RemoteControlManoeuvre_2", + "SetObjectEnableStatus_2", + "GetObjectEnableStatus_1", + "CreateDirectory_1", + "GetTestOrigin_0", + "replay_1", + "control_0", + "Exit_0", + "start_ext_trigg_1", + "ClearAllScenario_0", + "DownloadDirectoryContent_1", + "DownloadTrajFiles_0"}; char SystemControlCommandArgCnt[SYSTEM_CONTROL_ARG_CHAR_COUNT]; char SystemControlStrippedCommand[SYSTEM_CONTROL_COMMAND_MAX_LENGTH]; char SystemControlArgument[SYSTEM_CONTROL_ARG_MAX_COUNT][SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH]; - const char * STR_SYSTEM_CONTROL_RX_PACKET_SIZE="1280"; - const char * STR_SYSTEM_CONTROL_TX_PACKET_SIZE="1200"; + const char* STR_SYSTEM_CONTROL_RX_PACKET_SIZE = "1280"; + const char* STR_SYSTEM_CONTROL_TX_PACKET_SIZE = "1200"; content_dir_info SystemControlDirectoryInfo; U8 ModeU8 = 0; ServiceSessionType SessionData; struct timeval CurrentTimeStruct; - I32 FileLengthI32 = 0; - U8 RVSSRateU8 = DEFAULT_RVSS_RATE; - U32 RVSSConfigU32 = DEFAULT_RVSS_CONF; - U64 OldTimeU64 = 0; - U64 PollRateU64 = 0; + I32 FileLengthI32 = 0; + U8 RVSSRateU8 = DEFAULT_RVSS_RATE; + U32 RVSSConfigU32 = DEFAULT_RVSS_CONF; + U64 OldTimeU64 = 0; + U64 PollRateU64 = 0; U64 CurrentTimeU64 = 0; volatile int iExit; - I32 ClientResult = 0; + I32 ClientResult = 0; ServerState_t SystemControlState = SERVER_STATE_UNDEFINED; I32 ServerHandle; I32 ClientSocket = 0; struct sockaddr_in RVSSChannelAddr; struct in_addr ip_addr; I32 RVSSChannelSocket; - struct timeval nextRVSSSendTime = { 0, 0 }; + struct timeval nextRVSSSendTime = {0, 0}; - OBCState_t objectControlState = OBC_STATE_UNDEFINED; - SystemControlCommand_t SystemControlCommand = Idle_0; + OBCState_t objectControlState = OBC_STATE_UNDEFINED; + SystemControlCommand_t SystemControlCommand = Idle_0; SystemControlCommand_t PreviousSystemControlCommand = Idle_0; - uint16_t responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; + uint16_t responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; int CommandArgCount = 0, CurrentInputArgCount = 0; char pcBuffer[IPC_BUFFER_SIZE]; char inchr; struct timeval tvTime; - const struct timeval VirtualMachineLagCompensation = { VIRTUAL_MACHINE_LAG_COMPENSATION_S, - VIRTUAL_MACHINE_LAG_COMPENSATION_US - }; + const struct timeval VirtualMachineLagCompensation = {VIRTUAL_MACHINE_LAG_COMPENSATION_S, + VIRTUAL_MACHINE_LAG_COMPENSATION_US}; ObjectPosition OP; int i, i1; char *StartPtr, *StopPtr, *CmdPtr, *OpeningQuotationMarkPtr, *ClosingQuotationMarkPtr, *StringPos; @@ -299,6 +363,4 @@ class SystemControl : public Module ROSChannels::Failure::Sub failureSub; ROSChannels::GetStatusResponse::Sub getStatusResponseSub; - - }; diff --git a/atos/modules/SystemControl/src/main.cpp b/atos/modules/SystemControl/src/main.cpp index 7cbf827e1..73624aed7 100644 --- a/atos/modules/SystemControl/src/main.cpp +++ b/atos/modules/SystemControl/src/main.cpp @@ -3,8 +3,8 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include "systemcontrol.hpp" #include "rclcpp/executor.hpp" +#include "systemcontrol.hpp" using namespace std::chrono; @@ -12,7 +12,7 @@ using namespace std::chrono; static std::shared_ptr sc; static void systemcontrol_task(int argc, char** argv); -int main(int argc, char** argv){ +int main(int argc, char** argv) { systemcontrol_task(argc, argv); return 0; } @@ -20,30 +20,30 @@ int main(int argc, char** argv){ /* Initializes Logs, Shared memory and mode, then runs the main loop */ -void systemcontrol_task(int argc, char** argv){ +void systemcontrol_task(int argc, char** argv) { // Initialize ROS node rclcpp::init(argc, argv); sc = std::make_shared(); sc->initialize(); - + // Set up signal handlers - auto f = [](int signo) -> void {sc->signalHandler(signo);}; - if (std::signal(SIGINT,f) == SIG_ERR){ + auto f = [](int signo) -> void { sc->signalHandler(signo); }; + if (std::signal(SIGINT, f) == SIG_ERR) { util_error("Unable to initialize signal handler"); } rclcpp::executors::SingleThreadedExecutor executor; - while (rclcpp::ok() && !sc->shouldExit()){ + while (rclcpp::ok() && !sc->shouldExit()) { sc->receiveUserCommand(); sc->processUserCommand(); sc->sendUnsolicitedData(); sc->pollModuleStatus(); - // If SystemControl is not in work state and clientResult < 0 then wait at most QUEUE_EMPTY_POLL_PERIOD for a msg - // else keep running at full speed. - const int64_t sleeptime=sc->isWorking() ? 0 : sc->getQueueEmptyPollPeriod(); - executor.spin_node_once(sc,duration(sleeptime)); + // If SystemControl is not in work state and clientResult < 0 then wait at most QUEUE_EMPTY_POLL_PERIOD for a + // msg else keep running at full speed. + const int64_t sleeptime = sc->isWorking() ? 0 : sc->getQueueEmptyPollPeriod(); + executor.spin_node_once(sc, duration(sleeptime)); } ReadWriteAccess_t dataDictOperationResult = DataDictionaryDestructor(); if (dataDictOperationResult != WRITE_OK && dataDictOperationResult != READ_WRITE_OK) { diff --git a/atos/modules/SystemControl/src/systemcontrol.cpp b/atos/modules/SystemControl/src/systemcontrol.cpp index acdfd8d6e..910504b52 100644 --- a/atos/modules/SystemControl/src/systemcontrol.cpp +++ b/atos/modules/SystemControl/src/systemcontrol.cpp @@ -13,10 +13,12 @@ #define SYSTEM_CONTROL_GETSTATUS_TIME_MS 5000 #define SYSTEM_CONTROL_GETSTATUS_TIMEOUT_MS 2000 -#define SYSTEM_CONTROL_NO_OF_MODULES_IN_USE 2 //TODO Create a file containing a list of which modules should be used. Check with this list to see if each module has responded. +#define SYSTEM_CONTROL_NO_OF_MODULES_IN_USE \ + 2 // TODO Create a file containing a list of which modules should be used. Check with this list to see if each + // module has responded. -#define SYSTEM_CONTROL_CONTROL_PORT 54241 // Default port, control channel -#define SYSTEM_CONTROL_PROCESS_PORT 54242 // Default port, process channel +#define SYSTEM_CONTROL_CONTROL_PORT 54241 // Default port, control channel +#define SYSTEM_CONTROL_PROCESS_PORT 54242 // Default port, process channel #define GetCurrentDir getcwd #define REMOVE_FILE 1 @@ -30,11 +32,11 @@ #define ENABLE_COMMAND_STRING "ENABLE" #define DISABLE_COMMAND_STRING "DISABLE" -#define ATOS_GENERIC_FILE_TYPE 1 -#define ATOS_TRAJ_FILE_TYPE 2 -#define ATOS_CONF_FILE_TYPE 3 -#define ATOS_GEOFENCE_FILE_TYPE 4 -#define ATOS_OBJECT_FILE_TYPE 5 +#define ATOS_GENERIC_FILE_TYPE 1 +#define ATOS_TRAJ_FILE_TYPE 2 +#define ATOS_CONF_FILE_TYPE 3 +#define ATOS_GEOFENCE_FILE_TYPE 4 +#define ATOS_OBJECT_FILE_TYPE 5 #define MSCP_RESPONSE_DATALENGTH_BYTES 4 #define MSCP_RESPONSE_STATUS_CODE_BYTES 2 @@ -43,74 +45,75 @@ using namespace ROSChannels; using std::placeholders::_1; -SystemControl::SystemControl() -: Module(SystemControl::module_name), -initPub(*this), -connectPub(*this), -disconnectPub(*this), -armPub(*this), -disarmPub(*this), -startPub(*this), -stopPub(*this), -abortPub(*this), -allClearPub(*this), -backToStartPub(*this), -remoteControlEnablePub(*this), -remoteControlDisablePub(*this), -exitPub(*this), -getStatusPub(*this), -failureSub(*this, std::bind(&SystemControl::onFailureMessage, this, _1)), -getStatusResponseSub(*this, std::bind(&SystemControl::onGetStatusResponse, this, _1)) -{ -}; - -const int64_t SystemControl::getQueueEmptyPollPeriod(){return QUEUE_EMPTY_POLL_PERIOD_NS;} - -void SystemControl::onAbortMessage(const Abort::message_type::SharedPtr){} - -void SystemControl::onAllClearMessage(const AllClear::message_type::SharedPtr){} - -void SystemControl::onFailureMessage(const Failure::message_type::SharedPtr msg){ +SystemControl::SystemControl() : + Module(SystemControl::module_name), + initPub(*this), + connectPub(*this), + disconnectPub(*this), + armPub(*this), + disarmPub(*this), + startPub(*this), + stopPub(*this), + abortPub(*this), + allClearPub(*this), + backToStartPub(*this), + remoteControlEnablePub(*this), + remoteControlDisablePub(*this), + exitPub(*this), + getStatusPub(*this), + failureSub(*this, std::bind(&SystemControl::onFailureMessage, this, _1)), + getStatusResponseSub(*this, std::bind(&SystemControl::onGetStatusResponse, this, _1)) {}; + +const int64_t SystemControl::getQueueEmptyPollPeriod() { + return QUEUE_EMPTY_POLL_PERIOD_NS; +} + +void SystemControl::onAbortMessage(const Abort::message_type::SharedPtr) {} + +void SystemControl::onAllClearMessage(const AllClear::message_type::SharedPtr) {} + +void SystemControl::onFailureMessage(const Failure::message_type::SharedPtr msg) { if (SystemControlState == SERVER_STATE_INWORK) { - enum COMMAND failedCommand = (COMMAND) (msg->data); + enum COMMAND failedCommand = (COMMAND)(msg->data); if (failedCommand == COMM_INIT && PreviousSystemControlCommand == InitializeScenario_0) { - SystemControlState = SERVER_STATE_IDLE; + SystemControlState = SERVER_STATE_IDLE; SystemControlCommand = Idle_0; RCLCPP_INFO(get_logger(), "Initialization failed"); // TODO: report to user? + } else { + RCLCPP_ERROR(get_logger(), + "Unhandled FAILURE (command: %u) reply in state %s", + msg->data, + SystemControlStatesArr[SystemControlState]); } - else { - RCLCPP_ERROR(get_logger(), "Unhandled FAILURE (command: %u) reply in state %s", - msg->data, SystemControlStatesArr[SystemControlState]); - } - } - else { - RCLCPP_WARN(get_logger(), "Received unexpected FAILURE (command: %u) reply in state %s", - msg->data, SystemControlStatesArr[SystemControlState]); - // TODO: React more? + } else { + RCLCPP_WARN(get_logger(), + "Received unexpected FAILURE (command: %u) reply in state %s", + msg->data, + SystemControlStatesArr[SystemControlState]); + // TODO: React more? } } -void SystemControl::onGetStatusResponse(const GetStatusResponse::message_type::SharedPtr msg){ +void SystemControl::onGetStatusResponse(const GetStatusResponse::message_type::SharedPtr msg) { moduleResponseTable[msg->data] = std::chrono::steady_clock::now(); } -void SystemControl::onBackToStartResponse(const BackToStartResponse::message_type::SharedPtr msg){ - if(msg->data == BTS_FAIL){ +void SystemControl::onBackToStartResponse(const BackToStartResponse::message_type::SharedPtr msg) { + if (msg->data == BTS_FAIL) { RCLCPP_DEBUG(get_logger(), "Back-to-start result: %d", msg->data); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "BTS:", - "0", 1, - &ClientSocket, 0); //TODO: Send SMCP response the right way(?) - } - else if(msg->data == BTS_PASS){ + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, + "BTS:", + "0", + 1, + &ClientSocket, + 0); // TODO: Send SMCP response the right way(?) + } else if (msg->data == BTS_PASS) { RCLCPP_DEBUG(get_logger(), "Back-to-start result: %d", msg->data); - memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "BTS:", - "1", 1, - &ClientSocket, 0); - } - else{ + memset(ControlResponseBuffer, 0, sizeof(ControlResponseBuffer)); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "BTS:", "1", 1, &ClientSocket, 0); + } else { RCLCPP_ERROR(get_logger(), "Cannot parse COMM_BACKTOSTART_RESPONSE message."); } } @@ -119,20 +122,19 @@ void SystemControl::signalHandler(int signo) { if (signo == SIGINT) { RCLCPP_WARN(get_logger(), "Caught keyboard interrupt"); iExit = 1; - } - else { + } else { RCLCPP_ERROR(get_logger(), "Caught unhandled signal"); } } -bool SystemControl::isWorking(){ +bool SystemControl::isWorking() { return !(SystemControlState != SERVER_STATE_INWORK && (ClientResult < 0)); } -bool SystemControl::shouldExit(){ +bool SystemControl::shouldExit() { return iExit; } -void SystemControl::initialize(){ +void SystemControl::initialize() { if (requestDataDictInitialization()) { // Map state and object data into memory @@ -154,29 +156,23 @@ void SystemControl::initialize(){ DataDictionaryGetRVSSRateU8(&RVSSRateU8); RCLCPP_INFO(get_logger(), "Real-time variable subscription service rate set to %u Hz", RVSSRateU8); - } - else { + } else { throw std::runtime_error("Unable to initialize data dictionary"); } if (ModeU8 == 0) { - } - else if (ModeU8 == 1) { + } else if (ModeU8 == 1) { SessionData.SessionIdU32 = 0; - SessionData.UserIdU32 = 0; - SessionData.UserTypeU8 = 0; - - PollRateU64 = SYSTEM_CONTROL_SERVICE_POLL_TIME_MS; - CurrentTimeU64 = - (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; - OldTimeU64 = CurrentTimeU64; + SessionData.UserIdU32 = 0; + SessionData.UserTypeU8 = 0; + PollRateU64 = SYSTEM_CONTROL_SERVICE_POLL_TIME_MS; + CurrentTimeU64 = (uint64_t)CurrentTimeStruct.tv_sec * 1000 + (uint64_t)CurrentTimeStruct.tv_usec / 1000; + OldTimeU64 = CurrentTimeU64; } } - -void SystemControl::receiveUserCommand() -{ +void SystemControl::receiveUserCommand() { if (SystemControlState == SERVER_STATE_ERROR) { this->abortPub.publish(Abort::message_type()); return; @@ -185,7 +181,7 @@ void SystemControl::receiveUserCommand() if (ModeU8 == 0) { if (ClientSocket <= 0) { if (SystemControlState == SERVER_STATE_UNDEFINED) { - //Do some initialization + // Do some initialization SystemControlState = SERVER_STATE_INITIALIZED; } @@ -193,18 +189,16 @@ void SystemControl::receiveUserCommand() ClientResult = SystemControlInitServer(&ClientSocket, &ServerHandle, &ip_addr); bzero(UserControlIPchar, SMALL_BUFFER_SIZE_20); - sprintf((char* )UserControlIPchar, "%s", inet_ntoa(ip_addr)); + sprintf((char*)UserControlIPchar, "%s", inet_ntoa(ip_addr)); RCLCPP_INFO(get_logger(), "UserControl IP address is %s", inet_ntoa(ip_addr)); - SystemControlCreateProcessChannel(UserControlIPchar, SYSTEM_CONTROL_PROCESS_PORT, - &RVSSChannelSocket, &RVSSChannelAddr); - + SystemControlCreateProcessChannel( + UserControlIPchar, SYSTEM_CONTROL_PROCESS_PORT, &RVSSChannelSocket, &RVSSChannelAddr); } if (USE_LOCAL_USER_CONTROL == 1) { ClientResult = - SystemControlConnectServer(&ClientSocket, LOCAL_USER_CONTROL_IP, - LOCAL_USER_CONTROL_PORT); - SystemControlCreateProcessChannel(LOCAL_USER_CONTROL_IP, SYSTEM_CONTROL_PROCESS_PORT, - &RVSSChannelSocket, &RVSSChannelAddr); + SystemControlConnectServer(&ClientSocket, LOCAL_USER_CONTROL_IP, LOCAL_USER_CONTROL_PORT); + SystemControlCreateProcessChannel( + LOCAL_USER_CONTROL_IP, SYSTEM_CONTROL_PROCESS_PORT, &RVSSChannelSocket, &RVSSChannelAddr); } SystemControlState = SERVER_STATE_IDLE; @@ -213,20 +207,18 @@ void SystemControl::receiveUserCommand() PreviousSystemControlCommand = SystemControlCommand; bzero(pcBuffer, IPC_BUFFER_SIZE); - ClientResult = SystemControlReceiveUserControlData(ClientSocket, pcBuffer, sizeof (pcBuffer)); + ClientResult = SystemControlReceiveUserControlData(ClientSocket, pcBuffer, sizeof(pcBuffer)); if (ClientResult <= -1) { if (errno != EAGAIN && errno != EWOULDBLOCK) { RCLCPP_ERROR(get_logger(), "Failed to receive from command socket"); - try{ + try { this->abortPub.publish(Abort::message_type()); - } - catch(...){ + } catch (...) { util_error("Fatal communication fault when sending ABORT command"); } } - } - else if (ClientResult == 0) { + } else if (ClientResult == 0) { RCLCPP_INFO(get_logger(), "Client closed connection"); close(ClientSocket); ClientSocket = -1; @@ -235,31 +227,27 @@ void SystemControl::receiveUserCommand() ServerHandle = -1; } - SystemControlCommand = AbortScenario_0; //Oops no client is connected, go to AbortScenario_0 - SystemControlState == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? - } - else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { + SystemControlCommand = AbortScenario_0; // Oops no client is connected, go to AbortScenario_0 + SystemControlState == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? + } else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { // TODO: Move this entire decoding process into a separate function for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); CurrentInputArgCount = 0; - StartPtr = pcBuffer; - StopPtr = pcBuffer; - CmdPtr = NULL; - StringPos = pcBuffer; + StartPtr = pcBuffer; + StopPtr = pcBuffer; + CmdPtr = NULL; + StringPos = pcBuffer; // Check so that all POST request mandatory content is contained in the message - for (i = 0; - i < sizeof (POSTRequestMandatoryContent) / sizeof (POSTRequestMandatoryContent[0]); - ++i) { + for (i = 0; i < sizeof(POSTRequestMandatoryContent) / sizeof(POSTRequestMandatoryContent[0]); ++i) { StringPos = strstr(StringPos, POSTRequestMandatoryContent[i]); if (StringPos == NULL) { CmdPtr = NULL; break; - } - else { + } else { CmdPtr = StringPos + strlen(POSTRequestMandatoryContent[i]); } } @@ -270,53 +258,48 @@ void SystemControl::receiveUserCommand() if (HTTPHeader.Host[0] == '\0') { RCLCPP_INFO(get_logger(), "Unspecified host in request <%s>", pcBuffer); - } - else if (SystemControlVerifyHostAddress(HTTPHeader.Host)) { + } else if (SystemControlVerifyHostAddress(HTTPHeader.Host)) { // Find opening parenthesis StartPtr = strchr(CmdPtr, '('); // If there was no opening or closing parenthesis, the format is not correct if (StartPtr == NULL || strchr(StartPtr, ')') == NULL) - RCLCPP_WARN(get_logger(), - "Received command not conforming to MSCP standards"); + RCLCPP_WARN(get_logger(), "Received command not conforming to MSCP standards"); else { StartPtr++; while (StopPtr != NULL) { - StopPtr = (char *)strchr(StartPtr, ','); + StopPtr = (char*)strchr(StartPtr, ','); // If there are no commas past this point, just copy the rest if (StopPtr == NULL) { - strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, - (uint64_t) strchr(StartPtr, ')') - (uint64_t) StartPtr); + strncpy(SystemControlArgument[CurrentInputArgCount], + StartPtr, + (uint64_t)strchr(StartPtr, ')') - (uint64_t)StartPtr); } // Otherwise, check if the comma we found was inside quotation marks else { - OpeningQuotationMarkPtr = (char *)strchr(StartPtr, '"'); + OpeningQuotationMarkPtr = (char*)strchr(StartPtr, '"'); if (OpeningQuotationMarkPtr == NULL) { // It was not within quotation marks: copy until the next comma - strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, - (uint64_t) StopPtr - (uint64_t) StartPtr); - } - else if (OpeningQuotationMarkPtr != NULL - && OpeningQuotationMarkPtr < StopPtr) { - // A quotation mark was found and it was before the next comma: find the closing quotation mark - ClosingQuotationMarkPtr = - (char *)strchr(OpeningQuotationMarkPtr + 1, '"'); - + strncpy(SystemControlArgument[CurrentInputArgCount], + StartPtr, + (uint64_t)StopPtr - (uint64_t)StartPtr); + } else if (OpeningQuotationMarkPtr != NULL && OpeningQuotationMarkPtr < StopPtr) { + // A quotation mark was found and it was before the next comma: find the closing + // quotation mark + ClosingQuotationMarkPtr = (char*)strchr(OpeningQuotationMarkPtr + 1, '"'); if (ClosingQuotationMarkPtr == NULL) { - CmdPtr = NULL; + CmdPtr = NULL; StopPtr = NULL; - RCLCPP_WARN(get_logger(), - "Received MSCP command with single quotation mark"); + RCLCPP_WARN(get_logger(), "Received MSCP command with single quotation mark"); break; - } - else { + } else { // Copy all arguments within quotation marks including the quotation marks strncpy(SystemControlArgument[CurrentInputArgCount], OpeningQuotationMarkPtr + 1, - (uint64_t) (ClosingQuotationMarkPtr) - - (uint64_t) (OpeningQuotationMarkPtr + 1)); + (uint64_t)(ClosingQuotationMarkPtr) - + (uint64_t)(OpeningQuotationMarkPtr + 1)); // Find next comma after closing quotation mark StopPtr = strchr(ClosingQuotationMarkPtr, ','); } @@ -324,45 +307,39 @@ void SystemControl::receiveUserCommand() } StartPtr = StopPtr + 1; if (SystemControlArgument[CurrentInputArgCount][0] != '\0') { - CurrentInputArgCount++; // In case the argument was empty, don't add it to the argument count + CurrentInputArgCount++; // In case the argument was empty, don't add it to the argument + // count } } if (CmdPtr != NULL) { SystemControlFindCommand(CmdPtr, &SystemControlCommand, &CommandArgCount); RCLCPP_DEBUG(get_logger(), "Received MSCP command %s", SystemControlCommand); - } - else + } else RCLCPP_WARN(get_logger(), "Invalid MSCP command received"); } + } else { + RCLCPP_INFO( + get_logger(), "Request specified host <%s> not among known local addresses", HTTPHeader.Host); } - else { - RCLCPP_INFO(get_logger(), - "Request specified host <%s> not among known local addresses", - HTTPHeader.Host); - } - } - else { + } else { RCLCPP_WARN(get_logger(), "Received badly formatted HTTP request: <%s>, must contain \"POST\" and \"\\r\\n\\r\\n\"", pcBuffer); } - } - else { + } else { RCLCPP_WARN(get_logger(), "Ignored received TCP message which was too large to handle"); } - } - else if (ModeU8 == 1) { /* use util.c function to call time - gettimeofday(&CurrentTimeStruct, NULL); - CurrentTimeU64 = (uint64_t)CurrentTimeStruct.tv_sec*1000 + (uint64_t)CurrentTimeStruct.tv_usec/1000; - */ + } else if (ModeU8 == 1) { /* use util.c function to call time + gettimeofday(&CurrentTimeStruct, NULL); + CurrentTimeU64 = (uint64_t)CurrentTimeStruct.tv_sec*1000 + + (uint64_t)CurrentTimeStruct.tv_usec/1000; + */ CurrentTimeU64 = UtilgetCurrentUTCtimeMS(); - TimeDiffU64 = CurrentTimeU64 - OldTimeU64; - + TimeDiffU64 = CurrentTimeU64 - OldTimeU64; } - if (DataDictionaryGetOBCState(&objectControlState) != READ_OK) { RCLCPP_ERROR(get_logger(), "Error fetching object control state"); // TODO more? @@ -371,67 +348,74 @@ void SystemControl::receiveUserCommand() if (SystemControlState == SERVER_STATE_INWORK) { if (SystemControlCommand == AbortScenario_0) { SystemControlCommand = SystemControlCommand; - } - else if (SystemControlCommand == GetServerStatus_0) { - RCLCPP_DEBUG(get_logger(), "State: %s, OBCState: %s, PreviousCommand: %s", - SystemControlStatesArr[SystemControlState], - SystemControlOBCStatesArr[objectControlState], - SystemControlCommandsArr[PreviousSystemControlCommand]); + } else if (SystemControlCommand == GetServerStatus_0) { + RCLCPP_DEBUG(get_logger(), + "State: %s, OBCState: %s, PreviousCommand: %s", + SystemControlStatesArr[SystemControlState], + SystemControlOBCStatesArr[objectControlState], + SystemControlCommandsArr[PreviousSystemControlCommand]); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); ControlResponseBuffer[0] = SystemControlState; - ControlResponseBuffer[1] = objectControlState; //OBCStateU8; - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", - ControlResponseBuffer, 2, &ClientSocket, 0); + ControlResponseBuffer[1] = objectControlState; // OBCStateU8; + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", ControlResponseBuffer, 2, &ClientSocket, 0); SystemControlCommand = PreviousSystemControlCommand; - } - else if (SystemControlCommand != PreviousSystemControlCommand) { + } else if (SystemControlCommand != PreviousSystemControlCommand) { RCLCPP_WARN(get_logger(), "Command not allowed, SystemControl is busy in state %s, PreviousCommand: %s", SystemControlStatesArr[SystemControlState], SystemControlCommandsArr[PreviousSystemControlCommand]); - SystemControlSendLog - ("[SystemControl] Command not allowed, SystemControl is busy in state INWORK.\n", - &ClientSocket, 0); + SystemControlSendLog( + "[SystemControl] Command not allowed, SystemControl is busy in state INWORK.\n", &ClientSocket, 0); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, "", - ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, "", ControlResponseBuffer, 0, &ClientSocket, 0); SystemControlCommand = PreviousSystemControlCommand; } } } // Redeclaration of timercmp to avoid cppcheck flagging it as a bug -# define timercmp(a, b, CMP) \ - (((a)->tv_sec == (b)->tv_sec) \ - ? ((a)->tv_usec CMP (b)->tv_usec) \ - : ((a)->tv_sec CMP (b)->tv_sec)) +#define timercmp(a, b, CMP) \ + (((a)->tv_sec == (b)->tv_sec) ? ((a)->tv_usec CMP(b)->tv_usec) : ((a)->tv_sec CMP(b)->tv_sec)) -void SystemControl::sendUnsolicitedData(){ - TimeSetToCurrentSystemTime(&tvTime); +void SystemControl::sendUnsolicitedData() { + TimeSetToCurrentSystemTime(&tvTime); if (timercmp(&tvTime, &nextRVSSSendTime, >)) { SystemControlUpdateRVSSSendTime(&nextRVSSSendTime, RVSSRateU8); if (RVSSChannelSocket != 0 && RVSSConfigU32 > 0) { - memset(RVSSData, 0, sizeof (RVSSData)); + memset(RVSSData, 0, sizeof(RVSSData)); if (RVSSConfigU32 & RVSS_TIME_CHANNEL) { SystemControlBuildRVSSTimeChannelMessage(RVSSData, &RVSSMessageLengthU32, 0); - UtilSendUDPData((uint8_t*) module_name.c_str(), &RVSSChannelSocket, &RVSSChannelAddr, (uint8_t*) RVSSData, - RVSSMessageLengthU32, 0); + UtilSendUDPData((uint8_t*)module_name.c_str(), + &RVSSChannelSocket, + &RVSSChannelAddr, + (uint8_t*)RVSSData, + RVSSMessageLengthU32, + 0); } if (RVSSConfigU32 & RVSS_ATOS_CHANNEL) { - SystemControlBuildRVSSATOSChannelMessage(RVSSData, &RVSSMessageLengthU32, - SystemControlState, 0); - UtilSendUDPData((uint8_t*) module_name.c_str(), &RVSSChannelSocket, &RVSSChannelAddr, (uint8_t*) RVSSData, - RVSSMessageLengthU32, 0); + SystemControlBuildRVSSATOSChannelMessage(RVSSData, &RVSSMessageLengthU32, SystemControlState, 0); + UtilSendUDPData((uint8_t*)module_name.c_str(), + &RVSSChannelSocket, + &RVSSChannelAddr, + (uint8_t*)RVSSData, + RVSSMessageLengthU32, + 0); } if (RVSSConfigU32 & RVSS_ASP_CHANNEL) { SystemControlBuildRVSSAspChannelMessage(RVSSData, &RVSSMessageLengthU32, 0); - UtilSendUDPData((uint8_t*) module_name.c_str(), &RVSSChannelSocket, &RVSSChannelAddr, (uint8_t*) RVSSData, - RVSSMessageLengthU32, 0); + UtilSendUDPData((uint8_t*)module_name.c_str(), + &RVSSChannelSocket, + &RVSSChannelAddr, + (uint8_t*)RVSSData, + RVSSMessageLengthU32, + 0); } if (RVSSConfigU32 & RVSS_MONITOR_CHANNEL) { @@ -444,715 +428,722 @@ void SystemControl::sendUnsolicitedData(){ } } -void SystemControl::processUserCommand() -{ +void SystemControl::processUserCommand() { switch (SystemControlCommand) { - // can you access GetServerParameterList_0, GetServerParameter_1, SetServerParameter_2 and DISarmScenario and Exit from the GUI - case Idle_0: - break; - case GetServerStatus_0: - DataDictionaryGetOBCState(&objectControlState); - if (SystemControlCommand != PreviousSystemControlCommand) { - RCLCPP_DEBUG(get_logger(), "State: %s, OBCState: %s", - SystemControlStatesArr[SystemControlState], - SystemControlOBCStatesArr[objectControlState]); - } - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - ControlResponseBuffer[0] = SystemControlState; - ControlResponseBuffer[1] = objectControlState; - appendSysInfoString(ControlResponseBuffer + 2, sizeof (ControlResponseBuffer) - 2); - RCLCPP_DEBUG(get_logger(), "Sending response: %s", ControlResponseBuffer); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", - ControlResponseBuffer, strlen(ControlResponseBuffer), - &ClientSocket, 0); - break; - case GetServerParameterList_0: - SystemControlCommand = Idle_0; - bzero(ParameterListchar, SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE); - SystemControlReadServerParameterList(ParameterListchar, 0); - SystemControlSendControlResponse(strlen(ParameterListchar) > - 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : - SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameterList:", - ParameterListchar, strlen(ParameterListchar), &ClientSocket, 0); - break; - case GetTestOrigin_0: - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - DataDictionaryGetOriginLatitudeString(TextBuffer20, SMALL_BUFFER_SIZE_20); - strcat(ControlResponseBuffer, TextBuffer20); - strcat(ControlResponseBuffer, ";"); - DataDictionaryGetOriginLongitudeString(TextBuffer20, SMALL_BUFFER_SIZE_20); - strcat(ControlResponseBuffer, TextBuffer20); - strcat(ControlResponseBuffer, ";"); - DataDictionaryGetOriginAltitudeString(TextBuffer20, SMALL_BUFFER_SIZE_20); - strcat(ControlResponseBuffer, TextBuffer20); - strcat(ControlResponseBuffer, ";"); - - SystemControlSendControlResponse(strlen(ParameterListchar) > - 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : - SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetTestOrigin:", - ControlResponseBuffer, strlen(ControlResponseBuffer), - &ClientSocket, 0); - break; - case GetServerParameter_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlGetServerParameter(SystemControlArgument[0], ControlResponseBuffer, - SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE, 0); - SystemControlSendControlResponse(strlen(ControlResponseBuffer) > - 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : - SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameter:", - ControlResponseBuffer, strlen(ControlResponseBuffer), - &ClientSocket, 0); - } - else { - RCLCPP_ERROR(get_logger(), "Wrong parameter count in GetServerParameter(Name)!"); - SystemControlCommand = Idle_0; - } - break; - case SetServerParameter_2: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSetServerParameter(SystemControlArgument[0], SystemControlArgument[1], 1); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SetServerParameter:", - ControlResponseBuffer, 0, &ClientSocket, 0); - } - else { - RCLCPP_ERROR(get_logger(), "Wrong parameter count in SetServerParameter(Name, Value)!"); - SystemControlCommand = Idle_0; - } - break; - case CheckFileDirectoryExist_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CheckFileDirectoryExist:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - RCLCPP_ERROR(get_logger(), "Wrong parameter count in CheckFFExist(path)!"); - SystemControlCommand = Idle_0; - } - break; - case CreateDirectory_1: - if (CurrentInputArgCount == CommandArgCount) { + // can you access GetServerParameterList_0, GetServerParameter_1, SetServerParameter_2 and DISarmScenario + // and Exit from the GUI + case Idle_0: + break; + case GetServerStatus_0: + DataDictionaryGetOBCState(&objectControlState); + if (SystemControlCommand != PreviousSystemControlCommand) { + RCLCPP_DEBUG(get_logger(), + "State: %s, OBCState: %s", + SystemControlStatesArr[SystemControlState], + SystemControlOBCStatesArr[objectControlState]); + } SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCreateDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CreateDirectory:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - RCLCPP_ERROR(get_logger(), "Wrong parameter count in CreateDirectory(path)!"); + ControlResponseBuffer[0] = SystemControlState; + ControlResponseBuffer[1] = objectControlState; + appendSysInfoString(ControlResponseBuffer + 2, sizeof(ControlResponseBuffer) - 2); + RCLCPP_DEBUG(get_logger(), "Sending response: %s", ControlResponseBuffer); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, + "GetServerStatus:", + ControlResponseBuffer, + strlen(ControlResponseBuffer), + &ClientSocket, + 0); + break; + case GetServerParameterList_0: SystemControlCommand = Idle_0; - } - break; - case GetRootDirectoryContent_0: - RCLCPP_INFO(get_logger(), "GetRootDirectory called: defaulting to GetDirectoryContent"); - case GetDirectoryContent_1: - if (CurrentInputArgCount == CommandArgCount) { + bzero(ParameterListchar, SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE); + SystemControlReadServerParameterList(ParameterListchar, 0); + SystemControlSendControlResponse(strlen(ParameterListchar) > 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK + : SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, + "GetServerParameterList:", + ParameterListchar, + strlen(ParameterListchar), + &ClientSocket, + 0); + break; + case GetTestOrigin_0: SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", - ControlResponseBuffer, 1, &ClientSocket, 0); - if (ControlResponseBuffer[0] == FOLDER_EXIST) { - UtilCreateDirContent((uint8_t*) SystemControlArgument[0],(uint8_t*) "dir.info"); + DataDictionaryGetOriginLatitudeString(TextBuffer20, SMALL_BUFFER_SIZE_20); + strcat(ControlResponseBuffer, TextBuffer20); + strcat(ControlResponseBuffer, ";"); + DataDictionaryGetOriginLongitudeString(TextBuffer20, SMALL_BUFFER_SIZE_20); + strcat(ControlResponseBuffer, TextBuffer20); + strcat(ControlResponseBuffer, ";"); + DataDictionaryGetOriginAltitudeString(TextBuffer20, SMALL_BUFFER_SIZE_20); + strcat(ControlResponseBuffer, TextBuffer20); + strcat(ControlResponseBuffer, ";"); + + SystemControlSendControlResponse(strlen(ParameterListchar) > 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK + : SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, + "GetTestOrigin:", + ControlResponseBuffer, + strlen(ControlResponseBuffer), + &ClientSocket, + 0); + break; + case GetServerParameter_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - FileLengthI32 = SystemControlBuildFileContentInfo("dir.info", 0); - SystemControlFileDownloadResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, - "SubGetDirectoryContent:", FileLengthI32, &ClientSocket, - 0); - SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, - SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); - SystemControlDestroyFileContentInfo("dir.info", 1); + SystemControlGetServerParameter( + SystemControlArgument[0], ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE, 0); + SystemControlSendControlResponse(strlen(ControlResponseBuffer) > 0 + ? SYSTEM_CONTROL_RESPONSE_CODE_OK + : SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, + "GetServerParameter:", + ControlResponseBuffer, + strlen(ControlResponseBuffer), + &ClientSocket, + 0); + } else { + RCLCPP_ERROR(get_logger(), "Wrong parameter count in GetServerParameter(Name)!"); + SystemControlCommand = Idle_0; } - } - else { - RCLCPP_ERROR(get_logger(), - "Wrong parameter count in GetDirectoryContent(path)! got:%d, expected:%d", - CurrentInputArgCount, CommandArgCount); - SystemControlCommand = Idle_0; - } - break; - case DeleteTrajectory_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = - SystemControlDeleteTrajectory(SystemControlArgument[0], - sizeof (SystemControlArgument[0])); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteTrajectory:", - ControlResponseBuffer, 1, &ClientSocket, 0); - } - else { - RCLCPP_ERROR(get_logger(), - "Wrong parameter count in DeleteTrajectory(name)! got:%d, expected:%d", - CurrentInputArgCount, CommandArgCount); - SystemControlCommand = Idle_0; - } - break; - case DeleteGeofence_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = - SystemControlDeleteGeofence(SystemControlArgument[0], sizeof (SystemControlArgument[0])); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteGeofence:", - ControlResponseBuffer, 1, &ClientSocket, 0); - } - else { - RCLCPP_ERROR(get_logger(), - "Wrong parameter count in DeleteGeofence(name)! got:%d, expected:%d", - CurrentInputArgCount, CommandArgCount); - SystemControlCommand = Idle_0; - } - break; - case DeleteFileDirectory_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = - SystemControlDeleteGenericFile(SystemControlArgument[0], - sizeof (SystemControlArgument[0])); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteFileDirectory:", - ControlResponseBuffer, 1, &ClientSocket, 0); - } - else { - RCLCPP_ERROR(get_logger(), - "Wrong parameter count in DeleteFileDirectory(path)! got:%d, expected:%d", - CurrentInputArgCount, CommandArgCount); - SystemControlCommand = Idle_0; - } - break; - case ClearTrajectories_0: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = SystemControlClearTrajectories(); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", - ControlResponseBuffer, 1, &ClientSocket, 0); - } - else { - RCLCPP_ERROR(get_logger(), - "Wrong parameter count in ClearTrajectories()! got:%d, expected:%d", - CurrentInputArgCount, CommandArgCount); - SystemControlCommand = Idle_0; - } - break; - case ClearGeofences_0: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = SystemControlClearGeofences(); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearGeofences:", - ControlResponseBuffer, 1, &ClientSocket, 0); - } - else { - RCLCPP_ERROR(get_logger(), - "Wrong parameter count in ClearGeofences()! got:%d, expected:%d", - CurrentInputArgCount, CommandArgCount); - SystemControlCommand = Idle_0; - } - break; - case ClearObjects_0: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = SystemControlClearObjects(); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearObjects:", - ControlResponseBuffer, 1, &ClientSocket, 0); - } - else { - RCLCPP_ERROR(get_logger(), - "Wrong parameter count in ClearObjects()! got:%d, expected:%d", - CurrentInputArgCount, CommandArgCount); - SystemControlCommand = Idle_0; - } - break; - case DownloadFile_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DownloadFile:", - ControlResponseBuffer, 1, &ClientSocket, 0); - if (ControlResponseBuffer[0] == FILE_EXIST) { + break; + case SetServerParameter_2: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - FileLengthI32 = SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); - SystemControlFileDownloadResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", - FileLengthI32, &ClientSocket, 0); - SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], - STR_SYSTEM_CONTROL_TX_PACKET_SIZE, - SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); - SystemControlDestroyFileContentInfo(SystemControlArgument[0], 0); + SystemControlSetServerParameter(SystemControlArgument[0], SystemControlArgument[1], 1); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "SetServerParameter:", ControlResponseBuffer, 0, &ClientSocket, 0); + } else { + RCLCPP_ERROR(get_logger(), "Wrong parameter count in SetServerParameter(Name, Value)!"); + SystemControlCommand = Idle_0; } + break; + case CheckFileDirectoryExist_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, + "CheckFileDirectoryExist:", + ControlResponseBuffer, + 1, + &ClientSocket, + 0); - } - else { - RCLCPP_ERROR(get_logger(), "Wrong parameter count in GetDirectoryContent(path)!"); - SystemControlCommand = Idle_0; - } - break; - case DownloadTrajFiles_0: - case DownloadDirectoryContent_1: - if (CurrentInputArgCount == CommandArgCount) { - char functionReturnName[50]; - memset(functionReturnName, 0, 50); - memset(ControlResponseBuffer, 0, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - if(SystemControlCommand == DownloadTrajFiles_0){ - strcat(functionReturnName, "DownloadTrajFiles:"); - ControlResponseBuffer[0] = FOLDER_EXIST; - } else if(SystemControlCommand == DownloadDirectoryContent_1){ - strcat(functionReturnName, "DownloadDirectoryContent:"); + } else { + RCLCPP_ERROR(get_logger(), "Wrong parameter count in CheckFFExist(path)!"); + SystemControlCommand = Idle_0; + } + break; + case CreateDirectory_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCreateDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "CreateDirectory:", ControlResponseBuffer, 1, &ClientSocket, 0); + + } else { + RCLCPP_ERROR(get_logger(), "Wrong parameter count in CreateDirectory(path)!"); + SystemControlCommand = Idle_0; + } + break; + case GetRootDirectoryContent_0: + RCLCPP_INFO(get_logger(), "GetRootDirectory called: defaulting to GetDirectoryContent"); + case GetDirectoryContent_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", ControlResponseBuffer, 1, &ClientSocket, 0); + if (ControlResponseBuffer[0] == FOLDER_EXIST) { + UtilCreateDirContent((uint8_t*)SystemControlArgument[0], (uint8_t*)"dir.info"); + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + FileLengthI32 = SystemControlBuildFileContentInfo("dir.info", 0); + SystemControlFileDownloadResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", FileLengthI32, &ClientSocket, 0); + SystemControlSendFileContent(&ClientSocket, + "dir.info", + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + SystemControlDirectoryInfo.info_buffer, + KEEP_FILE, + 0); + SystemControlDestroyFileContentInfo("dir.info", 1); + } + } else { + RCLCPP_ERROR(get_logger(), + "Wrong parameter count in GetDirectoryContent(path)! got:%d, expected:%d", + CurrentInputArgCount, + CommandArgCount); + SystemControlCommand = Idle_0; + } + break; + case DeleteTrajectory_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof(ControlResponseBuffer)); + *ControlResponseBuffer = + SystemControlDeleteTrajectory(SystemControlArgument[0], sizeof(SystemControlArgument[0])); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteTrajectory:", ControlResponseBuffer, 1, &ClientSocket, 0); + } else { + RCLCPP_ERROR(get_logger(), + "Wrong parameter count in DeleteTrajectory(name)! got:%d, expected:%d", + CurrentInputArgCount, + CommandArgCount); + SystemControlCommand = Idle_0; } - if(ControlResponseBuffer[0] == FOLDER_EXIST){ - if(SystemControlCommand == DownloadTrajFiles_0){ - UtilCreateDirContent((uint8_t*) ATOS_TRAJ_DIRECTORY_STRING,(uint8_t*) "dir.info"); - } else if(SystemControlCommand == DownloadDirectoryContent_1){ - UtilCreateDirContent((uint8_t*) SystemControlArgument[0], (uint8_t*) "dir.info"); + break; + case DeleteGeofence_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof(ControlResponseBuffer)); + *ControlResponseBuffer = + SystemControlDeleteGeofence(SystemControlArgument[0], sizeof(SystemControlArgument[0])); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteGeofence:", ControlResponseBuffer, 1, &ClientSocket, 0); + } else { + RCLCPP_ERROR(get_logger(), + "Wrong parameter count in DeleteGeofence(name)! got:%d, expected:%d", + CurrentInputArgCount, + CommandArgCount); + SystemControlCommand = Idle_0; + } + break; + case DeleteFileDirectory_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof(ControlResponseBuffer)); + *ControlResponseBuffer = + SystemControlDeleteGenericFile(SystemControlArgument[0], sizeof(SystemControlArgument[0])); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteFileDirectory:", ControlResponseBuffer, 1, &ClientSocket, 0); + } else { + RCLCPP_ERROR(get_logger(), + "Wrong parameter count in DeleteFileDirectory(path)! got:%d, expected:%d", + CurrentInputArgCount, + CommandArgCount); + SystemControlCommand = Idle_0; + } + break; + case ClearTrajectories_0: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof(ControlResponseBuffer)); + *ControlResponseBuffer = SystemControlClearTrajectories(); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", ControlResponseBuffer, 1, &ClientSocket, 0); + } else { + RCLCPP_ERROR(get_logger(), + "Wrong parameter count in ClearTrajectories()! got:%d, expected:%d", + CurrentInputArgCount, + CommandArgCount); + SystemControlCommand = Idle_0; + } + break; + case ClearGeofences_0: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof(ControlResponseBuffer)); + *ControlResponseBuffer = SystemControlClearGeofences(); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearGeofences:", ControlResponseBuffer, 1, &ClientSocket, 0); + } else { + RCLCPP_ERROR(get_logger(), + "Wrong parameter count in ClearGeofences()! got:%d, expected:%d", + CurrentInputArgCount, + CommandArgCount); + SystemControlCommand = Idle_0; + } + break; + case ClearObjects_0: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof(ControlResponseBuffer)); + *ControlResponseBuffer = SystemControlClearObjects(); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearObjects:", ControlResponseBuffer, 1, &ClientSocket, 0); + } else { + RCLCPP_ERROR(get_logger(), + "Wrong parameter count in ClearObjects()! got:%d, expected:%d", + CurrentInputArgCount, + CommandArgCount); + SystemControlCommand = Idle_0; + } + break; + case DownloadFile_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "DownloadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); + if (ControlResponseBuffer[0] == FILE_EXIST) { + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + FileLengthI32 = SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); + SystemControlFileDownloadResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", FileLengthI32, &ClientSocket, 0); + SystemControlSendFileContent(&ClientSocket, + SystemControlArgument[0], + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + SystemControlDirectoryInfo.info_buffer, + KEEP_FILE, + 0); + SystemControlDestroyFileContentInfo(SystemControlArgument[0], 0); } - char TestDirectoryPath[MAX_PATH_LENGTH]; - memset(TestDirectoryPath, 0,MAX_PATH_LENGTH); - char CompletePath[MAX_PATH_LENGTH]; - memset(CompletePath, 0, MAX_PATH_LENGTH); - char InPath[MAX_PATH_LENGTH]; - memset(InPath, 0, MAX_PATH_LENGTH); - - UtilGetTestDirectoryPath(TestDirectoryPath, sizeof (TestDirectoryPath)); - strcat(CompletePath, TestDirectoryPath); - strcat(CompletePath,"dir.info"); - int rows = UtilCountFileRowsInPath(CompletePath, strlen(CompletePath)); - char RowBuffer[SMALL_BUFFER_SIZE_128]; + } else { + RCLCPP_ERROR(get_logger(), "Wrong parameter count in GetDirectoryContent(path)!"); + SystemControlCommand = Idle_0; + } + break; + case DownloadTrajFiles_0: + case DownloadDirectoryContent_1: + if (CurrentInputArgCount == CommandArgCount) { + char functionReturnName[50]; + memset(functionReturnName, 0, 50); + memset(ControlResponseBuffer, 0, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + if (SystemControlCommand == DownloadTrajFiles_0) { + strcat(functionReturnName, "DownloadTrajFiles:"); + ControlResponseBuffer[0] = FOLDER_EXIST; + } else if (SystemControlCommand == DownloadDirectoryContent_1) { + strcat(functionReturnName, "DownloadDirectoryContent:"); + SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + } + if (ControlResponseBuffer[0] == FOLDER_EXIST) { + if (SystemControlCommand == DownloadTrajFiles_0) { + UtilCreateDirContent((uint8_t*)ATOS_TRAJ_DIRECTORY_STRING, (uint8_t*)"dir.info"); + } else if (SystemControlCommand == DownloadDirectoryContent_1) { + UtilCreateDirContent((uint8_t*)SystemControlArgument[0], (uint8_t*)"dir.info"); + } - for(int i = 0; i < rows; i ++) - { + char TestDirectoryPath[MAX_PATH_LENGTH]; + memset(TestDirectoryPath, 0, MAX_PATH_LENGTH); + char CompletePath[MAX_PATH_LENGTH]; memset(CompletePath, 0, MAX_PATH_LENGTH); + char InPath[MAX_PATH_LENGTH]; + memset(InPath, 0, MAX_PATH_LENGTH); + + UtilGetTestDirectoryPath(TestDirectoryPath, sizeof(TestDirectoryPath)); strcat(CompletePath, TestDirectoryPath); - strcat(CompletePath,"dir.info"); - UtilGetRowInFile(CompletePath, strlen(CompletePath), i, RowBuffer, SMALL_BUFFER_SIZE_128); - if(*RowBuffer == 'F'){ - memset(InPath, 0, MAX_PATH_LENGTH); - if(SystemControlCommand == DownloadTrajFiles_0) strcat(InPath, ATOS_TRAJ_DIRECTORY_STRING); - else if(SystemControlCommand == DownloadDirectoryContent_1) strcat(InPath, SystemControlArgument[0]); - strcat(InPath, strstr(RowBuffer, "-")+1); - memset(ControlResponseBuffer, 0, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - FileLengthI32 = SystemControlBuildFileContentInfo(InPath, 0); - SystemControlFileDownloadResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, functionReturnName, - FileLengthI32, &ClientSocket, 0); - SystemControlSendFileContent(&ClientSocket, InPath, - STR_SYSTEM_CONTROL_TX_PACKET_SIZE, - SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 1); - SystemControlDestroyFileContentInfo(InPath, 0); + strcat(CompletePath, "dir.info"); + int rows = UtilCountFileRowsInPath(CompletePath, strlen(CompletePath)); + char RowBuffer[SMALL_BUFFER_SIZE_128]; + + for (int i = 0; i < rows; i++) { + memset(CompletePath, 0, MAX_PATH_LENGTH); + strcat(CompletePath, TestDirectoryPath); + strcat(CompletePath, "dir.info"); + UtilGetRowInFile(CompletePath, strlen(CompletePath), i, RowBuffer, SMALL_BUFFER_SIZE_128); + if (*RowBuffer == 'F') { + memset(InPath, 0, MAX_PATH_LENGTH); + if (SystemControlCommand == DownloadTrajFiles_0) + strcat(InPath, ATOS_TRAJ_DIRECTORY_STRING); + else if (SystemControlCommand == DownloadDirectoryContent_1) + strcat(InPath, SystemControlArgument[0]); + strcat(InPath, strstr(RowBuffer, "-") + 1); + memset(ControlResponseBuffer, 0, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + FileLengthI32 = SystemControlBuildFileContentInfo(InPath, 0); + SystemControlFileDownloadResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, functionReturnName, FileLengthI32, &ClientSocket, 0); + SystemControlSendFileContent(&ClientSocket, + InPath, + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + SystemControlDirectoryInfo.info_buffer, + KEEP_FILE, + 1); + SystemControlDestroyFileContentInfo(InPath, 0); + } } + SystemControlDirectoryInfo.exist = 1; // Force to exist + SystemControlDestroyFileContentInfo("dir.info", 1); + } else { + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, functionReturnName, ControlResponseBuffer, 3, &ClientSocket, 0); } - SystemControlDirectoryInfo.exist = 1; //Force to exist - SystemControlDestroyFileContentInfo("dir.info", 1); - + SystemControlCommand = Idle_0; } else { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, functionReturnName, - ControlResponseBuffer, 3, &ClientSocket, 0); - } - SystemControlCommand = Idle_0; - } else { - RCLCPP_ERROR(get_logger(), "Wrong parameter count in GetDirectoryContent(path)!"); - SystemControlCommand = Idle_0; - } - break; - case UploadFile_4: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - bzero(RxFilePath, MAX_FILE_PATH); - SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], - SystemControlArgument[2], SystemControlArgument[3], - ControlResponseBuffer, RxFilePath, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", - ControlResponseBuffer, 1, &ClientSocket, 0); - RCLCPP_DEBUG(get_logger(), "UploadFile filelength: %s", SystemControlArgument[1]); - if (ControlResponseBuffer[0] == SERVER_PREPARED_BIG_PACKET_SIZE) //Server is ready to receive data - { - RCLCPP_INFO(get_logger(), "Receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, RxFilePath, - SystemControlArgument[1], STR_SYSTEM_CONTROL_RX_PACKET_SIZE, - ControlResponseBuffer, 0); - } - else if (ControlResponseBuffer[0] == PATH_INVALID_MISSING) { - RCLCPP_INFO(get_logger(), "Failed receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, RxFilePath, SystemControlArgument[1], - STR_SYSTEM_CONTROL_RX_PACKET_SIZE, ControlResponseBuffer, 0); - SystemControlDeleteFileDirectory(RxFilePath, ControlResponseBuffer, 0); - ControlResponseBuffer[0] = PATH_INVALID_MISSING; - } - else { - RCLCPP_INFO(get_logger(), "Receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, RxFilePath, - SystemControlArgument[1], SystemControlArgument[2], - ControlResponseBuffer, 0); - } - - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubUploadFile:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - RCLCPP_ERROR(get_logger(), - "Wrong parameter count in UploadFile(path, filesize, packetsize, filetype)!"); - SystemControlCommand = Idle_0; - } - break; - case InitializeScenario_0: - if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_IDLE) { - try{ - this->initPub.publish(Init::message_type()); + RCLCPP_ERROR(get_logger(), "Wrong parameter count in GetDirectoryContent(path)!"); + SystemControlCommand = Idle_0; } - catch(...){ - RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending INIT command"); - SystemControlState = SERVER_STATE_ERROR; + break; + case UploadFile_4: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + bzero(RxFilePath, MAX_FILE_PATH); + SystemControlUploadFile(SystemControlArgument[0], + SystemControlArgument[1], + SystemControlArgument[2], + SystemControlArgument[3], + ControlResponseBuffer, + RxFilePath, + 0); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); + RCLCPP_DEBUG(get_logger(), "UploadFile filelength: %s", SystemControlArgument[1]); + if (ControlResponseBuffer[0] == SERVER_PREPARED_BIG_PACKET_SIZE) // Server is ready to receive data + { + RCLCPP_INFO(get_logger(), "Receiving file: %s", SystemControlArgument[0]); + SystemControlReceiveRxData(&ClientSocket, + RxFilePath, + SystemControlArgument[1], + STR_SYSTEM_CONTROL_RX_PACKET_SIZE, + ControlResponseBuffer, + 0); + } else if (ControlResponseBuffer[0] == PATH_INVALID_MISSING) { + RCLCPP_INFO(get_logger(), "Failed receiving file: %s", SystemControlArgument[0]); + SystemControlReceiveRxData(&ClientSocket, + RxFilePath, + SystemControlArgument[1], + STR_SYSTEM_CONTROL_RX_PACKET_SIZE, + ControlResponseBuffer, + 0); + SystemControlDeleteFileDirectory(RxFilePath, ControlResponseBuffer, 0); + ControlResponseBuffer[0] = PATH_INVALID_MISSING; + } else { + RCLCPP_INFO(get_logger(), "Receiving file: %s", SystemControlArgument[0]); + SystemControlReceiveRxData(&ClientSocket, + RxFilePath, + SystemControlArgument[1], + SystemControlArgument[2], + ControlResponseBuffer, + 0); + } - } - SystemControlState = SERVER_STATE_INWORK; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "InitializeScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubUploadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending INIT.\n", &ClientSocket, 0); - } - else if (SystemControlState == SERVER_STATE_INWORK && objectControlState == OBC_STATE_INITIALIZED) { - SystemControlSendLog - ("[SystemControl] Simulate that all objects becomes successfully configured.\n", - &ClientSocket, 0); - SystemControlCommand = Idle_0; - SystemControlState = SERVER_STATE_IDLE; - } - else if (SystemControlState == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "InitializeScenario:", ControlResponseBuffer, 0, - &ClientSocket, 0); - SystemControlSendLog("[SystemControl] INIT received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case ConnectObject_0: - if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_INITIALIZED) { - try{ - this->connectPub.publish(Connect::message_type()); + } else { + RCLCPP_ERROR(get_logger(), + "Wrong parameter count in UploadFile(path, filesize, packetsize, filetype)!"); + SystemControlCommand = Idle_0; } - catch(...){ - RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending CONNECT command"); - SystemControlState = SERVER_STATE_ERROR; + break; + case InitializeScenario_0: + if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_IDLE) { + try { + this->initPub.publish(Init::message_type()); + } catch (...) { + RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending INIT command"); + SystemControlState = SERVER_STATE_ERROR; + } + SystemControlState = SERVER_STATE_INWORK; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "InitializeScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); - } - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ConnectObject:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending CONNECT.\n", &ClientSocket, 0); - } - else if (SystemControlState == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { - SystemControlSendLog("[SystemControl] Simulate that all objects are connected.\n", - &ClientSocket, 0); - SystemControlCommand = Idle_0; - SystemControlState = SERVER_STATE_IDLE; - } - else if (SystemControlState == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, - 0); - SystemControlSendLog("[SystemControl] CONNECT received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case DisconnectObject_0: - if (SystemControlState == SERVER_STATE_IDLE) { - try{ - this->disconnectPub.publish(Disconnect::message_type()); - } - catch(...){ - RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending DISCONNECT command"); - SystemControlState = SERVER_STATE_ERROR; - } - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisconnectObject:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending DISCONNECT.\n", &ClientSocket, 0); - } - else { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, - 0); - SystemControlSendLog("[SystemControl] DISCONNECT received, state errors!\n", &ClientSocket, - 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case ArmScenario_0: - if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_CONNECTED) { - SystemControlState = SERVER_STATE_INWORK; - try{ - this->armPub.publish(Arm::message_type()); - } - catch(...){ - RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending ARM command"); - SystemControlState = SERVER_STATE_ERROR; + SystemControlSendLog("[SystemControl] Sending INIT.\n", &ClientSocket, 0); + } else if (SystemControlState == SERVER_STATE_INWORK && objectControlState == OBC_STATE_INITIALIZED) { + SystemControlSendLog( + "[SystemControl] Simulate that all objects becomes successfully configured.\n", &ClientSocket, 0); + SystemControlCommand = Idle_0; + SystemControlState = SERVER_STATE_IDLE; + } else if (SystemControlState == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "InitializeScenario:", + ControlResponseBuffer, + 0, + &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] INIT received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; } - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ArmScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending ARM.\n", &ClientSocket, 0); - } - else if (SystemControlState == SERVER_STATE_INWORK && objectControlState == OBC_STATE_ARMED) { - SystemControlSendLog("[SystemControl] Simulate that all objects become armed.\n", - &ClientSocket, 0); - - SystemControlCommand = Idle_0; - SystemControlState = SERVER_STATE_IDLE; - } - else if (SystemControlState == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "ArmScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] ARM received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case RemoteControl_1: - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; - if (CurrentInputArgCount == CommandArgCount) { - if (SystemControlState == SERVER_STATE_IDLE - && (objectControlState == OBC_STATE_CONNECTED - || objectControlState == OBC_STATE_REMOTECTRL)) { - if (!strcasecmp(SystemControlArgument[0], ENABLE_COMMAND_STRING) - && objectControlState == OBC_STATE_CONNECTED) { - RCLCPP_INFO(get_logger(), "Requesting enabling of remote control"); - this->remoteControlEnablePub.publish(RemoteControlEnable::message_type()); - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; - } - else if (!strcasecmp(SystemControlArgument[0], DISABLE_COMMAND_STRING) - && objectControlState == OBC_STATE_REMOTECTRL) { - RCLCPP_INFO(get_logger(), "Requesting disabling of remote control"); - this->remoteControlDisablePub.publish(RemoteControlDisable::message_type()); - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; - } - else { - RCLCPP_WARN(get_logger(), "Incorrect remote control command"); - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; + break; + case ConnectObject_0: + if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_INITIALIZED) { + try { + this->connectPub.publish(Connect::message_type()); + } catch (...) { + RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending CONNECT command"); + SystemControlState = SERVER_STATE_ERROR; } + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending CONNECT.\n", &ClientSocket, 0); + } else if (SystemControlState == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { + SystemControlSendLog("[SystemControl] Simulate that all objects are connected.\n", &ClientSocket, 0); + SystemControlCommand = Idle_0; + SystemControlState = SERVER_STATE_IDLE; + } else if (SystemControlState == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "ConnectObject:", + ControlResponseBuffer, + 0, + &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] CONNECT received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; } - else { - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; + break; + case DisconnectObject_0: + if (SystemControlState == SERVER_STATE_IDLE) { + try { + this->disconnectPub.publish(Disconnect::message_type()); + } catch (...) { + RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending DISCONNECT command"); + SystemControlState = SERVER_STATE_ERROR; + } + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisconnectObject:", ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending DISCONNECT.\n", &ClientSocket, 0); + } else { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "ConnectObject:", + ControlResponseBuffer, + 0, + &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] DISCONNECT received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; } + break; + case ArmScenario_0: + if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_CONNECTED) { + SystemControlState = SERVER_STATE_INWORK; + try { + this->armPub.publish(Arm::message_type()); + } catch (...) { + RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending ARM command"); + SystemControlState = SERVER_STATE_ERROR; + } + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "ArmScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending ARM.\n", &ClientSocket, 0); + } else if (SystemControlState == SERVER_STATE_INWORK && objectControlState == OBC_STATE_ARMED) { + SystemControlSendLog("[SystemControl] Simulate that all objects become armed.\n", &ClientSocket, 0); - } - else { - RCLCPP_WARN(get_logger(), "Remote control command parameter count error"); - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; - } - SystemControlCommand = Idle_0; - SystemControlSendControlResponse(responseCode, "RemoteControl:", - ControlResponseBuffer, 0, &ClientSocket, 0); - break; - case RemoteControlManoeuvre_2: - if (CurrentInputArgCount == CommandArgCount) { - if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_REMOTECTRL) { - auto msg = BackToStart::message_type(); - if (inet_pton(AF_INET, SystemControlArgument[0], &msg.object_ip) != -1) { - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; - switch (atoi(SystemControlArgument[1])) { - case MSCP_BACK_TO_START: - msg.manoeuvre = MANOEUVRE_BACK_TO_START; - break; - default: - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_FUNCTION_NOT_AVAILABLE; - } - if (responseCode != SYSTEM_CONTROL_RESPONSE_CODE_FUNCTION_NOT_AVAILABLE) { - this->backToStartPub.publish(msg); + SystemControlCommand = Idle_0; + SystemControlState = SERVER_STATE_IDLE; + } else if (SystemControlState == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "ArmScenario:", + ControlResponseBuffer, + 0, + &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] ARM received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case RemoteControl_1: + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; + if (CurrentInputArgCount == CommandArgCount) { + if (SystemControlState == SERVER_STATE_IDLE && + (objectControlState == OBC_STATE_CONNECTED || objectControlState == OBC_STATE_REMOTECTRL)) { + if (!strcasecmp(SystemControlArgument[0], ENABLE_COMMAND_STRING) && + objectControlState == OBC_STATE_CONNECTED) { + RCLCPP_INFO(get_logger(), "Requesting enabling of remote control"); + this->remoteControlEnablePub.publish(RemoteControlEnable::message_type()); responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; + } else if (!strcasecmp(SystemControlArgument[0], DISABLE_COMMAND_STRING) && + objectControlState == OBC_STATE_REMOTECTRL) { + RCLCPP_INFO(get_logger(), "Requesting disabling of remote control"); + this->remoteControlDisablePub.publish(RemoteControlDisable::message_type()); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; + } else { + RCLCPP_WARN(get_logger(), "Incorrect remote control command"); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; } + } else { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; } - else { - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; - } - } - else { - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; + + } else { + RCLCPP_WARN(get_logger(), "Remote control command parameter count error"); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; } - } - else { - RCLCPP_WARN(get_logger(), "Remote control manoeuvre command parameter count error"); - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; - } - SystemControlCommand = Idle_0; - SystemControlSendControlResponse(responseCode, "RemoteControlManoeuvre:", - ControlResponseBuffer, 0, &ClientSocket, 0); - break; - case GetObjectEnableStatus_1: - if (CurrentInputArgCount == CommandArgCount) { - if (SystemControlState == SERVER_STATE_IDLE) { - memset(pcBuffer, 0, sizeof (pcBuffer)); - ObjectEnabledCommandType enableCommand; - - if (inet_pton(AF_INET, SystemControlArgument[0], &enableCommand.objectIP) != -1) { - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; - DataDictionaryGetObjectEnableStatusByIp(enableCommand.objectIP, - &enableCommand.Enabled); - memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - ControlResponseBuffer[0] = (uint8_t) enableCommand.Enabled; - SystemControlSendControlResponse(responseCode, "GetObjectEnableStatus:", - ControlResponseBuffer, 1, &ClientSocket, 0); + SystemControlCommand = Idle_0; + SystemControlSendControlResponse( + responseCode, "RemoteControl:", ControlResponseBuffer, 0, &ClientSocket, 0); + break; + case RemoteControlManoeuvre_2: + if (CurrentInputArgCount == CommandArgCount) { + if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_REMOTECTRL) { + auto msg = BackToStart::message_type(); + if (inet_pton(AF_INET, SystemControlArgument[0], &msg.object_ip) != -1) { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; + switch (atoi(SystemControlArgument[1])) { + case MSCP_BACK_TO_START: + msg.manoeuvre = MANOEUVRE_BACK_TO_START; + break; + default: + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_FUNCTION_NOT_AVAILABLE; + } + if (responseCode != SYSTEM_CONTROL_RESPONSE_CODE_FUNCTION_NOT_AVAILABLE) { + this->backToStartPub.publish(msg); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; + } + } else { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; + } + } else { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; } - else - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; + } else { + RCLCPP_WARN(get_logger(), "Remote control manoeuvre command parameter count error"); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; } - else - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; - } - else { - RCLCPP_WARN(get_logger(), "GetObjectEnableStatus command parameter count error"); - responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; - } - - if (responseCode != SYSTEM_CONTROL_RESPONSE_CODE_OK) - SystemControlSendControlResponse(responseCode, "GetObjectEnableStatus:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlCommand = Idle_0; - break; - case StartScenario_1: - if (CurrentInputArgCount == CommandArgCount) { - if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) //Temporary! - { - bzero(pcBuffer, IPC_BUFFER_SIZE); - TimeSetToCurrentSystemTime(&tvTime); - - if (TIME_COMPENSATE_LAGING_VM) - timersub(&tvTime, &VirtualMachineLagCompensation, &tvTime); + SystemControlCommand = Idle_0; + SystemControlSendControlResponse( + responseCode, "RemoteControlManoeuvre:", ControlResponseBuffer, 0, &ClientSocket, 0); + break; + case GetObjectEnableStatus_1: + if (CurrentInputArgCount == CommandArgCount) { + if (SystemControlState == SERVER_STATE_IDLE) { + memset(pcBuffer, 0, sizeof(pcBuffer)); + ObjectEnabledCommandType enableCommand; - RCLCPP_INFO(get_logger(), "Current timestamp (epoch): %lu", TimeGetAsUTCms(&tvTime)); + if (inet_pton(AF_INET, SystemControlArgument[0], &enableCommand.objectIP) != -1) { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; + DataDictionaryGetObjectEnableStatusByIp(enableCommand.objectIP, &enableCommand.Enabled); + memset(ControlResponseBuffer, 0, sizeof(ControlResponseBuffer)); + ControlResponseBuffer[0] = (uint8_t)enableCommand.Enabled; + SystemControlSendControlResponse( + responseCode, "GetObjectEnableStatus:", ControlResponseBuffer, 1, &ClientSocket, 0); + } else + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; + } else + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; + } else { + RCLCPP_WARN(get_logger(), "GetObjectEnableStatus command parameter count error"); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; + } - DelayedStartU32 = atoi(SystemControlArgument[0]); - sprintf(pcBuffer, "%" PRIi64 ";%" PRIu32 ";", TimeGetAsUTCms(&tvTime), DelayedStartU32); - RCLCPP_INFO(get_logger(), "Sending START <%s> (delayed +%u ms)", pcBuffer, - DelayedStartU32); - try{ - this->startPub.publish(Start::message_type()); - } - catch(...){ - RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending STRT command"); - SystemControlState = SERVER_STATE_ERROR; + if (responseCode != SYSTEM_CONTROL_RESPONSE_CODE_OK) + SystemControlSendControlResponse( + responseCode, "GetObjectEnableStatus:", ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlCommand = Idle_0; + break; + case StartScenario_1: + if (CurrentInputArgCount == CommandArgCount) { + if (SystemControlState == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) // Temporary! + { + bzero(pcBuffer, IPC_BUFFER_SIZE); + TimeSetToCurrentSystemTime(&tvTime); + + if (TIME_COMPENSATE_LAGING_VM) + timersub(&tvTime, &VirtualMachineLagCompensation, &tvTime); + + RCLCPP_INFO(get_logger(), "Current timestamp (epoch): %lu", TimeGetAsUTCms(&tvTime)); + + DelayedStartU32 = atoi(SystemControlArgument[0]); + sprintf(pcBuffer, "%" PRIi64 ";%" PRIu32 ";", TimeGetAsUTCms(&tvTime), DelayedStartU32); + RCLCPP_INFO(get_logger(), "Sending START <%s> (delayed +%u ms)", pcBuffer, DelayedStartU32); + try { + this->startPub.publish(Start::message_type()); + } catch (...) { + RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending STRT command"); + SystemControlState = SERVER_STATE_ERROR; + } + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "StartScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlState = SERVER_STATE_INWORK; + // SystemControlState = SERVER_STATE_IDLE; //Temporary! + // SystemControlCommand = Idle_0; //Temporary! + } else if (SystemControlState == SERVER_STATE_INWORK && objectControlState == OBC_STATE_RUNNING) { + + SystemControlCommand = Idle_0; + SystemControlState = SERVER_STATE_IDLE; + } else if (SystemControlState == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "StartScenario:", + ControlResponseBuffer, + 0, + &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] START received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; } + + } else + RCLCPP_WARN(get_logger(), "START command parameter count error"); + break; + case stop_0: + try { + this->stopPub.publish(Stop::message_type()); + SystemControlState = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "StartScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlState = SERVER_STATE_INWORK; - //SystemControlState = SERVER_STATE_IDLE; //Temporary! - //SystemControlCommand = Idle_0; //Temporary! + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "stop:", ControlResponseBuffer, 0, &ClientSocket, 0); + } catch (...) { + this->disconnectPub.publish(Disconnect::message_type()); + RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending DISCONSTOPNECTSTOP command"); + SystemControlState = SERVER_STATE_ERROR; } - else if (SystemControlState == SERVER_STATE_INWORK && objectControlState == OBC_STATE_RUNNING) { - + break; + case AbortScenario_0: + try { + this->abortPub.publish(Abort::message_type()); + SystemControlState = SERVER_STATE_IDLE; SystemControlCommand = Idle_0; - SystemControlState = SERVER_STATE_IDLE; - } - else if (SystemControlState == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "StartScenario:", ControlResponseBuffer, 0, - &ClientSocket, 0); - SystemControlSendLog("[SystemControl] START received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; + if (ClientSocket >= 0) { + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + ControlResponseBuffer[0] = 1; + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "AbortScenario:", ControlResponseBuffer, 1, &ClientSocket, 0); + } + } catch (...) { + RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending ABORT command"); + SystemControlState = SERVER_STATE_ERROR; } - - } - else - RCLCPP_WARN(get_logger(), "START command parameter count error"); - break; - case stop_0: - try{ - this->stopPub.publish(Stop::message_type()); - SystemControlState = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "stop:", - ControlResponseBuffer, 0, &ClientSocket, 0); - } - catch(...){this->disconnectPub.publish(Disconnect::message_type()); - RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending DISCONSTOPNECTSTOP command"); - SystemControlState = SERVER_STATE_ERROR; - } - break; - case AbortScenario_0: - try{ - this->abortPub.publish(Abort::message_type()); - SystemControlState = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - if (ClientSocket >= 0) { - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - ControlResponseBuffer[0] = 1; - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "AbortScenario:", - ControlResponseBuffer, 1, &ClientSocket, 0); + break; + case ClearAllScenario_0: + try { + this->allClearPub.publish(AllClear::message_type()); + SystemControlState = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; + if (ClientSocket >= 0) { + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + ControlResponseBuffer[0] = 1; + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearAllScenario:", ControlResponseBuffer, 1, &ClientSocket, 0); + } + } catch (...) { + RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending COMM_ABORT_DONE command"); + SystemControlState = SERVER_STATE_ERROR; } - } - catch(...){ - RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending ABORT command"); - SystemControlState = SERVER_STATE_ERROR; - } - break; - case ClearAllScenario_0: - try{ - this->allClearPub.publish(AllClear::message_type()); - SystemControlState = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - if (ClientSocket >= 0) { + break; + case Exit_0: + try { + this->exitPub.publish(Exit::message_type()); + iExit = 1; + usleep(1000000); + SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - ControlResponseBuffer[0] = 1; - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearAllScenario:", - ControlResponseBuffer, 1, &ClientSocket, 0); - } - } - catch (...){ - RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending COMM_ABORT_DONE command"); - SystemControlState = SERVER_STATE_ERROR; - } - break; - case Exit_0: - try{ - this->exitPub.publish(Exit::message_type()); - iExit = 1; - usleep(1000000); - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "Exit:", - ControlResponseBuffer, 0, &ClientSocket, 0); - close(ClientSocket); - ClientSocket = -1; - if (USE_LOCAL_USER_CONTROL == 0) { - close(ServerHandle); - ServerHandle = -1; + SystemControlSendControlResponse( + SYSTEM_CONTROL_RESPONSE_CODE_OK, "Exit:", ControlResponseBuffer, 0, &ClientSocket, 0); + close(ClientSocket); + ClientSocket = -1; + if (USE_LOCAL_USER_CONTROL == 0) { + close(ServerHandle); + ServerHandle = -1; + } + RCLCPP_INFO(get_logger(), "Server closing"); + } catch (...) { + RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending EXIT command"); + SystemControlState = SERVER_STATE_ERROR; } - RCLCPP_INFO(get_logger(), "Server closing"); - } - catch(...){ - RCLCPP_ERROR(get_logger(), "Fatal communication fault when sending EXIT command"); - SystemControlState = SERVER_STATE_ERROR; - - } - break; + break; - default: + default: - break; + break; } } void SystemControl::pollModuleStatus() { - static auto nextPollTime = std::chrono::steady_clock::now(); + static auto nextPollTime = std::chrono::steady_clock::now(); constexpr auto pollPeriod = std::chrono::seconds(1); if (std::chrono::steady_clock::now() > nextPollTime) { getStatusPub.publish(ROSChannels::GetStatus::message_type()); @@ -1160,34 +1151,36 @@ void SystemControl::pollModuleStatus() { } } -SystemControl::SystemControlCommand_t SystemControl::SystemControlFindCommand(const char *CommandBuffer, - SystemControl::SystemControlCommand_t * CurrentCommand, - int *CommandArgCount) { +SystemControl::SystemControlCommand_t SystemControl::SystemControlFindCommand( + const char* CommandBuffer, + SystemControl::SystemControlCommand_t* CurrentCommand, + int* CommandArgCount) { SystemControl::SystemControlCommand_t command; char StrippedCommandBuffer[SYSTEM_CONTROL_COMMAND_MAX_LENGTH]; bzero(StrippedCommandBuffer, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); - //printf("CommandBuffer: %s\n", CommandBuffer); - strncpy(StrippedCommandBuffer, CommandBuffer, - (uint64_t) strchr(CommandBuffer, '(') - (uint64_t) CommandBuffer); - //printf("StrippedCommandBuffer: %s\n", StrippedCommandBuffer); + // printf("CommandBuffer: %s\n", CommandBuffer); + strncpy(StrippedCommandBuffer, CommandBuffer, (uint64_t)strchr(CommandBuffer, '(') - (uint64_t)CommandBuffer); + // printf("StrippedCommandBuffer: %s\n", StrippedCommandBuffer); - for (command = Idle_0; command != nocommand; command=(SystemControlCommand_t)(command+1)) { + for (command = Idle_0; command != nocommand; command = (SystemControlCommand_t)(command + 1)) { bzero(SystemControl::SystemControlCommandArgCnt, SYSTEM_CONTROL_ARG_CHAR_COUNT); bzero(SystemControl::SystemControlStrippedCommand, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); - strncpy(SystemControlStrippedCommand, SystemControlCommandsArr[(int)command], - (uint64_t) strchr(SystemControlCommandsArr[(int)command], - '_') - (uint64_t) SystemControlCommandsArr[(int)command]); - strncpy(SystemControlCommandArgCnt, strchr(SystemControlCommandsArr[(int)command], '_') + 1, + strncpy(SystemControlStrippedCommand, + SystemControlCommandsArr[(int)command], + (uint64_t)strchr(SystemControlCommandsArr[(int)command], '_') - + (uint64_t)SystemControlCommandsArr[(int)command]); + strncpy(SystemControlCommandArgCnt, + strchr(SystemControlCommandsArr[(int)command], '_') + 1, strlen(SystemControlCommandsArr[(int)command]) - - ((uint64_t) strchr(SystemControlCommandsArr[(int)command], '_') - - (uint64_t) SystemControlCommandsArr[(int)command] + 1)); + ((uint64_t)strchr(SystemControlCommandsArr[(int)command], '_') - + (uint64_t)SystemControlCommandsArr[(int)command] + 1)); if (!strcmp(SystemControlStrippedCommand, StrippedCommandBuffer)) { { *CommandArgCount = atoi(SystemControlCommandArgCnt); - *CurrentCommand = command; + *CurrentCommand = command; return command; } } @@ -1195,40 +1188,38 @@ SystemControl::SystemControlCommand_t SystemControl::SystemControlFindCommand(co return nocommand; } - /*! - * \brief SystemControlReceiveUserControlData Performs similarly to the recv function (see manpage for recv) except that it - * only fills the input data buffer with messages ending with ";\r\n\r\n" and saves any remaining data in a local + * \brief SystemControlReceiveUserControlData Performs similarly to the recv function (see manpage for recv) except that + * it only fills the input data buffer with messages ending with ";\r\n\r\n" and saves any remaining data in a local * buffer awaiting the next call to this function. * \param socket Socket on which MSCP HTTP communication is expected to arrive * \param dataBuffer Data buffer where read data is to be stored * \param dataBufferLength Maximum number of bytes possible to store in the data buffer - * \return Number of bytes printed to dataBuffer where 0 means that the connection has been severed. A return value of -1 - * constitutes an error with the appropriate errno has been set (see manpage for recv) with the addition of + * \return Number of bytes printed to dataBuffer where 0 means that the connection has been severed. A return value of + * -1 constitutes an error with the appropriate errno has been set (see manpage for recv) with the addition of * - ENOBUFS if the data buffer is too small to hold the received message */ -ssize_t SystemControl::SystemControlReceiveUserControlData(I32 socket, char * dataBuffer, size_t dataBufferLength) { +ssize_t SystemControl::SystemControlReceiveUserControlData(I32 socket, char* dataBuffer, size_t dataBufferLength) { static char recvBuffer[TCP_RECV_BUFFER_SIZE]; - static size_t bytesInBuffer = 0; + static size_t bytesInBuffer = 0; const char endOfMessagePattern[] = ";\r\n\r\n"; - char *endOfMessage = NULL; + char* endOfMessage = NULL; ssize_t readResult; size_t messageLength = 0; - readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof (recvBuffer) - bytesInBuffer, MSG_DONTWAIT); + readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof(recvBuffer) - bytesInBuffer, MSG_DONTWAIT); if (readResult > 0) { bytesInBuffer += (size_t)readResult; } if (bytesInBuffer > 0) { if ((endOfMessage = strstr(recvBuffer, endOfMessagePattern)) != NULL) { - endOfMessage += sizeof (endOfMessagePattern) - 1; + endOfMessage += sizeof(endOfMessagePattern) - 1; messageLength = (size_t)(endOfMessage - recvBuffer); - } - else { + } else { messageLength = 0; - readResult = -1; - errno = EAGAIN; + readResult = -1; + errno = EAGAIN; RCLCPP_WARN(get_logger(), "Part of message received"); } @@ -1236,11 +1227,10 @@ ssize_t SystemControl::SystemControlReceiveUserControlData(I32 socket, char * da if (dataBufferLength < messageLength) { RCLCPP_WARN(get_logger(), "Discarding message too large for data buffer"); readResult = -1; - errno = ENOBUFS; - } - else { + errno = ENOBUFS; + } else { memcpy(dataBuffer, recvBuffer, messageLength); - readResult = (ssize_t) messageLength; + readResult = (ssize_t)messageLength; } bytesInBuffer -= messageLength; memmove(recvBuffer, recvBuffer + messageLength, bytesInBuffer); @@ -1250,19 +1240,18 @@ ssize_t SystemControl::SystemControlReceiveUserControlData(I32 socket, char * da return readResult; } -void SystemControl::SystemControlSendMONR(const char * MONRStr, I32 * Sockfd, U8 Debug) { +void SystemControl::SystemControlSendMONR(const char* MONRStr, I32* Sockfd, U8 Debug) { int i, n, j, t; char Length[4]; - char Header[2] = { 0, 2 }; + char Header[2] = {0, 2}; char Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(MONRStr); - Length[0] = (char) (n >> 24); - Length[1] = (char) (n >> 16); - Length[2] = (char) (n >> 8); - Length[3] = (char) n; - + n = 2 + strlen(MONRStr); + Length[0] = (char)(n >> 24); + Length[1] = (char)(n >> 16); + Length[2] = (char)(n >> 8); + Length[3] = (char)n; if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { for (i = 0, j = 0; i < 4; i++, j++) @@ -1272,31 +1261,28 @@ void SystemControl::SystemControlSendMONR(const char * MONRStr, I32 * Sockfd, U8 t = strlen(MONRStr); for (i = 0; i < t; i++, j++) Data[j] = *(MONRStr + i); - //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData((uint8_t*) "System Control", (uint8_t*) Data, n + 4, Sockfd, 0); - } - else + // SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData((uint8_t*)"System Control", (uint8_t*)Data, n + 4, Sockfd, 0); + } else RCLCPP_ERROR(get_logger(), "MONR string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); } - -void SystemControl::SystemControlSendLog(const char * LogString, I32 * Sockfd, U8 Debug) { +void SystemControl::SystemControlSendLog(const char* LogString, I32* Sockfd, U8 Debug) { int i, n, j, t; char Length[4]; - char Header[2] = { 0, 2 }; + char Header[2] = {0, 2}; char Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(LogString); - Length[0] = (char) (n >> 24); - Length[1] = (char) (n >> 16); - Length[2] = (char) (n >> 8); - Length[3] = (char) n; - - //SystemControlSendBytes(Length, 4, Sockfd, 0); - //SystemControlSendBytes(Header, 5, Sockfd, 0); - //SystemControlSendBytes(LogString, strlen(LogString), Sockfd, 0); + n = 2 + strlen(LogString); + Length[0] = (char)(n >> 24); + Length[1] = (char)(n >> 16); + Length[2] = (char)(n >> 8); + Length[3] = (char)n; + // SystemControlSendBytes(Length, 4, Sockfd, 0); + // SystemControlSendBytes(Header, 5, Sockfd, 0); + // SystemControlSendBytes(LogString, strlen(LogString), Sockfd, 0); if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { for (i = 0, j = 0; i < 4; i++, j++) @@ -1306,30 +1292,31 @@ void SystemControl::SystemControlSendLog(const char * LogString, I32 * Sockfd, U t = strlen(LogString); for (i = 0; i < t; i++, j++) Data[j] = *(LogString + i); - //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData((uint8_t*) "System Control", (uint8_t*) Data, n + 4, Sockfd, 0); - } - else + // SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData((uint8_t*)"System Control", (uint8_t*)Data, n + 4, Sockfd, 0); + } else RCLCPP_ERROR(get_logger(), "Log string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); - } - -void SystemControl::SystemControlSendControlResponse(U16 ResponseStatus, const char * ResponseString, const char * ResponseData, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { +void SystemControl::SystemControlSendControlResponse(U16 ResponseStatus, + const char* ResponseString, + const char* ResponseData, + I32 ResponseDataLength, + I32* Sockfd, + U8 Debug) { int i, n, j, t; char Length[4]; char Status[2]; char Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(ResponseString) + ResponseDataLength; - Length[0] = (char) (n >> 24); - Length[1] = (char) (n >> 16); - Length[2] = (char) (n >> 8); - Length[3] = (char) n; - Status[0] = (char) (ResponseStatus >> 8); - Status[1] = (char) ResponseStatus; + n = 2 + strlen(ResponseString) + ResponseDataLength; + Length[0] = (char)(n >> 24); + Length[1] = (char)(n >> 16); + Length[2] = (char)(n >> 8); + Length[3] = (char)n; + Status[0] = (char)(ResponseStatus >> 8); + Status[1] = (char)ResponseStatus; if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { for (i = 0, j = 0; i < 4; i++, j++) @@ -1348,29 +1335,30 @@ void SystemControl::SystemControlSendControlResponse(U16 ResponseStatus, const c printf("\n"); } - //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData((uint8_t*) "System Control", (uint8_t*) Data, n + 4, Sockfd, 0); - } - else + // SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData((uint8_t*)"System Control", (uint8_t*)Data, n + 4, Sockfd, 0); + } else RCLCPP_ERROR(get_logger(), "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); } - -void SystemControl::SystemControlFileDownloadResponse(U16 ResponseStatus, const char * ResponseString, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { +void SystemControl::SystemControlFileDownloadResponse(U16 ResponseStatus, + const char* ResponseString, + I32 ResponseDataLength, + I32* Sockfd, + U8 Debug) { int i, n, j, t; char Length[MSCP_RESPONSE_DATALENGTH_BYTES]; char Status[MSCP_RESPONSE_STATUS_CODE_BYTES]; char Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = MSCP_RESPONSE_STATUS_CODE_BYTES + strlen(ResponseString) + ResponseDataLength; - Length[0] = (char) (n >> 24); - Length[1] = (char) (n >> 16); - Length[2] = (char) (n >> 8); - Length[3] = (char) n; - Status[0] = (char) (ResponseStatus >> 8); - Status[1] = (char) ResponseStatus; + n = MSCP_RESPONSE_STATUS_CODE_BYTES + strlen(ResponseString) + ResponseDataLength; + Length[0] = (char)(n >> 24); + Length[1] = (char)(n >> 16); + Length[2] = (char)(n >> 8); + Length[3] = (char)n; + Status[0] = (char)(ResponseStatus >> 8); + Status[1] = (char)ResponseStatus; for (i = 0, j = 0; i < MSCP_RESPONSE_DATALENGTH_BYTES; i++, j++) Data[j] = Length[i]; @@ -1386,28 +1374,31 @@ void SystemControl::SystemControlFileDownloadResponse(U16 ResponseStatus, const printf("\n"); } - UtilSendTCPData((uint8_t*) "System Control", (uint8_t*) Data, - MSCP_RESPONSE_DATALENGTH_BYTES + MSCP_RESPONSE_STATUS_CODE_BYTES + - strlen(ResponseString), Sockfd, 0); + UtilSendTCPData((uint8_t*)"System Control", + (uint8_t*)Data, + MSCP_RESPONSE_DATALENGTH_BYTES + MSCP_RESPONSE_STATUS_CODE_BYTES + strlen(ResponseString), + Sockfd, + 0); } - - -I32 SystemControl::SystemControlBuildControlResponse(U16 ResponseStatus, char * ResponseString, char * ResponseData, - I32 ResponseDataLength, U8 Debug) { +I32 SystemControl::SystemControlBuildControlResponse(U16 ResponseStatus, + char* ResponseString, + char* ResponseData, + I32 ResponseDataLength, + U8 Debug) { int i = 0, n = 0, j = 0, t = 0; char Length[MSCP_RESPONSE_DATALENGTH_BYTES]; char Status[MSCP_RESPONSE_STATUS_CODE_BYTES]; char Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = MSCP_RESPONSE_STATUS_CODE_BYTES + strlen(ResponseString) + ResponseDataLength; - Length[0] = (char) (n >> 24); - Length[1] = (char) (n >> 16); - Length[2] = (char) (n >> 8); - Length[3] = (char) n; - Status[0] = (char) (ResponseStatus >> 8); - Status[1] = (char) ResponseStatus; + n = MSCP_RESPONSE_STATUS_CODE_BYTES + strlen(ResponseString) + ResponseDataLength; + Length[0] = (char)(n >> 24); + Length[1] = (char)(n >> 16); + Length[2] = (char)(n >> 8); + Length[3] = (char)n; + Status[0] = (char)(ResponseStatus >> 8); + Status[1] = (char)ResponseStatus; if (n + MSCP_RESPONSE_DATALENGTH_BYTES < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { for (i = 0, j = 0; i < MSCP_RESPONSE_DATALENGTH_BYTES; i++, j++) @@ -1421,7 +1412,7 @@ I32 SystemControl::SystemControlBuildControlResponse(U16 ResponseStatus, char * Data[j] = ResponseData[i]; for (i = 0; i < n; i++) - *(ResponseData + i) = Data[i]; //Copy back + *(ResponseData + i) = Data[i]; // Copy back if (Debug) { for (i = 0; i < n + MSCP_RESPONSE_DATALENGTH_BYTES; i++) @@ -1429,18 +1420,13 @@ I32 SystemControl::SystemControlBuildControlResponse(U16 ResponseStatus, char * printf("\n"); } - } - else + } else RCLCPP_ERROR(get_logger(), "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); - - - return n; } - -void SystemControl::SystemControlSendBytes(const char *data, int length, int *sockfd, int debug) { +void SystemControl::SystemControlSendBytes(const char* data, int length, int* sockfd, int debug) { int i, n; if (debug == 1) { @@ -1448,7 +1434,7 @@ void SystemControl::SystemControlSendBytes(const char *data, int length, int *so int i = 0; for (i = 0; i < length; i++) - printf("%d ", (char) * (data + i)); + printf("%d ", (char)*(data + i)); printf("\n"); } @@ -1458,22 +1444,19 @@ void SystemControl::SystemControlSendBytes(const char *data, int length, int *so } } - - -I32 SystemControl::SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct in_addr *ip_addr) { +I32 SystemControl::SystemControlInitServer(int* ClientSocket, int* ServerHandle, struct in_addr* ip_addr) { struct sockaddr_in command_server_addr; struct sockaddr_in cli_addr; socklen_t cli_length; unsigned int control_port = SYSTEM_CONTROL_CONTROL_PORT; - int optval = 1; - int result = 0; - int sockFlags = 0; + int optval = 1; + int result = 0; + int sockFlags = 0; enum COMMAND iCommand; ssize_t bytesReceived = 0; - /* Init user control socket */ RCLCPP_INFO(get_logger(), "Init control socket"); @@ -1483,11 +1466,11 @@ I32 SystemControl::SystemControlInitServer(int *ClientSocket, int *ServerHandle, ::exit(1); } - bzero((char *)&command_server_addr, sizeof (command_server_addr)); + bzero((char*)&command_server_addr, sizeof(command_server_addr)); - command_server_addr.sin_family = AF_INET; + command_server_addr.sin_family = AF_INET; command_server_addr.sin_addr.s_addr = INADDR_ANY; - command_server_addr.sin_port = htons(control_port); + command_server_addr.sin_port = htons(control_port); optval = 1; result = setsockopt(*ServerHandle, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof optval); @@ -1497,7 +1480,7 @@ I32 SystemControl::SystemControlInitServer(int *ClientSocket, int *ServerHandle, ::exit(1); } - if (bind(*ServerHandle, (struct sockaddr *)&command_server_addr, sizeof (command_server_addr)) < 0) { + if (bind(*ServerHandle, (struct sockaddr*)&command_server_addr, sizeof(command_server_addr)) < 0) { perror("[SystemControl] ERR: Failed to bind to control socket"); ::exit(1); } @@ -1506,7 +1489,7 @@ I32 SystemControl::SystemControlInitServer(int *ClientSocket, int *ServerHandle, RCLCPP_INFO(get_logger(), "Listening for connection from client..."); listen(*ServerHandle, 1); - cli_length = sizeof (cli_addr); + cli_length = sizeof(cli_addr); /* Set socket to nonblocking */ sockFlags = fcntl(*ServerHandle, F_GETFL, 0); @@ -1518,15 +1501,15 @@ I32 SystemControl::SystemControlInitServer(int *ClientSocket, int *ServerHandle, util_error("Error calling fcntl"); do { - *ClientSocket = accept(*ServerHandle, (struct sockaddr *)&cli_addr, &cli_length); + *ClientSocket = accept(*ServerHandle, (struct sockaddr*)&cli_addr, &cli_length); if ((*ClientSocket == -1 && errno != EAGAIN && errno != EWOULDBLOCK) || iExit) util_error("Failed to establish connection"); } while (*ClientSocket == -1); - RCLCPP_INFO(get_logger(), "Connection established: %s:%i", inet_ntoa(cli_addr.sin_addr), - htons(command_server_addr.sin_port)); + RCLCPP_INFO( + get_logger(), "Connection established: %s:%i", inet_ntoa(cli_addr.sin_addr), htons(command_server_addr.sin_port)); - ip_addr->s_addr = cli_addr.sin_addr.s_addr; //Set IP-address of Usercontrol + ip_addr->s_addr = cli_addr.sin_addr.s_addr; // Set IP-address of Usercontrol if (*ClientSocket < 0) { perror("[SystemControl] ERR: Failed to accept from central"); @@ -1536,11 +1519,9 @@ I32 SystemControl::SystemControlInitServer(int *ClientSocket, int *ServerHandle, return result; } - - -I32 SystemControl::SystemControlConnectServer(int *sockfd, const char *name, const uint32_t port) { +I32 SystemControl::SystemControlConnectServer(int* sockfd, const char* name, const uint32_t port) { struct sockaddr_in serv_addr; - struct hostent *server; + struct hostent* server; char buffer[256]; int iResult; @@ -1556,41 +1537,39 @@ I32 SystemControl::SystemControlConnectServer(int *sockfd, const char *name, con util_error("[SystemControl] ERR: Unknown host "); } - bzero((char *)&serv_addr, sizeof (serv_addr)); + bzero((char*)&serv_addr, sizeof(serv_addr)); serv_addr.sin_family = AF_INET; - bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); + bcopy((char*)server->h_addr, (char*)&serv_addr.sin_addr.s_addr, server->h_length); serv_addr.sin_port = htons(port); RCLCPP_INFO(get_logger(), "Attempting to connect to control socket: %s:%i", name, port); do { - iResult = ::connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); + iResult = ::connect(*sockfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)); if (iResult < 0) { if (errno == ECONNREFUSED) { RCLCPP_WARN(get_logger(), "Unable to connect to UserControl, retrying in 3 sec..."); (void)sleep(3); - } - else { + } else { util_error("[SystemControl] ERR: Failed to connect to control socket"); } } } while (iResult < 0); - iResult = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); RCLCPP_DEBUG(get_logger(), "ATOS connected to UserControl: %s:%i", name, port); return iResult; - } - -void SystemControl::SystemControlCreateProcessChannel(const char * name, const U32 port, I32 * sockfd, - struct sockaddr_in *addr) { +void SystemControl::SystemControlCreateProcessChannel(const char* name, + const U32 port, + I32* sockfd, + struct sockaddr_in* addr) { int result; - struct hostent *object; + struct hostent* object; /* Connect to object safety socket */ @@ -1608,9 +1587,9 @@ void SystemControl::SystemControlCreateProcessChannel(const char * name, const U util_error("[SystemControl] ERR: Unknown host"); } - bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); + bcopy((char*)object->h_addr, (char*)&addr->sin_addr.s_addr, object->h_length); addr->sin_family = AF_INET; - addr->sin_port = htons(port); + addr->sin_port = htons(port); /* set socket to non-blocking */ result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); @@ -1626,7 +1605,7 @@ void SystemControl::SystemControlCreateProcessChannel(const char * name, const U * \param addr IP address to match against own IPs * \return true if match, false if not */ -char SystemControl::SystemControlVerifyHostAddress(char *addr) { +char SystemControl::SystemControlVerifyHostAddress(char* addr) { struct ifaddrs *ifaddr, *ifa; int family, s, n; char host[NI_MAXHOST]; @@ -1647,8 +1626,12 @@ char SystemControl::SystemControlVerifyHostAddress(char *addr) { family = ifa->ifa_addr->sa_family; if (family == AF_INET || family == AF_INET6) { s = getnameinfo(ifa->ifa_addr, - (family == AF_INET) ? sizeof (struct sockaddr_in) : - sizeof (struct sockaddr_in6), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST); + (family == AF_INET) ? sizeof(struct sockaddr_in) : sizeof(struct sockaddr_in6), + host, + NI_MAXHOST, + NULL, + 0, + NI_NUMERICHOST); if (s != 0) { RCLCPP_ERROR(get_logger(), "getnameinfo() failed: %s", gai_strerror(s)); continue; @@ -1658,8 +1641,7 @@ char SystemControl::SystemControlVerifyHostAddress(char *addr) { return 1; else continue; - } - else { + } else { continue; } } @@ -1668,14 +1650,12 @@ char SystemControl::SystemControlVerifyHostAddress(char *addr) { return 0; } - -I32 SystemControl::SystemControlGetServerParameter(char * ParameterName, char * ReturnValue, U32 BufferLength, - U8 Debug) { +I32 SystemControl::SystemControlGetServerParameter(char* ParameterName, char* ReturnValue, U32 BufferLength, U8 Debug) { bzero(ReturnValue, 20); dbl ValueDbl = 0; U32 ValueU32 = 0; U16 ValueU16 = 0; - U8 ValueU8 = 0; + U8 ValueU8 = 0; sprintf(ReturnValue, "%s", ParameterName); strcat(ReturnValue, ": "); @@ -1684,99 +1664,75 @@ I32 SystemControl::SystemControlGetServerParameter(char * ParameterName, char * if (strcmp("OriginLatitude", ParameterName) == 0) { DataDictionaryGetOriginLatitudeDbl(&ValueDbl); sprintf(ReturnValue + strlen(ReturnValue), "%3.12f", ValueDbl); - } - else if (strcmp("OriginLongitude", ParameterName) == 0) { + } else if (strcmp("OriginLongitude", ParameterName) == 0) { DataDictionaryGetOriginLongitudeDbl(&ValueDbl); sprintf(ReturnValue + strlen(ReturnValue), "%3.12f", ValueDbl); - } - else if (strcmp("OriginAltitude", ParameterName) == 0) { + } else if (strcmp("OriginAltitude", ParameterName) == 0) { DataDictionaryGetOriginAltitudeDbl(&ValueDbl); sprintf(ReturnValue + strlen(ReturnValue), "%3.12f", ValueDbl); - } - else if (strcmp("VisualizationServerName", ParameterName) == 0) { + } else if (strcmp("VisualizationServerName", ParameterName) == 0) { DataDictionaryGetVisualizationServerIPString(ReturnValue + strlen(ReturnValue), BufferLength); - } - else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) { + } else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) { DataDictionaryGetASPMaxTimeDiffDbl(&ValueDbl); sprintf(ReturnValue + strlen(ReturnValue), "%3.3f", ValueDbl); - } - else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) { + } else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) { DataDictionaryGetASPMaxTrajDiffDbl(&ValueDbl); sprintf(ReturnValue + strlen(ReturnValue), "%3.3f", ValueDbl); - } - else if (strcmp("ASPStepBackCount", ParameterName) == 0) { + } else if (strcmp("ASPStepBackCount", ParameterName) == 0) { DataDictionaryGetASPStepBackCountU32(&ValueU32); sprintf(ReturnValue + strlen(ReturnValue), "%" PRIu32, ValueU32); - } - else if (strcmp("ASPFilterLevel", ParameterName) == 0) { + } else if (strcmp("ASPFilterLevel", ParameterName) == 0) { DataDictionaryGetASPFilterLevelDbl(&ValueDbl); sprintf(ReturnValue + strlen(ReturnValue), "%3.3f", ValueDbl); - } - else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) { + } else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) { DataDictionaryGetASPMaxDeltaTimeDbl(&ValueDbl); sprintf(ReturnValue + strlen(ReturnValue), "%3.3f", ValueDbl); - } - else if (strcmp("TimeServerIP", ParameterName) == 0) { + } else if (strcmp("TimeServerIP", ParameterName) == 0) { DataDictionaryGetTimeServerIPString(ReturnValue + strlen(ReturnValue), BufferLength); - } - else if (strcmp("TimeServerPort", ParameterName) == 0) { + } else if (strcmp("TimeServerPort", ParameterName) == 0) { DataDictionaryGetTimeServerPortU16(&ValueU16); sprintf(ReturnValue, "%" PRIu16, ValueU16); - } - else if (strcmp("SimulatorIP", ParameterName) == 0) { + } else if (strcmp("SimulatorIP", ParameterName) == 0) { DataDictionaryGetSimulatorIPString(ReturnValue + strlen(ReturnValue), BufferLength); - } - else if (strcmp("SimulatorTCPPort", ParameterName) == 0) { + } else if (strcmp("SimulatorTCPPort", ParameterName) == 0) { DataDictionaryGetSimulatorTCPPortU16(&ValueU16); sprintf(ReturnValue + strlen(ReturnValue), "%" PRIu16, ValueU16); - } - else if (strcmp("SimulatorUDPPort", ParameterName) == 0) { + } else if (strcmp("SimulatorUDPPort", ParameterName) == 0) { DataDictionaryGetSimulatorUDPPortU16(&ValueU16); sprintf(ReturnValue + strlen(ReturnValue), "%" PRIu16, ValueU16); - } - else if (strcmp("SimulatorMode", ParameterName) == 0) { + } else if (strcmp("SimulatorMode", ParameterName) == 0) { DataDictionaryGetSimulatorModeU8(&ValueU8); sprintf(ReturnValue + strlen(ReturnValue), "%" PRIu8, ValueU8); - } - else if (strcmp("VOILReceivers", ParameterName) == 0) { + } else if (strcmp("VOILReceivers", ParameterName) == 0) { DataDictionaryGetVOILReceiversString(ReturnValue + strlen(ReturnValue), BufferLength); - } - else if (strcmp("DTMReceivers", ParameterName) == 0) { + } else if (strcmp("DTMReceivers", ParameterName) == 0) { DataDictionaryGetDTMReceiversString(ReturnValue + strlen(ReturnValue), BufferLength); - } - else if (strcmp("SupervisorIP", ParameterName) == 0) { + } else if (strcmp("SupervisorIP", ParameterName) == 0) { DataDictionaryGetExternalSupervisorIPString(ReturnValue + strlen(ReturnValue), BufferLength); - } - else if (strcmp("SupervisorTCPPort", ParameterName) == 0) { + } else if (strcmp("SupervisorTCPPort", ParameterName) == 0) { DataDictionaryGetSupervisorTCPPortU16(&ValueU16); sprintf(ReturnValue + strlen(ReturnValue), "%" PRIu16, ValueU16); - } - else if (strcmp("MiscData", ParameterName) == 0) { + } else if (strcmp("MiscData", ParameterName) == 0) { DataDictionaryGetMiscData(ReturnValue + strlen(ReturnValue), BufferLength); - } - else if (strcmp("RVSSConfig", ParameterName) == 0) { + } else if (strcmp("RVSSConfig", ParameterName) == 0) { DataDictionaryGetRVSSConfigU32(&ValueU32); sprintf(ReturnValue + strlen(ReturnValue), "%" PRIu32, ValueU32); - } - else if (strcmp("RVSSRate", ParameterName) == 0) { + } else if (strcmp("RVSSRate", ParameterName) == 0) { DataDictionaryGetRVSSRateU8(&ValueU8); sprintf(ReturnValue + strlen(ReturnValue), "%" PRIu8, ValueU8); - } - else if (strcmp("ScenarioName", ParameterName) == 0) { + } else if (strcmp("ScenarioName", ParameterName) == 0) { DataDictionaryGetScenarioName(ReturnValue + strlen(ReturnValue), BufferLength); } return 0; } - - -I32 SystemControl::SystemControlSetServerParameter(char * parameterName, char * newValue, U8 debug) { +I32 SystemControl::SystemControlSetServerParameter(char* parameterName, char* newValue, U8 debug) { ReadWriteAccess_t result = PARAMETER_NOTFOUND; U32 object_transmitter_ids[MAX_OBJECTS]; U32 numberOfObjects; GeoPositionType origin; - char *endptr; + char* endptr; if (parameterName == NULL || newValue == NULL) { RCLCPP_ERROR(get_logger(), "Set server parameter null pointer error"); @@ -1784,223 +1740,233 @@ I32 SystemControl::SystemControlSetServerParameter(char * parameterName, char * } if (debug) { - RCLCPP_WARN(get_logger(),"SetServerParameter: %s = %s", parameterName, newValue); + RCLCPP_WARN(get_logger(), "SetServerParameter: %s = %s", parameterName, newValue); } enum ConfigurationFileParameter parameter = - UtilParseConfigurationParameter(parameterName, strlen(parameterName) + 1); + UtilParseConfigurationParameter(parameterName, strlen(parameterName) + 1); switch (parameter) { - case CONFIGURATION_PARAMETER_SCENARIO_NAME: - result = DataDictionarySetScenarioName(newValue, strlen(newValue) + 1); - break; - case CONFIGURATION_PARAMETER_ORIGIN_LATITUDE: - if ((result = DataDictionaryGetNumberOfObjects(&numberOfObjects)) != READ_OK) { - RCLCPP_ERROR(get_logger(), "Data dictionary number of objects read error" - "- cannot set origin on objects"); - break; - } - - if ((result = - DataDictionaryGetObjectTransmitterIDs(object_transmitter_ids, numberOfObjects)) != READ_OK) { - RCLCPP_ERROR(get_logger(), - "Data dictionary TransmitterID read error" "- cannot set origin on objects"); + case CONFIGURATION_PARAMETER_SCENARIO_NAME: + result = DataDictionarySetScenarioName(newValue, strlen(newValue) + 1); break; - } + case CONFIGURATION_PARAMETER_ORIGIN_LATITUDE: + if ((result = DataDictionaryGetNumberOfObjects(&numberOfObjects)) != READ_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary number of objects read error" + "- cannot set origin on objects"); + break; + } - for (unsigned int i = 0; i < numberOfObjects; ++i) { - if (object_transmitter_ids[i] == 0) { - continue; + if ((result = DataDictionaryGetObjectTransmitterIDs(object_transmitter_ids, numberOfObjects)) != READ_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary TransmitterID read error" + "- cannot set origin on objects"); + break; } - else { - if (DataDictionaryGetOrigin(object_transmitter_ids[i], &origin) != READ_OK) { - RCLCPP_ERROR(get_logger(), "Data dictionary origin read error" - "- cannot read the origin for object %u", object_transmitter_ids[i]); - result = UNDEFINED; + + for (unsigned int i = 0; i < numberOfObjects; ++i) { + if (object_transmitter_ids[i] == 0) { continue; - } - origin.Latitude = strtod(newValue, &endptr); - if (endptr == newValue) { - RCLCPP_ERROR(get_logger(), "Unable to convert origin setting %s to double", newValue); - result = UNDEFINED; - break; - } - if (DataDictionarySetOrigin(&object_transmitter_ids[i], &origin) != WRITE_OK) { - RCLCPP_ERROR(get_logger(), "Data dictionary origin write error" - "- cannot set the origin for object %u", object_transmitter_ids[i]); - result = UNDEFINED; - break; + } else { + if (DataDictionaryGetOrigin(object_transmitter_ids[i], &origin) != READ_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary origin read error" + "- cannot read the origin for object %u", + object_transmitter_ids[i]); + result = UNDEFINED; + continue; + } + origin.Latitude = strtod(newValue, &endptr); + if (endptr == newValue) { + RCLCPP_ERROR(get_logger(), "Unable to convert origin setting %s to double", newValue); + result = UNDEFINED; + break; + } + if (DataDictionarySetOrigin(&object_transmitter_ids[i], &origin) != WRITE_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary origin write error" + "- cannot set the origin for object %u", + object_transmitter_ids[i]); + result = UNDEFINED; + break; + } } } - } - - // TODO remove - result = DataDictionarySetOriginLatitudeDbl(newValue); - break; - case CONFIGURATION_PARAMETER_ORIGIN_LONGITUDE: - if ((result = DataDictionaryGetNumberOfObjects(&numberOfObjects)) != READ_OK) { - RCLCPP_ERROR(get_logger(), "Data dictionary number of objects read error" - "- cannot set origin on objects"); - break; - } - if ((result = - DataDictionaryGetObjectTransmitterIDs(object_transmitter_ids, numberOfObjects)) != READ_OK) { - RCLCPP_ERROR(get_logger(), - "Data dictionary TransmitterID read error" "- cannot set origin on objects"); + // TODO remove + result = DataDictionarySetOriginLatitudeDbl(newValue); break; - } + case CONFIGURATION_PARAMETER_ORIGIN_LONGITUDE: + if ((result = DataDictionaryGetNumberOfObjects(&numberOfObjects)) != READ_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary number of objects read error" + "- cannot set origin on objects"); + break; + } - for (unsigned int i = 0; i < numberOfObjects; ++i) { - if (object_transmitter_ids[i] == 0) { - continue; + if ((result = DataDictionaryGetObjectTransmitterIDs(object_transmitter_ids, numberOfObjects)) != READ_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary TransmitterID read error" + "- cannot set origin on objects"); + break; } - else { - if (DataDictionaryGetOrigin(object_transmitter_ids[i], &origin) != READ_OK) { - RCLCPP_ERROR(get_logger(), "Data dictionary origin read error" - "- cannot read the origin for object %u", object_transmitter_ids[i]); - result = UNDEFINED; + + for (unsigned int i = 0; i < numberOfObjects; ++i) { + if (object_transmitter_ids[i] == 0) { continue; - } - origin.Longitude = strtod(newValue, &endptr); - if (endptr == newValue) { - RCLCPP_ERROR(get_logger(), "Unable to convert origin setting %s to double", newValue); - result = UNDEFINED; - break; - } - if (DataDictionarySetOrigin(&object_transmitter_ids[i], &origin) != WRITE_OK) { - RCLCPP_ERROR(get_logger(), "Data dictionary origin write error" - "- cannot set the origin for object %u", object_transmitter_ids[i]); - result = UNDEFINED; - break; + } else { + if (DataDictionaryGetOrigin(object_transmitter_ids[i], &origin) != READ_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary origin read error" + "- cannot read the origin for object %u", + object_transmitter_ids[i]); + result = UNDEFINED; + continue; + } + origin.Longitude = strtod(newValue, &endptr); + if (endptr == newValue) { + RCLCPP_ERROR(get_logger(), "Unable to convert origin setting %s to double", newValue); + result = UNDEFINED; + break; + } + if (DataDictionarySetOrigin(&object_transmitter_ids[i], &origin) != WRITE_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary origin write error" + "- cannot set the origin for object %u", + object_transmitter_ids[i]); + result = UNDEFINED; + break; + } } } - } - - // TODO remove - result = DataDictionarySetOriginLongitudeDbl(newValue); - break; - case CONFIGURATION_PARAMETER_ORIGIN_ALTITUDE: - if ((result = DataDictionaryGetNumberOfObjects(&numberOfObjects)) != READ_OK) { - RCLCPP_ERROR(get_logger(), "Data dictionary number of objects read error" - "- cannot set origin on objects"); - break; - } - if ((result = - DataDictionaryGetObjectTransmitterIDs(object_transmitter_ids, numberOfObjects)) != READ_OK) { - RCLCPP_ERROR(get_logger(), - "Data dictionary TransmitterID read error" "- cannot set origin on objects"); + // TODO remove + result = DataDictionarySetOriginLongitudeDbl(newValue); break; - } + case CONFIGURATION_PARAMETER_ORIGIN_ALTITUDE: + if ((result = DataDictionaryGetNumberOfObjects(&numberOfObjects)) != READ_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary number of objects read error" + "- cannot set origin on objects"); + break; + } - for (unsigned int i = 0; i < numberOfObjects; ++i) { - if (object_transmitter_ids[i] == 0) { - continue; + if ((result = DataDictionaryGetObjectTransmitterIDs(object_transmitter_ids, numberOfObjects)) != READ_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary TransmitterID read error" + "- cannot set origin on objects"); + break; } - else { - if (DataDictionaryGetOrigin(object_transmitter_ids[i], &origin) != READ_OK) { - RCLCPP_ERROR(get_logger(), "Data dictionary origin read error" - "- cannot read the origin for object %u", object_transmitter_ids[i]); - result = UNDEFINED; + + for (unsigned int i = 0; i < numberOfObjects; ++i) { + if (object_transmitter_ids[i] == 0) { continue; - } - origin.Altitude = strtod(newValue, &endptr); - if (endptr == newValue) { - RCLCPP_ERROR(get_logger(), "Unable to convert origin setting %s to double", newValue); - result = UNDEFINED; - break; - } - if (DataDictionarySetOrigin(&object_transmitter_ids[i], &origin) != WRITE_OK) { - RCLCPP_ERROR(get_logger(), "Data dictionary origin write error" - "- cannot set the origin for object %u", object_transmitter_ids[i]); - result = UNDEFINED; - break; + } else { + if (DataDictionaryGetOrigin(object_transmitter_ids[i], &origin) != READ_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary origin read error" + "- cannot read the origin for object %u", + object_transmitter_ids[i]); + result = UNDEFINED; + continue; + } + origin.Altitude = strtod(newValue, &endptr); + if (endptr == newValue) { + RCLCPP_ERROR(get_logger(), "Unable to convert origin setting %s to double", newValue); + result = UNDEFINED; + break; + } + if (DataDictionarySetOrigin(&object_transmitter_ids[i], &origin) != WRITE_OK) { + RCLCPP_ERROR(get_logger(), + "Data dictionary origin write error" + "- cannot set the origin for object %u", + object_transmitter_ids[i]); + result = UNDEFINED; + break; + } } } - } - // TODO remove - result = DataDictionarySetOriginAltitudeDbl(newValue); - break; - case CONFIGURATION_PARAMETER_VISUALIZATION_SERVER_NAME: - result = DataDictionarySetVisualizationServerU32(newValue); - break; - case CONFIGURATION_PARAMETER_ASP_MAX_TIME_DIFF: - result = DataDictionarySetASPMaxTimeDiffDbl(newValue); - break; - case CONFIGURATION_PARAMETER_ASP_MAX_TRAJ_DIFF: - result = DataDictionarySetASPMaxTrajDiffDbl(newValue); - break; - case CONFIGURATION_PARAMETER_ASP_STEP_BACK_COUNT: - result = DataDictionarySetASPStepBackCountU32(newValue); - break; - case CONFIGURATION_PARAMETER_ASP_FILTER_LEVEL: - result = DataDictionarySetASPFilterLevelDbl(newValue); - break; - case CONFIGURATION_PARAMETER_ASP_MAX_DELTA_TIME: - result = DataDictionarySetASPMaxDeltaTimeDbl(newValue); - break; - case CONFIGURATION_PARAMETER_TIME_SERVER_IP: - result = DataDictionarySetTimeServerIPU32(newValue); - break; - case CONFIGURATION_PARAMETER_TIME_SERVER_PORT: - result = DataDictionarySetTimeServerPortU16(newValue); - break; - case CONFIGURATION_PARAMETER_SIMULATOR_IP: - result = DataDictionarySetSimulatorIPU32(newValue); - break; - case CONFIGURATION_PARAMETER_SIMULATOR_PORT_TCP: - result = DataDictionarySetSimulatorTCPPortU16(newValue); - break; - case CONFIGURATION_PARAMETER_SIMULATOR_PORT_UDP: - result = DataDictionarySetSimulatorUDPPortU16(newValue); - break; - case CONFIGURATION_PARAMETER_SIMULATOR_MODE: - result = DataDictionarySetSimulatorModeU8(newValue); - break; - case CONFIGURATION_PARAMETER_VOIL_RECEIVERS: - result = DataDictionarySetVOILReceiversString(newValue); - break; - case CONFIGURATION_PARAMETER_DTM_RECEIVERS: - result = DataDictionarySetDTMReceiversString(newValue); - break; - case CONFIGURATION_PARAMETER_EXTERNAL_SUPERVISOR_IP: - result = DataDictionarySetExternalSupervisorIPU32(newValue); - break; - case CONFIGURATION_PARAMETER_EXTERNAL_SUPERVISOR_PORT_TCP: - result = DataDictionarySetSupervisorTCPPortU16(newValue); - break; - case CONFIGURATION_PARAMETER_RVSS_CONFIG: - result = DataDictionarySetRVSSConfigU32((uint32_t) strtoul(newValue, NULL, 10)); - break; - case CONFIGURATION_PARAMETER_RVSS_RATE: - result = DataDictionarySetRVSSRateU8((uint8_t) strtoul(newValue, NULL, 10)); - break; - case CONFIGURATION_PARAMETER_MISC_DATA: - RCLCPP_WARN(get_logger(), "Unable to set MiscData - size unknown"); - result = DataDictionarySetMiscData(newValue, 0); - break; - case CONFIGURATION_PARAMETER_INVALID: - RCLCPP_WARN(get_logger(), "Attempted to set invalid parameter %s", parameterName); - default: - RCLCPP_ERROR(get_logger(), "No action is implemented for setting parameter %s"); + // TODO remove + result = DataDictionarySetOriginAltitudeDbl(newValue); + break; + case CONFIGURATION_PARAMETER_VISUALIZATION_SERVER_NAME: + result = DataDictionarySetVisualizationServerU32(newValue); + break; + case CONFIGURATION_PARAMETER_ASP_MAX_TIME_DIFF: + result = DataDictionarySetASPMaxTimeDiffDbl(newValue); + break; + case CONFIGURATION_PARAMETER_ASP_MAX_TRAJ_DIFF: + result = DataDictionarySetASPMaxTrajDiffDbl(newValue); + break; + case CONFIGURATION_PARAMETER_ASP_STEP_BACK_COUNT: + result = DataDictionarySetASPStepBackCountU32(newValue); + break; + case CONFIGURATION_PARAMETER_ASP_FILTER_LEVEL: + result = DataDictionarySetASPFilterLevelDbl(newValue); + break; + case CONFIGURATION_PARAMETER_ASP_MAX_DELTA_TIME: + result = DataDictionarySetASPMaxDeltaTimeDbl(newValue); + break; + case CONFIGURATION_PARAMETER_TIME_SERVER_IP: + result = DataDictionarySetTimeServerIPU32(newValue); + break; + case CONFIGURATION_PARAMETER_TIME_SERVER_PORT: + result = DataDictionarySetTimeServerPortU16(newValue); + break; + case CONFIGURATION_PARAMETER_SIMULATOR_IP: + result = DataDictionarySetSimulatorIPU32(newValue); + break; + case CONFIGURATION_PARAMETER_SIMULATOR_PORT_TCP: + result = DataDictionarySetSimulatorTCPPortU16(newValue); + break; + case CONFIGURATION_PARAMETER_SIMULATOR_PORT_UDP: + result = DataDictionarySetSimulatorUDPPortU16(newValue); + break; + case CONFIGURATION_PARAMETER_SIMULATOR_MODE: + result = DataDictionarySetSimulatorModeU8(newValue); + break; + case CONFIGURATION_PARAMETER_VOIL_RECEIVERS: + result = DataDictionarySetVOILReceiversString(newValue); + break; + case CONFIGURATION_PARAMETER_DTM_RECEIVERS: + result = DataDictionarySetDTMReceiversString(newValue); + break; + case CONFIGURATION_PARAMETER_EXTERNAL_SUPERVISOR_IP: + result = DataDictionarySetExternalSupervisorIPU32(newValue); + break; + case CONFIGURATION_PARAMETER_EXTERNAL_SUPERVISOR_PORT_TCP: + result = DataDictionarySetSupervisorTCPPortU16(newValue); + break; + case CONFIGURATION_PARAMETER_RVSS_CONFIG: + result = DataDictionarySetRVSSConfigU32((uint32_t)strtoul(newValue, NULL, 10)); + break; + case CONFIGURATION_PARAMETER_RVSS_RATE: + result = DataDictionarySetRVSSRateU8((uint8_t)strtoul(newValue, NULL, 10)); + break; + case CONFIGURATION_PARAMETER_MISC_DATA: + RCLCPP_WARN(get_logger(), "Unable to set MiscData - size unknown"); + result = DataDictionarySetMiscData(newValue, 0); + break; + case CONFIGURATION_PARAMETER_INVALID: + RCLCPP_WARN(get_logger(), "Attempted to set invalid parameter %s", parameterName); + default: + RCLCPP_ERROR(get_logger(), "No action is implemented for setting parameter %s"); } return result == WRITE_OK ? 0 : -1; } +I32 SystemControl::SystemControlReadServerParameterList(char* ParameterList, U8 Debug) { - -I32 SystemControl::SystemControlReadServerParameterList(char * ParameterList, U8 Debug) { - - char *line = NULL; + char* line = NULL; size_t len = 0; - FILE *fd; + FILE* fd; char confPathDir[MAX_FILE_PATH]; ssize_t read; - UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); + UtilGetConfDirectoryPath(confPathDir, sizeof(confPathDir)); strcat(confPathDir, CONF_FILE_NAME); fd = fopen(confPathDir, "r"); @@ -2013,47 +1979,45 @@ I32 SystemControl::SystemControlReadServerParameterList(char * ParameterList, U8 } fclose(fd); free(line); - } - else { + } else { RCLCPP_ERROR(get_logger(), "Unable to open file %s", confPathDir); } if (Debug) { - RCLCPP_WARN(get_logger(),"ParameterList = %s\n", ParameterList); + RCLCPP_WARN(get_logger(), "ParameterList = %s\n", ParameterList); } return strlen(ParameterList); } - -I32 SystemControl::SystemControlBuildFileContentInfo(const char * Path, U8 Debug) { +I32 SystemControl::SystemControlBuildFileContentInfo(const char* Path, U8 Debug) { struct stat st; char CompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); if (SystemControlDirectoryInfo.exist) return -1; - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + UtilGetTestDirectoryPath(CompletePath, sizeof(CompletePath)); strcat(CompletePath, Path); stat(CompletePath, &st); printf("CompletePath = %s\n", CompletePath); // Create mmap of the file and return the length SystemControlDirectoryInfo.fd = open(CompletePath, O_RDWR); - SystemControlDirectoryInfo.info_buffer = - (char*) mmap(NULL, st.st_size, PROT_READ | PROT_WRITE, MAP_PRIVATE, SystemControlDirectoryInfo.fd, 0); - SystemControlDirectoryInfo.size = st.st_size; + SystemControlDirectoryInfo.info_buffer = + (char*)mmap(NULL, st.st_size, PROT_READ | PROT_WRITE, MAP_PRIVATE, SystemControlDirectoryInfo.fd, 0); + SystemControlDirectoryInfo.size = st.st_size; SystemControlDirectoryInfo.exist = 1; return st.st_size; } -I32 SystemControl::SystemControlDestroyFileContentInfo(const char * Path, U8 RemoveFile) { +I32 SystemControl::SystemControlDestroyFileContentInfo(const char* Path, U8 RemoveFile) { char CompletePath[MAX_FILE_PATH]; struct stat st; - if (!SystemControlDirectoryInfo.exist){ + if (!SystemControlDirectoryInfo.exist) { return -1; } - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + UtilGetTestDirectoryPath(CompletePath, sizeof(CompletePath)); strcat(CompletePath, Path); munmap(SystemControlDirectoryInfo.info_buffer, SystemControlDirectoryInfo.size); close(SystemControlDirectoryInfo.fd); @@ -2064,14 +2028,14 @@ I32 SystemControl::SystemControlDestroyFileContentInfo(const char * Path, U8 Rem return 0; } -I32 SystemControl::SystemControlCheckFileDirectoryExist(char * ParameterName, char * ReturnValue, U8 Debug) { +I32 SystemControl::SystemControlCheckFileDirectoryExist(char* ParameterName, char* ReturnValue, U8 Debug) { - DIR *pDir; - FILE *fd; + DIR* pDir; + FILE* fd; char CompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + UtilGetTestDirectoryPath(CompletePath, sizeof(CompletePath)); strcat(CompletePath, ParameterName); *ReturnValue = PATH_INVALID_MISSING; @@ -2080,19 +2044,16 @@ I32 SystemControl::SystemControlCheckFileDirectoryExist(char * ParameterName, ch if (pDir == NULL) { fd = fopen(CompletePath, "r"); if (fd != NULL) { - *ReturnValue = FILE_EXIST; //File exist + *ReturnValue = FILE_EXIST; // File exist fclose(fd); } - } - else { - *ReturnValue = FOLDER_EXIST; //Directory exist + } else { + *ReturnValue = FOLDER_EXIST; // Directory exist closedir(pDir); } - if (Debug) - RCLCPP_WARN(get_logger(),"%d %s", *ReturnValue, CompletePath); - + RCLCPP_WARN(get_logger(), "%d %s", *ReturnValue, CompletePath); return 0; } @@ -2103,7 +2064,7 @@ I32 SystemControl::SystemControlCheckFileDirectoryExist(char * ParameterName, ch * \param nameLen Length of the name string * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE */ -char SystemControl::SystemControlDeleteTrajectory(const char * trajectoryName, const size_t nameLen) { +char SystemControl::SystemControlDeleteTrajectory(const char* trajectoryName, const size_t nameLen) { return UtilDeleteTrajectoryFile(trajectoryName, nameLen) ? FAILED_DELETE : SUCCEEDED_DELETE; } @@ -2113,7 +2074,7 @@ char SystemControl::SystemControlDeleteTrajectory(const char * trajectoryName, c * \param nameLen Length of the name string * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE */ -char SystemControl::SystemControlDeleteGeofence(const char * geofenceName, const size_t nameLen) { +char SystemControl::SystemControlDeleteGeofence(const char* geofenceName, const size_t nameLen) { return UtilDeleteGeofenceFile(geofenceName, nameLen) ? FAILED_DELETE : SUCCEEDED_DELETE; } @@ -2123,7 +2084,7 @@ char SystemControl::SystemControlDeleteGeofence(const char * geofenceName, const * \param nameLen Length of the name string * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE */ -char SystemControl::SystemControlDeleteGenericFile(const char * filePath, const size_t nameLen) { +char SystemControl::SystemControlDeleteGenericFile(const char* filePath, const size_t nameLen) { return UtilDeleteGenericFile(filePath, nameLen) ? FAILED_DELETE : SUCCEEDED_DELETE; } @@ -2160,14 +2121,14 @@ char SystemControl::SystemControlClearObjects(void) { return SUCCEEDED_DELETE; } -I32 SystemControl::SystemControlDeleteFileDirectory(const char * Path, char * ReturnValue, U8 Debug) { +I32 SystemControl::SystemControlDeleteFileDirectory(const char* Path, char* ReturnValue, U8 Debug) { - DIR *pDir; - FILE *fd; + DIR* pDir; + FILE* fd; char CompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + UtilGetTestDirectoryPath(CompletePath, sizeof(CompletePath)); strcat(CompletePath, Path); *ReturnValue = PATH_INVALID_MISSING; @@ -2176,24 +2137,20 @@ I32 SystemControl::SystemControlDeleteFileDirectory(const char * Path, char * Re if (pDir == NULL) { fd = fopen(CompletePath, "r"); if (fd == NULL) { - *ReturnValue = PATH_INVALID_MISSING; //Missing file - } - else { - if (0 == remove(CompletePath)) //Delete file + *ReturnValue = PATH_INVALID_MISSING; // Missing file + } else { + if (0 == remove(CompletePath)) // Delete file { *ReturnValue = SUCCEEDED_DELETE; - } - else { + } else { *ReturnValue = FAILED_DELETE; } } - } - else { - if (0 == remove(CompletePath)) //Delete directory + } else { + if (0 == remove(CompletePath)) // Delete directory { *ReturnValue = SUCCEEDED_DELETE; - } - else { + } else { *ReturnValue = FAILED_DELETE; } } @@ -2206,15 +2163,14 @@ I32 SystemControl::SystemControlDeleteFileDirectory(const char * Path, char * Re return 0; } +I32 SystemControl::SystemControlCreateDirectory(const char* Path, char* ReturnValue, U8 Debug) { -I32 SystemControl::SystemControlCreateDirectory(const char * Path, char * ReturnValue, U8 Debug) { - - DIR *pDir; - FILE *fd; + DIR* pDir; + FILE* fd; char CompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + UtilGetTestDirectoryPath(CompletePath, sizeof(CompletePath)); strcat(CompletePath, Path); *ReturnValue = PATH_INVALID_MISSING; @@ -2223,26 +2179,23 @@ I32 SystemControl::SystemControlCreateDirectory(const char * Path, char * Return if (pDir == NULL) { fd = fopen(CompletePath, "r"); if (fd != NULL) { - *ReturnValue = FILE_EXIST; //This is a file! + *ReturnValue = FILE_EXIST; // This is a file! fclose(fd); - } - else { - if (0 == mkdir(CompletePath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)) //Make the new directory + } else { + if (0 == mkdir(CompletePath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)) // Make the new directory { *ReturnValue = SUCCEDED_CREATE_FOLDER; - } - else { + } else { *ReturnValue = FAILED_CREATE_FOLDER; } } - } - else { - *ReturnValue = FOLDER_EXIST; //Directory exist + } else { + *ReturnValue = FOLDER_EXIST; // Directory exist closedir(pDir); } if (Debug) - RCLCPP_WARN(get_logger(),"%d %s", *(ReturnValue), CompletePath); + RCLCPP_WARN(get_logger(), "%d %s", *(ReturnValue), CompletePath); if (*ReturnValue == SUCCEDED_CREATE_FOLDER) RCLCPP_INFO(get_logger(), "Directory created: %s", CompletePath); @@ -2250,65 +2203,69 @@ I32 SystemControl::SystemControlCreateDirectory(const char * Path, char * Return return 0; } +I32 SystemControl::SystemControlUploadFile(const char* Filename, + const char* FileSize, + const char* PacketSize, + const char* FileType, + char* ReturnValue, + char* CompleteFilePath, + U8 Debug) { -I32 SystemControl::SystemControlUploadFile(const char * Filename, const char * FileSize, const char * PacketSize, const char * FileType, char * ReturnValue, - char * CompleteFilePath, U8 Debug) { - - FILE *fd; + FILE* fd; char CompletePath[MAX_FILE_PATH]; - memset(CompletePath, 0, sizeof (CompletePath)); - //GetCurrentDir(CompletePath, MAX_FILE_PATH); - //strcat(CompletePath, Filename); + memset(CompletePath, 0, sizeof(CompletePath)); + // GetCurrentDir(CompletePath, MAX_FILE_PATH); + // strcat(CompletePath, Filename); if (Filename == NULL || FileSize == NULL || PacketSize == NULL || FileType == NULL || ReturnValue == NULL) { RCLCPP_ERROR(get_logger(), "Invalid function parameter passed to upload file handler function"); return -1; } switch (atoi(FileType)) { - case ATOS_GENERIC_FILE_TYPE: - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - break; - case ATOS_TRAJ_FILE_TYPE: - UtilGetTrajDirectoryPath(CompletePath, sizeof (CompletePath)); - break; - case ATOS_CONF_FILE_TYPE: - UtilGetConfDirectoryPath(CompletePath, sizeof (CompletePath)); - break; - case ATOS_GEOFENCE_FILE_TYPE: - UtilGetGeofenceDirectoryPath(CompletePath, sizeof (CompletePath)); - break; - case ATOS_OBJECT_FILE_TYPE: - UtilGetObjectDirectoryPath(CompletePath, sizeof (CompletePath)); - break; - default: - RCLCPP_ERROR(get_logger(), "Received invalid file type upload request"); - //Create temporary file for handling data anyway - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, "/file.tmp"); - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - fclose(fd); - remove(CompletePath); //Remove file if exist - } - fd = fopen(CompletePath, "w+"); //Create the temporary file + case ATOS_GENERIC_FILE_TYPE: + UtilGetTestDirectoryPath(CompletePath, sizeof(CompletePath)); + break; + case ATOS_TRAJ_FILE_TYPE: + UtilGetTrajDirectoryPath(CompletePath, sizeof(CompletePath)); + break; + case ATOS_CONF_FILE_TYPE: + UtilGetConfDirectoryPath(CompletePath, sizeof(CompletePath)); + break; + case ATOS_GEOFENCE_FILE_TYPE: + UtilGetGeofenceDirectoryPath(CompletePath, sizeof(CompletePath)); + break; + case ATOS_OBJECT_FILE_TYPE: + UtilGetObjectDirectoryPath(CompletePath, sizeof(CompletePath)); + break; + default: + RCLCPP_ERROR(get_logger(), "Received invalid file type upload request"); + // Create temporary file for handling data anyway + UtilGetTestDirectoryPath(CompletePath, sizeof(CompletePath)); + strcat(CompletePath, "/file.tmp"); + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + fclose(fd); + remove(CompletePath); // Remove file if exist + } + fd = fopen(CompletePath, "w+"); // Create the temporary file - *ReturnValue = PATH_INVALID_MISSING; - return -1; + *ReturnValue = PATH_INVALID_MISSING; + return -1; } strcat(CompletePath, Filename); strcpy(CompleteFilePath, CompletePath); if (Debug) { - RCLCPP_WARN(get_logger(),"Filename: %s", Filename); - RCLCPP_WARN(get_logger(),"FileSize: %s", FileSize); - RCLCPP_WARN(get_logger(),"PacketSize: %s", PacketSize); - RCLCPP_WARN(get_logger(),"FileType: %s", FileType); - RCLCPP_WARN(get_logger(),"CompletePath: %s", CompletePath); - RCLCPP_WARN(get_logger(),"CompleteFilePath: %s", CompleteFilePath); + RCLCPP_WARN(get_logger(), "Filename: %s", Filename); + RCLCPP_WARN(get_logger(), "FileSize: %s", FileSize); + RCLCPP_WARN(get_logger(), "PacketSize: %s", PacketSize); + RCLCPP_WARN(get_logger(), "FileType: %s", FileType); + RCLCPP_WARN(get_logger(), "CompletePath: %s", CompletePath); + RCLCPP_WARN(get_logger(), "CompleteFilePath: %s", CompleteFilePath); } - if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) { //Check packet size + if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) { // Check packet size *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; return 0; } @@ -2316,26 +2273,25 @@ I32 SystemControl::SystemControlUploadFile(const char * Filename, const char * F fd = fopen(CompletePath, "r"); if (fd != NULL) { fclose(fd); - remove(CompletePath); //Remove file if exist + remove(CompletePath); // Remove file if exist RCLCPP_INFO(get_logger(), "Deleted existing file <%s>", CompletePath); } - fd = fopen(CompletePath, "w+"); //Create the file + fd = fopen(CompletePath, "w+"); // Create the file if (fd != NULL) { - *ReturnValue = SERVER_PREPARED; //Server prepared + *ReturnValue = SERVER_PREPARED; // Server prepared fclose(fd); return 0; - } - else { - //Failed to open path create temporary file - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + } else { + // Failed to open path create temporary file + UtilGetTestDirectoryPath(CompletePath, sizeof(CompletePath)); strcat(CompletePath, "/file.tmp"); fd = fopen(CompletePath, "r"); if (fd != NULL) { fclose(fd); - remove(CompletePath); //Remove file if exist + remove(CompletePath); // Remove file if exist } - fd = fopen(CompletePath, "w+"); //Create the temporary file + fd = fopen(CompletePath, "w+"); // Create the temporary file *ReturnValue = PATH_INVALID_MISSING; @@ -2345,35 +2301,35 @@ I32 SystemControl::SystemControlUploadFile(const char * Filename, const char * F return -1; } +I32 SystemControl::SystemControlReceiveRxData(I32* sockfd, + const char* Path, + const char* FileSize, + const char* PacketSize, + char* ReturnValue, + U8 Debug) { -I32 SystemControl::SystemControlReceiveRxData(I32 * sockfd, const char * Path, const char * FileSize, const char * PacketSize, char * ReturnValue, - U8 Debug) { - - FILE *fd; + FILE* fd; char CompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); - //UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + // UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, Path); - U32 FileSizeU32 = atoi(FileSize); + U32 FileSizeU32 = atoi(FileSize); U16 PacketSizeU16 = atoi(PacketSize); I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; char RxBuffer[SYSTEM_CONTROL_RX_PACKET_SIZE]; - U32 TotalRxCount = 0, TransmissionCount = (U32) (FileSizeU32 / PacketSizeU16), RestCount = - FileSizeU32 % PacketSizeU16; + U32 TotalRxCount = 0, TransmissionCount = (U32)(FileSizeU32 / PacketSizeU16), + RestCount = FileSizeU32 % PacketSizeU16; struct timeval CurTime; - if (Debug) { - RCLCPP_WARN(get_logger(),"Receive Rx data:"); - RCLCPP_WARN(get_logger(),"Path: %s", Path); - RCLCPP_WARN(get_logger(),"FileSize: %s", FileSize); - RCLCPP_WARN(get_logger(),"PacketSize: %s", PacketSize); - RCLCPP_WARN(get_logger(),"CompletePath: %s", CompletePath); + RCLCPP_WARN(get_logger(), "Receive Rx data:"); + RCLCPP_WARN(get_logger(), "Path: %s", Path); + RCLCPP_WARN(get_logger(), "FileSize: %s", FileSize); + RCLCPP_WARN(get_logger(), "PacketSize: %s", PacketSize); + RCLCPP_WARN(get_logger(), "CompletePath: %s", CompletePath); } - - fd = fopen(CompletePath, "w+"); if (fd != NULL) { @@ -2384,7 +2340,6 @@ I32 SystemControl::SystemControlReceiveRxData(I32 * sockfd, const char * Path, c gettimeofday(&CurTime, NULL); Time2 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; - if (i == TransmissionCount) { PacketSizeU16 = RestCount; } @@ -2397,8 +2352,8 @@ I32 SystemControl::SystemControlReceiveRxData(I32 * sockfd, const char * Path, c fwrite(RxBuffer, 1, ClientStatus, fd); fflush(fd); if (Debug) { - printf("%d, %d, %d, %d, %d, %d :", i, ClientStatus, TotalRxCount, TimeDiff, PacketSizeU16, - FileSizeU32); + printf( + "%d, %d, %d, %d, %d, %d :", i, ClientStatus, TotalRxCount, TimeDiff, PacketSizeU16, FileSizeU32); for (j = 0; j < 10; j++) printf("%x-", RxBuffer[j]); printf("...\n"); @@ -2408,7 +2363,6 @@ I32 SystemControl::SystemControlReceiveRxData(I32 * sockfd, const char * Path, c Time1 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; } - TimeDiff = abs(Time1 - Time2); } @@ -2416,35 +2370,35 @@ I32 SystemControl::SystemControlReceiveRxData(I32 * sockfd, const char * Path, c if (TotalRxCount == FileSizeU32) { *ReturnValue = FILE_UPLOADED; - } - else if (TotalRxCount > FileSizeU32) { + } else if (TotalRxCount > FileSizeU32) { *ReturnValue = FILE_TO_MUCH_DATA; remove(CompletePath); RCLCPP_INFO(get_logger(), "CORRUPT FILE, REMOVING..."); - } - else { + } else { *ReturnValue = TIME_OUT; remove(CompletePath); RCLCPP_INFO(get_logger(), "CORRUPT FILE, REMOVING..."); } RCLCPP_INFO(get_logger(), "Rec count = %d, Req count = %d", TotalRxCount, FileSizeU32); - } return 0; } - -I32 SystemControl::SystemControlSendFileContent(I32 * sockfd, const char * Path, const char * PacketSize, char * ReturnValue, U8 Remove, - U8 Debug) { - FILE *fd; +I32 SystemControl::SystemControlSendFileContent(I32* sockfd, + const char* Path, + const char* PacketSize, + char* ReturnValue, + U8 Remove, + U8 Debug) { + FILE* fd; char CompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + UtilGetTestDirectoryPath(CompletePath, sizeof(CompletePath)); strcat(CompletePath, Path); - U32 FileSizeU32 = 0; + U32 FileSizeU32 = 0; U16 PacketSizeU16 = atoi(PacketSize); I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; char TxBuffer[SYSTEM_CONTROL_TX_PACKET_SIZE]; @@ -2453,14 +2407,14 @@ I32 SystemControl::SystemControlSendFileContent(I32 * sockfd, const char * Path, struct stat st; stat(CompletePath, &st); - TransmissionCount = (U32) (st.st_size) / PacketSizeU16; - RestCount = (U32) (st.st_size) % PacketSizeU16; + TransmissionCount = (U32)(st.st_size) / PacketSizeU16; + RestCount = (U32)(st.st_size) % PacketSizeU16; if (Debug) { - RCLCPP_WARN(get_logger(),"Send file content:"); - RCLCPP_WARN(get_logger(),"%s", Path); - RCLCPP_WARN(get_logger(),"%s", PacketSize); - RCLCPP_WARN(get_logger(),"%s", CompletePath); + RCLCPP_WARN(get_logger(), "Send file content:"); + RCLCPP_WARN(get_logger(), "%s", Path); + RCLCPP_WARN(get_logger(), "%s", PacketSize); + RCLCPP_WARN(get_logger(), "%s", CompletePath); } fd = fopen(CompletePath, "r"); @@ -2470,15 +2424,15 @@ I32 SystemControl::SystemControlSendFileContent(I32 * sockfd, const char * Path, for (i = 0; i < TransmissionCount; i++) { bzero(TxBuffer, PacketSizeU16); fread(TxBuffer, 1, PacketSizeU16, fd); - //SystemControlSendBytes(TxBuffer, PacketSizeU16, sockfd, 0); //Send a packet - UtilSendTCPData((uint8_t*) "System Control", (uint8_t*) TxBuffer, PacketSizeU16, sockfd, 0); + // SystemControlSendBytes(TxBuffer, PacketSizeU16, sockfd, 0); //Send a packet + UtilSendTCPData((uint8_t*)"System Control", (uint8_t*)TxBuffer, PacketSizeU16, sockfd, 0); } if (RestCount > 0) { bzero(TxBuffer, PacketSizeU16); fread(TxBuffer, 1, RestCount, fd); - //SystemControlSendBytes(TxBuffer, RestCount, sockfd, 0); //Send the rest - UtilSendTCPData((uint8_t*) "System Control", (uint8_t*) TxBuffer, RestCount, sockfd, 0); + // SystemControlSendBytes(TxBuffer, RestCount, sockfd, 0); //Send the rest + UtilSendTCPData((uint8_t*)"System Control", (uint8_t*)TxBuffer, RestCount, sockfd, 0); } fclose(fd); @@ -2486,15 +2440,16 @@ I32 SystemControl::SystemControlSendFileContent(I32 * sockfd, const char * Path, if (Remove) remove(CompletePath); - RCLCPP_INFO(get_logger(), "Sent file: %s, total size = %d, transmissions = %d", CompletePath, - (PacketSizeU16 * TransmissionCount + RestCount), i + 1); - + RCLCPP_INFO(get_logger(), + "Sent file: %s, total size = %d, transmissions = %d", + CompletePath, + (PacketSizeU16 * TransmissionCount + RestCount), + i + 1); } return 0; } - /*! * \brief SystemControlUpdateRVSSSendTime Adds a time interval onto the specified time struct in accordance * with the rate parameter @@ -2504,11 +2459,11 @@ I32 SystemControl::SystemControlSendFileContent(I32 * sockfd, const char * Path, * \param RVSSRate_Hz Rate at which RVSS messages are to be sent - if this parameter is 0 the value * is clamped to 1 Hz */ -void SystemControl::SystemControlUpdateRVSSSendTime(struct timeval *currentRVSSSendTime, uint8_t RVSSRate_Hz) { +void SystemControl::SystemControlUpdateRVSSSendTime(struct timeval* currentRVSSSendTime, uint8_t RVSSRate_Hz) { struct timeval RVSSTimeInterval, timeDiff, currentTime; - RVSSRate_Hz = RVSSRate_Hz == 0 ? 1 : RVSSRate_Hz; // Minimum frequency 1 Hz - RVSSTimeInterval.tv_sec = (long)(1.0 / RVSSRate_Hz); + RVSSRate_Hz = RVSSRate_Hz == 0 ? 1 : RVSSRate_Hz; // Minimum frequency 1 Hz + RVSSTimeInterval.tv_sec = (long)(1.0 / RVSSRate_Hz); RVSSTimeInterval.tv_usec = (long)((1.0 / RVSSRate_Hz - RVSSTimeInterval.tv_sec) * 1000000.0); // If there is a large difference between the current time and the time at which RVSS was sent, update based @@ -2517,13 +2472,11 @@ void SystemControl::SystemControlUpdateRVSSSendTime(struct timeval *currentRVSSS timersub(¤tTime, currentRVSSSendTime, &timeDiff); if (timercmp(&timeDiff, &RVSSTimeInterval, <)) { timeradd(currentRVSSSendTime, &RVSSTimeInterval, currentRVSSSendTime); - } - else { + } else { timeradd(¤tTime, &RVSSTimeInterval, currentRVSSSendTime); } } - /* SystemControlBuildRVSSTimeChannelMessage builds a message from data in *GPSTime. The message is stored in *RVSSData. See the architecture document for the protocol of RVSS. @@ -2533,8 +2486,7 @@ See the architecture document for the protocol of RVSS. - *GPSTime current time data - Debug enable(1)/disable(0) debug printouts (Not used) */ -I32 SystemControl::SystemControlBuildRVSSTimeChannelMessage(char * RVSSData, U32 * RVSSDataLengthU32, - U8 Debug) { +I32 SystemControl::SystemControlBuildRVSSTimeChannelMessage(char* RVSSData, U32* RVSSDataLengthU32, U8 Debug) { struct timeval systemTime; time_t ttime; struct tm* ttm; @@ -2543,56 +2495,52 @@ I32 SystemControl::SystemControlBuildRVSSTimeChannelMessage(char * RVSSData, U32 ttime = systemTime.tv_sec; ttm = localtime(&ttime); - strftime(tmpbuf, sizeof (tmpbuf), "%Y-%m-%d %H:%M:%S", ttm); + strftime(tmpbuf, sizeof(tmpbuf), "%Y-%m-%d %H:%M:%S", ttm); snprintf(RVSSData, SYSTEM_CONTROL_RVSS_DATA_BUFFER, "%s.%06ld", tmpbuf, systemTime.tv_usec); if (Debug) { - RCLCPP_WARN(get_logger(),RVSSData); + RCLCPP_WARN(get_logger(), RVSSData); } *RVSSDataLengthU32 = strlen(RVSSData); return 0; } - /* -SystemControlBuildRVSSATOSChannelMessage builds a message from OBCState in DataDictionary and SysCtrlState. The message is stored in *RVSSData. -See the architecture document for the protocol of RVSS. +SystemControlBuildRVSSATOSChannelMessage builds a message from OBCState in DataDictionary and SysCtrlState. The message +is stored in *RVSSData. See the architecture document for the protocol of RVSS. - *RVSSData the buffer the message - *RVSSDataLengthU32 the length of the message - U8 SysCtrlState the SystemControl state (SystemControlState) - Debug enable(1)/disable(0) debug printouts (Not used) */ -I32 SystemControl::SystemControlBuildRVSSATOSChannelMessage(char * RVSSData, U32 * RVSSDataLengthU32, - U8 SysCtrlState, U8 Debug) { +I32 SystemControl::SystemControlBuildRVSSATOSChannelMessage(char* RVSSData, + U32* RVSSDataLengthU32, + U8 SysCtrlState, + U8 Debug) { I32 MessageIndex = 0, i; - char *p; - + char* p; RVSSATOSType RVSSATOSData; OBCState_t obcState; - RVSSATOSData.MessageLengthU32 = SwapU32((U32) sizeof (RVSSATOSType) - 4); - RVSSATOSData.ChannelCodeU32 = SwapU32((U32) RVSS_ATOS_CHANNEL); + RVSSATOSData.MessageLengthU32 = SwapU32((U32)sizeof(RVSSATOSType) - 4); + RVSSATOSData.ChannelCodeU32 = SwapU32((U32)RVSS_ATOS_CHANNEL); DataDictionaryGetOBCState(&obcState); - RVSSATOSData.OBCStateU8 = (uint8_t) (obcState); + RVSSATOSData.OBCStateU8 = (uint8_t)(obcState); RVSSATOSData.SysCtrlStateU8 = SysCtrlState; - p = (char *) & RVSSATOSData; - for (i = 0; i < sizeof (RVSSATOSType); i++) + p = (char*)&RVSSATOSData; + for (i = 0; i < sizeof(RVSSATOSType); i++) *(RVSSData + i) = *p++; *RVSSDataLengthU32 = i; - if (Debug) { - - } + if (Debug) {} return 0; } - - #define MAX_MONR_STRING_LENGTH 1024 /*! * \brief SystemControlSendRVSSMONRChannelMessages Sends RVSS monitoring data messages based on objects present @@ -2601,24 +2549,23 @@ I32 SystemControl::SystemControlBuildRVSSATOSChannelMessage(char * RVSSData, U32 * \param addr Address struct pointer for RVSS socket * \return 0 on success, -1 otherwise */ -int32_t SystemControl::SystemControlSendRVSSMonitorChannelMessages(int *socket, struct sockaddr_in *addr) { +int32_t SystemControl::SystemControlSendRVSSMonitorChannelMessages(int* socket, struct sockaddr_in* addr) { uint32_t messageLength = 0; - uint32_t RVSSChannel = RVSS_MONITOR_CHANNEL; + uint32_t RVSSChannel = RVSS_MONITOR_CHANNEL; char RVSSData[MAX_MONR_STRING_LENGTH]; - char *monitorDataString = RVSSData + sizeof (messageLength) + sizeof (RVSSChannel); - uint32_t *transmitterIDs = NULL; + char* monitorDataString = RVSSData + sizeof(messageLength) + sizeof(RVSSChannel); + uint32_t* transmitterIDs = NULL; uint32_t numberOfObjects; ObjectDataType monitorData; // Get number of objects present in shared memory if (DataDictionaryGetNumberOfObjects(&numberOfObjects) != READ_OK) { - RCLCPP_ERROR(get_logger(), - "Data dictionary number of objects read error - RVSS messages cannot be sent"); + RCLCPP_ERROR(get_logger(), "Data dictionary number of objects read error - RVSS messages cannot be sent"); return -1; } // Allocate an array for objects' transmitter IDs - transmitterIDs = (uint32_t*) malloc(numberOfObjects * sizeof (uint32_t)); + transmitterIDs = (uint32_t*)malloc(numberOfObjects * sizeof(uint32_t)); if (transmitterIDs == NULL) { RCLCPP_ERROR(get_logger(), "Memory allocation error - RVSS messages cannot be sent"); return -1; @@ -2627,8 +2574,7 @@ int32_t SystemControl::SystemControlSendRVSSMonitorChannelMessages(int *socket, // Get transmitter IDs for all connected objects if (DataDictionaryGetObjectTransmitterIDs(transmitterIDs, numberOfObjects) != READ_OK) { free(transmitterIDs); - RCLCPP_ERROR(get_logger(), - "Data dictionary transmitter ID read error - RVSS messages cannot be sent"); + RCLCPP_ERROR(get_logger(), "Data dictionary transmitter ID read error - RVSS messages cannot be sent"); return -1; } @@ -2642,45 +2588,43 @@ int32_t SystemControl::SystemControlSendRVSSMonitorChannelMessages(int *socket, struct timeval recvTime; if (DataDictionaryGetMonitorDataReceiveTime(transmitterIDs[i], &recvTime) != READ_OK) { - RCLCPP_ERROR(get_logger(), - "Data dictionary monitor data read error for transmitter ID %u - RVSS message cannot be sent", - transmitterIDs[i]); + RCLCPP_ERROR( + get_logger(), + "Data dictionary monitor data read error for transmitter ID %u - RVSS message cannot be sent", + transmitterIDs[i]); retval = -1; - } - else { + } else { if (recvTime.tv_sec == 0 && recvTime.tv_usec == 0) { continue; } } if (DataDictionaryGetObjectIPByTransmitterID(transmitterIDs[i], &monitorData.ClientIP) != READ_OK) { - RCLCPP_ERROR(get_logger(), - "Data dictionary monitor data read error for transmitter ID %u - RVSS message cannot be sent", - transmitterIDs[i]); + RCLCPP_ERROR( + get_logger(), + "Data dictionary monitor data read error for transmitter ID %u - RVSS message cannot be sent", + transmitterIDs[i]); retval = -1; - } - else if (DataDictionaryGetMonitorData(transmitterIDs[i], &monitorData.MonrData) != READ_OK) { - RCLCPP_ERROR(get_logger(), - "Data dictionary monitor data read error for transmitter ID %u - RVSS message cannot be sent", - transmitterIDs[i]); + } else if (DataDictionaryGetMonitorData(transmitterIDs[i], &monitorData.MonrData) != READ_OK) { + RCLCPP_ERROR( + get_logger(), + "Data dictionary monitor data read error for transmitter ID %u - RVSS message cannot be sent", + transmitterIDs[i]); retval = -1; - } - else if (UtilObjectDataToString(monitorData, monitorDataString, - sizeof (RVSSData) - (size_t)(monitorDataString - RVSSData)) == - -1) { + } else if (UtilObjectDataToString(monitorData, + monitorDataString, + sizeof(RVSSData) - (size_t)(monitorDataString - RVSSData)) == -1) { RCLCPP_ERROR(get_logger(), "Error building object data string"); retval = -1; - } - else { + } else { RCLCPP_DEBUG(get_logger(), "%s: Transmitter ID %u", __FUNCTION__, transmitterIDs[i]); - messageLength = - (uint32_t) (strlen(monitorDataString) + sizeof (messageLength) + sizeof (RVSSChannel)); + messageLength = (uint32_t)(strlen(monitorDataString) + sizeof(messageLength) + sizeof(RVSSChannel)); messageLength = htole32(messageLength); - RVSSChannel = htole32(RVSSChannel); - memcpy(RVSSData, &messageLength, sizeof (messageLength)); - memcpy(RVSSData + sizeof (messageLength), &RVSSChannel, sizeof (RVSSChannel)); + RVSSChannel = htole32(RVSSChannel); + memcpy(RVSSData, &messageLength, sizeof(messageLength)); + memcpy(RVSSData + sizeof(messageLength), &RVSSChannel, sizeof(RVSSChannel)); - UtilSendUDPData((uint8_t*) module_name.c_str(), socket, addr, (uint8_t*) RVSSData, messageLength, 0); + UtilSendUDPData((uint8_t*)module_name.c_str(), socket, addr, (uint8_t*)RVSSData, messageLength, 0); } } } @@ -2688,7 +2632,6 @@ int32_t SystemControl::SystemControlSendRVSSMonitorChannelMessages(int *socket, return retval; } - /* SystemControlBuildRVSSAspChannelMessage shall be used for sending ASP-debug data. The message is stored in *RVSSData. See the architecture document for the protocol of RVSS. @@ -2699,16 +2642,13 @@ See the architecture document for the protocol of RVSS. - Debug enable(1)/disable(0) debug printouts (Not used) */ -I32 SystemControl::SystemControlBuildRVSSAspChannelMessage(char * RVSSData, U32 * RVSSDataLengthU32, U8 Debug) { +I32 SystemControl::SystemControlBuildRVSSAspChannelMessage(char* RVSSData, U32* RVSSDataLengthU32, U8 Debug) { RVSSTimeType RVSSTimeData; - - - return 0; } -void SystemControl::appendSysInfoString(char *ControlResponseBuffer, const size_t bufferSize) { +void SystemControl::appendSysInfoString(char* ControlResponseBuffer, const size_t bufferSize) { size_t remainingBufferSpace = bufferSize; struct sysinfo info; @@ -2718,27 +2658,25 @@ void SystemControl::appendSysInfoString(char *ControlResponseBuffer, const size_ // Server uptime sysinfo(&info); - hours = info.uptime / 3600; + hours = info.uptime / 3600; minutes = (info.uptime - (3600 * hours)) / 60; seconds = (info.uptime - (3600 * hours) - (minutes * 60)); - snprintf(stringBuffer, sizeof (stringBuffer), "Machine uptime: %ld:%ld:%ld\n", hours, minutes, seconds); + snprintf(stringBuffer, sizeof(stringBuffer), "Machine uptime: %ld:%ld:%ld\n", hours, minutes, seconds); strncat(ControlResponseBuffer, stringBuffer, remainingBufferSpace - 1); remainingBufferSpace -= strlen(stringBuffer); unsigned long startTime = UtilGetPIDUptime(getpid()).tv_sec; unsigned long long timeAtStart = startTime / (unsigned long long)(sysconf(_SC_CLK_TCK)); - long serverUptime = (long)((unsigned long long)info.uptime - timeAtStart); + long serverUptime = (long)((unsigned long long)info.uptime - timeAtStart); - - hours = serverUptime / 3600; + hours = serverUptime / 3600; minutes = (serverUptime - (3600 * hours)) / 60; seconds = (serverUptime - (3600 * hours) - (minutes * 60)); sprintf(stringBuffer, "Server uptime: %ld:%ld:%ld\n", hours, minutes, seconds); strncat(ControlResponseBuffer, stringBuffer, remainingBufferSpace - 1); remainingBufferSpace -= strlen(stringBuffer); - //Make it clear that this is placeholder data + // Make it clear that this is placeholder data strncat(ControlResponseBuffer, "ATOS powerlevel: 90001\n", remainingBufferSpace - 1); } - diff --git a/atos/modules/TrajectoryletStreamer/inc/trajectoryletstreamer.hpp b/atos/modules/TrajectoryletStreamer/inc/trajectoryletstreamer.hpp index c60da4c4e..3e2cfc281 100644 --- a/atos/modules/TrajectoryletStreamer/inc/trajectoryletstreamer.hpp +++ b/atos/modules/TrajectoryletStreamer/inc/trajectoryletstreamer.hpp @@ -3,10 +3,6 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include -#include -#include -#include #include "atos_interfaces/srv/get_object_ids.hpp" #include "atos_interfaces/srv/get_object_trajectory.hpp" #include "module.hpp" @@ -14,14 +10,18 @@ #include "roschannels/commandchannels.hpp" #include "trajectory.hpp" #include "trajectorypublisher.hpp" +#include +#include +#include +#include namespace ATOS { class TrajectoryletStreamer : public Module { - public: +public: TrajectoryletStreamer(); - private: +private: static inline std::string const moduleName = "trajectorylet_streamer"; void onInitMessage(const ROSChannels::Init::message_type::SharedPtr); void onObjectsConnectedMessage(const ROSChannels::ObjectsConnected::message_type::SharedPtr); @@ -36,9 +36,9 @@ class TrajectoryletStreamer : public Module { ROSChannels::Abort::Sub abortSub; ROSChannels::Stop::Sub stopSub; - rclcpp::Client::SharedPtr idClient; //!< Client to request object ids + rclcpp::Client::SharedPtr idClient; //!< Client to request object ids rclcpp::Client::SharedPtr - trajectoryClient; //!< Client to request object trajectories + trajectoryClient; //!< Client to request object trajectories std::vector> publishers; @@ -47,4 +47,4 @@ class TrajectoryletStreamer : public Module { std::chrono::milliseconds chunkLength; }; -} // namespace ATOS +} // namespace ATOS diff --git a/atos/modules/TrajectoryletStreamer/inc/trajectorypublisher.hpp b/atos/modules/TrajectoryletStreamer/inc/trajectorypublisher.hpp index 981c5c8c7..127c44a99 100644 --- a/atos/modules/TrajectoryletStreamer/inc/trajectorypublisher.hpp +++ b/atos/modules/TrajectoryletStreamer/inc/trajectorypublisher.hpp @@ -4,12 +4,12 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "objectconfig.hpp" -#include "roschannels/pathchannel.hpp" #include "roschannels/commandchannels.hpp" +#include "roschannels/pathchannel.hpp" +#include #include -#include #include -#include +#include #include namespace ATOS { @@ -17,24 +17,25 @@ namespace ATOS { class TrajectoryPublisher { private: typedef std::pair::const_iterator, - std::vector::const_iterator> Chunk; + std::vector::const_iterator> + Chunk; + public: - TrajectoryPublisher( - rclcpp::Node& node, - const Trajectory&, - const uint32_t objectId, - const std::chrono::milliseconds chunkLength - = std::chrono::milliseconds(0)); + TrajectoryPublisher(rclcpp::Node& node, + const Trajectory&, + const uint32_t objectId, + const std::chrono::milliseconds chunkLength = std::chrono::milliseconds(0)); void setChunkLength(std::chrono::milliseconds chunkLength) { this->chunkLength = chunkLength; } + private: ROSChannels::Path::Pub pub; - ROSChannels::StartObject::Sub startObjectSub; + ROSChannels::StartObject::Sub startObjectSub; std::shared_ptr timer; - std::chrono::milliseconds chunkLength = std::chrono::milliseconds(0); + std::chrono::milliseconds chunkLength = std::chrono::milliseconds(0); std::chrono::milliseconds publishPeriod = std::chrono::milliseconds(50); std::unique_ptr startTime; std::chrono::steady_clock::time_point lastPublishTime; @@ -46,7 +47,6 @@ class TrajectoryPublisher { void getStartObjectMsg(const atos_interfaces::msg::ObjectTriggerStart::SharedPtr msg); nav_msgs::msg::Path chunkToPath(Chunk chunk, std::chrono::steady_clock::time_point); Chunk extractChunk(std::chrono::steady_clock::duration beginTime, std::chrono::steady_clock::duration endTime); - }; -} // namespace ATOS +} // namespace ATOS diff --git a/atos/modules/TrajectoryletStreamer/src/main.cpp b/atos/modules/TrajectoryletStreamer/src/main.cpp index 722155879..8f980093b 100644 --- a/atos/modules/TrajectoryletStreamer/src/main.cpp +++ b/atos/modules/TrajectoryletStreamer/src/main.cpp @@ -3,11 +3,11 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -#include #include "trajectoryletstreamer.hpp" +#include -int main(int argc, char **argv) { - rclcpp::init(argc,argv); +int main(int argc, char** argv) { + rclcpp::init(argc, argv); auto ts = std::make_shared(); rclcpp::spin(ts); rclcpp::shutdown(); diff --git a/atos/modules/TrajectoryletStreamer/src/trajectoryletstreamer.cpp b/atos/modules/TrajectoryletStreamer/src/trajectoryletstreamer.cpp index a295e5928..f070c3dcf 100644 --- a/atos/modules/TrajectoryletStreamer/src/trajectoryletstreamer.cpp +++ b/atos/modules/TrajectoryletStreamer/src/trajectoryletstreamer.cpp @@ -4,23 +4,22 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "trajectoryletstreamer.hpp" -#include #include "atos_interfaces/srv/get_object_ids.hpp" +#include using namespace ATOS; using namespace ROSChannels; using std::placeholders::_1; -TrajectoryletStreamer::TrajectoryletStreamer() - : Module(TrajectoryletStreamer::moduleName), - initSub(*this, std::bind(&TrajectoryletStreamer::onInitMessage, this, _1)), - connectedSub(*this, std::bind(&TrajectoryletStreamer::onObjectsConnectedMessage, this, _1)), - abortSub(*this, std::bind(&TrajectoryletStreamer::onAbortMessage, this, _1)), - stopSub(*this, std::bind(&TrajectoryletStreamer::onStopMessage, this, _1)) { +TrajectoryletStreamer::TrajectoryletStreamer() : + Module(TrajectoryletStreamer::moduleName), + initSub(*this, std::bind(&TrajectoryletStreamer::onInitMessage, this, _1)), + connectedSub(*this, std::bind(&TrajectoryletStreamer::onObjectsConnectedMessage, this, _1)), + abortSub(*this, std::bind(&TrajectoryletStreamer::onAbortMessage, this, _1)), + stopSub(*this, std::bind(&TrajectoryletStreamer::onStopMessage, this, _1)) { declare_parameter("chunk_duration", 0.0); - idClient = create_client(ServiceNames::getObjectIds); - trajectoryClient - = create_client(ServiceNames::getObjectTrajectory); + idClient = create_client(ServiceNames::getObjectIds); + trajectoryClient = create_client(ServiceNames::getObjectTrajectory); } void TrajectoryletStreamer::onInitMessage(const std_msgs::msg::Empty::SharedPtr) { @@ -36,10 +35,9 @@ void TrajectoryletStreamer::onObjectsConnectedMessage(const ObjectsConnected::me } } - void TrajectoryletStreamer::loadObjectFiles() { clearScenario(); - double res = 0.0; + double res = 0.0; auto success = get_parameter("chunk_duration", res); if (!success) { RCLCPP_ERROR(get_logger(), "Could not get parameter chunk_duration"); @@ -53,22 +51,23 @@ void TrajectoryletStreamer::loadObjectFiles() { auto idResponse = future.get(); for (const auto id : idResponse->ids) { auto trajectoryCallback = - [this](const rclcpp::Client::SharedFuture future) { - RCLCPP_INFO(get_logger(), "Got trajectory"); - auto trajResponse = future.get(); - if (!trajResponse->success) { - RCLCPP_ERROR(get_logger(), "Get trajectory service call failed for object %u", - trajResponse->id); - return; - } - ATOS::Trajectory traj(get_logger()); - traj.initializeFromCartesianTrajectory(trajResponse->trajectory); - trajectories[trajResponse->id] = std::make_unique(traj); - RCLCPP_INFO(get_logger(), "Loaded trajectory for object %u with %ld points", - trajResponse->id, trajectories[trajResponse->id]->size()); - }; + [this](const rclcpp::Client::SharedFuture future) { + RCLCPP_INFO(get_logger(), "Got trajectory"); + auto trajResponse = future.get(); + if (!trajResponse->success) { + RCLCPP_ERROR(get_logger(), "Get trajectory service call failed for object %u", trajResponse->id); + return; + } + ATOS::Trajectory traj(get_logger()); + traj.initializeFromCartesianTrajectory(trajResponse->trajectory); + trajectories[trajResponse->id] = std::make_unique(traj); + RCLCPP_INFO(get_logger(), + "Loaded trajectory for object %u with %ld points", + trajResponse->id, + trajectories[trajResponse->id]->size()); + }; auto trajRequest = std::make_shared(); - trajRequest->id = id; + trajRequest->id = id; trajectoryClient->async_send_request(trajRequest, trajectoryCallback); } }; @@ -87,4 +86,4 @@ void TrajectoryletStreamer::onAbortMessage(const ROSChannels::Abort::message_typ void TrajectoryletStreamer::onStopMessage(const ROSChannels::Stop::message_type::SharedPtr) { // TODO -} \ No newline at end of file +} diff --git a/atos/modules/TrajectoryletStreamer/src/trajectorypublisher.cpp b/atos/modules/TrajectoryletStreamer/src/trajectorypublisher.cpp index f56f2a71e..498e3e733 100644 --- a/atos/modules/TrajectoryletStreamer/src/trajectorypublisher.cpp +++ b/atos/modules/TrajectoryletStreamer/src/trajectorypublisher.cpp @@ -4,47 +4,43 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "trajectorypublisher.hpp" +#include #include #include -#include using namespace ATOS; using namespace std::chrono_literals; -TrajectoryPublisher::TrajectoryPublisher( - rclcpp::Node& node, - const Trajectory& _traj, - const uint32_t objectId, - const std::chrono::milliseconds chunkLength) - : pub(node, objectId), - startObjectSub(node, std::bind(&TrajectoryPublisher::getStartObjectMsg, this, std::placeholders::_1)), - chunkLength(chunkLength), - traj(std::make_unique(_traj)), - lastPublishedChunk(traj->points.end(), traj->points.end()) -{ +TrajectoryPublisher::TrajectoryPublisher(rclcpp::Node& node, + const Trajectory& _traj, + const uint32_t objectId, + const std::chrono::milliseconds chunkLength) : + pub(node, objectId), + startObjectSub(node, std::bind(&TrajectoryPublisher::getStartObjectMsg, this, std::placeholders::_1)), + chunkLength(chunkLength), + traj(std::make_unique(_traj)), + lastPublishedChunk(traj->points.end(), traj->points.end()) { timer = node.create_wall_timer(publishPeriod, std::bind(&TrajectoryPublisher::publishChunk, this)); } -void TrajectoryPublisher::publishChunk() -{ +void TrajectoryPublisher::publishChunk() { using std::chrono::steady_clock; Chunk chunk(traj->points.cbegin(), traj->points.cend()); - auto now = steady_clock::now(); + auto now = steady_clock::now(); auto trajTimeOffset = now; - auto timeIntoTraj = steady_clock::duration(0); + auto timeIntoTraj = steady_clock::duration(0); if (startTime) { - timeIntoTraj = now-*startTime; + timeIntoTraj = now - *startTime; trajTimeOffset = *startTime; } if (chunkLength.count() > 0) { chunk = extractChunk(timeIntoTraj, timeIntoTraj + chunkLength); } - if (chunk != lastPublishedChunk || - (chunkLength == 0ms && now - lastPublishTime > 1s)) { + if (chunk != lastPublishedChunk || (chunkLength == 0ms && now - lastPublishTime > 1s)) { auto trajMsg = chunkToPath(chunk, trajTimeOffset); pub.publish(trajMsg); lastPublishedChunk = chunk; - lastPublishTime = now; + lastPublishTime = now; } } @@ -55,51 +51,45 @@ void TrajectoryPublisher::getStartObjectMsg(const atos_interfaces::msg::ObjectTr } } - -TrajectoryPublisher::Chunk TrajectoryPublisher::extractChunk( - std::chrono::steady_clock::duration beginTime, - std::chrono::steady_clock::duration endTime) -{ +TrajectoryPublisher::Chunk TrajectoryPublisher::extractChunk(std::chrono::steady_clock::duration beginTime, + std::chrono::steady_clock::duration endTime) { using std::chrono::steady_clock; auto ret = Chunk(traj->points.cbegin(), traj->points.cend()); - ret.first = std::upper_bound(traj->points.cbegin(), traj->points.cend(), beginTime, - [](const steady_clock::duration& dur, const Trajectory::TrajectoryPoint& pt) { - return pt.getTime() > dur; - }); + ret.first = std::upper_bound( + traj->points.cbegin(), + traj->points.cend(), + beginTime, + [](const steady_clock::duration& dur, const Trajectory::TrajectoryPoint& pt) { return pt.getTime() > dur; }); if (ret.first == traj->points.cend()) { return ret; } - ret.second = std::upper_bound(ret.first, traj->points.cend(), endTime, - [](const steady_clock::duration& dur, const Trajectory::TrajectoryPoint& pt) { - return pt.getTime() > dur; - } - ); + ret.second = std::upper_bound( + ret.first, + traj->points.cend(), + endTime, + [](const steady_clock::duration& dur, const Trajectory::TrajectoryPoint& pt) { return pt.getTime() > dur; }); return ret; } -nav_msgs::msg::Path TrajectoryPublisher::chunkToPath( - Chunk chunk, - std::chrono::steady_clock::time_point relTimeOffset) -{ +nav_msgs::msg::Path TrajectoryPublisher::chunkToPath(Chunk chunk, std::chrono::steady_clock::time_point relTimeOffset) { nav_msgs::msg::Path ret; ret.header.frame_id = "map"; - auto rosTimeOffset = rclcpp::Time(relTimeOffset.time_since_epoch().count()); - ret.header.stamp = rosTimeOffset; + auto rosTimeOffset = rclcpp::Time(relTimeOffset.time_since_epoch().count()); + ret.header.stamp = rosTimeOffset; - std::transform(chunk.first, chunk.second, std::back_inserter(ret.poses), - [&](const Trajectory::TrajectoryPoint& pt){ - geometry_msgs::msg::PoseStamped pose; - pose.header.stamp = rosTimeOffset + rclcpp::Duration(pt.getTime()); - pose.pose.position.x = pt.getXCoord(); - pose.pose.position.y = pt.getYCoord(); - pose.pose.position.z = pt.getZCoord(); - tf2::Quaternion q; - q.setRPY(0, 0, pt.getHeading()); - tf2::convert(q, pose.pose.orientation); - return pose; - } - ); + std::transform( + chunk.first, chunk.second, std::back_inserter(ret.poses), [&](const Trajectory::TrajectoryPoint& pt) { + geometry_msgs::msg::PoseStamped pose; + pose.header.stamp = rosTimeOffset + rclcpp::Duration(pt.getTime()); + pose.pose.position.x = pt.getXCoord(); + pose.pose.position.y = pt.getYCoord(); + pose.pose.position.z = pt.getZCoord(); + tf2::Quaternion q; + q.setRPY(0, 0, pt.getHeading()); + tf2::convert(q, pose.pose.orientation); + return pose; + }); // Force same coordinate frame as header for (auto& pose : ret.poses) { pose.header.frame_id = ret.header.frame_id; diff --git a/atos/modules/conftest.py b/atos/modules/conftest.py index 23ddc3845..9f08f603a 100644 --- a/atos/modules/conftest.py +++ b/atos/modules/conftest.py @@ -1,4 +1,5 @@ from pathlib import Path + import pytest diff --git a/atos_gui/atos_gui/configpanel/configpanel.py b/atos_gui/atos_gui/configpanel/configpanel.py index 897564e51..bd86a4623 100644 --- a/atos_gui/atos_gui/configpanel/configpanel.py +++ b/atos_gui/atos_gui/configpanel/configpanel.py @@ -3,101 +3,238 @@ file, You can obtain one at https://mozilla.org/MPL/2.0/. """ -import threading +import json import os +import threading import time -import json -from rcl_interfaces.msg import ParameterType, Parameter -from rcl_interfaces.srv import SetParametersAtomically, GetParameters, ListParameters -from rclpy.node import Node +from nicegui import ui +from rcl_interfaces.msg import Parameter, ParameterType +from rcl_interfaces.srv import GetParameters, ListParameters, SetParametersAtomically from rclpy.client import Client +from rclpy.node import Node -from nicegui import ui from .local_file_picker import local_file_picker -CONF_PATH = os.path.join(os.path.expanduser('~'), ".astazero/ATOS/conf") +CONF_PATH = os.path.join(os.path.expanduser("~"), ".astazero/ATOS/conf") MAX_TIMEOUT = 1 + class ConfigPanelNode(Node): - """ This node is responsible for rendering the config panel and setting parameters in ATOS nodes. - """ + """This node is responsible for rendering the config panel and setting parameters in ATOS nodes.""" def __init__(self) -> None: - """ Initializes the node, fetches all ros parameters and renders the config panel with these values. - """ - super().__init__('config_panel') + """Initializes the node, fetches all ros parameters and renders the config panel with these values.""" + super().__init__("config_panel") self.json_schema = self.load_json_schema("atos-param-schema.json") modules = self.json_schema["modules"].keys() - time.sleep(0.5) # Allow time for this node to initialize before discovering other nodes, otherwise it will fail to find the others. + time.sleep( + 0.5 + ) # Allow time for this node to initialize before discovering other nodes, otherwise it will fail to find the others. self.active_nodes_and_namespaces = self.get_node_names_and_namespaces() - self.active_node_list = [node for node, namespace in self.active_nodes_and_namespaces if node in modules and "atos" in namespace] - self.active_node_list = list(set(self.active_node_list)) # Removes duplicates + self.active_node_list = [ + node + for node, namespace in self.active_nodes_and_namespaces + if node in modules and "atos" in namespace + ] + self.active_node_list = list(set(self.active_node_list)) # Removes duplicates self.parameter_clients = self.init_parameter_clients(self.active_node_list) self.parameters = {} - threading.Thread(target=self.fetch_parameters_list, args=(self.parameter_clients,)).start() + threading.Thread( + target=self.fetch_parameters_list, args=(self.parameter_clients,) + ).start() - @ui.page(path='/config', title="ATOS Config Panel") + @ui.page(path="/config", title="ATOS Config Panel") def render_configpanel() -> None: - with ui.splitter(value=30).classes('w-1/2') as self.splitter: + with ui.splitter(value=30).classes("w-1/2") as self.splitter: with self.splitter.before: - with ui.tabs().props('vertical').classes('w-fit') as tabs: - ui.tab('Home', icon='🏠') + with ui.tabs().props("vertical").classes("w-fit") as tabs: + ui.tab("Home", icon="🏠") for node in self.active_node_list: ui.tab(node.replace("_", " ")) with self.splitter.after: - with ui.tab_panels(tabs, value='Home'): - with ui.tab_panel('Home'): - ui.label('Welcome to ATOS config panel!').classes('text-h4') + with ui.tab_panels(tabs, value="Home"): + with ui.tab_panel("Home"): + ui.label("Welcome to ATOS config panel!").classes("text-h4") if self.active_node_list: - ui.label("Select a node to configure. Press the refresh button if you can't find the node you're looking for.") + ui.label( + "Select a node to configure. Press the refresh button if you can't find the node you're looking for." + ) else: ui.label("No nodes were discovered. Refreshing again.") self.refresh() - ui.button('Refresh', on_click=lambda: self.refresh(), icon='🔄') + ui.button( + "Refresh", on_click=lambda: self.refresh(), icon="🔄" + ) for node in self.active_node_list: with ui.tab_panel(node.replace("_", " ")): - for param_name in self.json_schema["modules"][node]["ros__parameters"]: # Should this loop through the active parameters instead? + for param_name in self.json_schema[ + "modules" + ][ + node + ][ + "ros__parameters" + ]: # Should this loop through the active parameters instead? param_text = param_name.replace("_", " ") - param_type = self.json_schema["modules"][node]["ros__parameters"][param_name]["type"] - param_description = self.json_schema["modules"][node]["ros__parameters"][param_name]["description"] + param_type = self.json_schema["modules"][node][ + "ros__parameters" + ][param_name]["type"] + param_description = self.json_schema["modules"][ + node + ]["ros__parameters"][param_name]["description"] with ui.row(): match param_type: case "file": with ui.column(): - ui.button(f'Select {param_name.replace("_", " ")}:', - on_click=lambda node=node, param_name=param_name: self.pick_file(node, param_name), - icon='📂') + ui.button( + f"Select {param_name.replace('_', ' ')}:", + on_click=lambda node=node, + param_name=param_name: self.pick_file( + node, param_name + ), + icon="📂", + ) case "bool": - ui.checkbox(param_text).bind_value(self.parameters, param_name).on('change', lambda result, node=node, param_name=param_name: self.set_parameter(node, param_name, result.value)) + ui.checkbox(param_text).bind_value( + self.parameters, param_name + ).on( + "change", + lambda result, + node=node, + param_name=param_name: self.set_parameter( + node, param_name, result.value + ), + ) case "int": - ui.number(param_text).bind_value(self.parameters, param_name).on('keydown.enter', lambda result, node=node, param_name=param_name: self.set_parameter(node, param_name, int(result.sender.value))) + ui.number(param_text).bind_value( + self.parameters, param_name + ).on( + "keydown.enter", + lambda result, + node=node, + param_name=param_name: self.set_parameter( + node, + param_name, + int(result.sender.value), + ), + ) case "double": - ui.number(param_text).bind_value(self.parameters, param_name).on('keydown.enter', lambda result, node=node, param_name=param_name: self.set_parameter(node, param_name, result.sender.value)) + ui.number(param_text).bind_value( + self.parameters, param_name + ).on( + "keydown.enter", + lambda result, + node=node, + param_name=param_name: self.set_parameter( + node, + param_name, + result.sender.value, + ), + ) case "string": - ui.input(param_text).bind_value(self.parameters, param_name).on('keydown.enter', lambda result, node=node, param_name=param_name: self.set_parameter(node, param_name, result.sender.value)) + ui.input(param_text).bind_value( + self.parameters, param_name + ).on( + "keydown.enter", + lambda result, + node=node, + param_name=param_name: self.set_parameter( + node, + param_name, + result.sender.value, + ), + ) case "file_array": with ui.column(): - ui.label(param_name.replace("_", " ") + ":") - ui.button('Choose new file:', on_click=lambda node=node, param_name=param_name: self.pick_file(node, param_name, multiple=True), icon='📂') + ui.label( + param_name.replace("_", " ") + + ":" + ) + ui.button( + "Choose new file:", + on_click=lambda node=node, + param_name=param_name: self.pick_file( + node, + param_name, + multiple=True, + ), + icon="📂", + ) case "byte_array": - ui.input(param_text).bind_value(self.parameters, param_name).on('keydown.enter', lambda result, node=node, param_name=param_name: self.set_parameter(node, param_name, result.sender.value)) + ui.input(param_text).bind_value( + self.parameters, param_name + ).on( + "keydown.enter", + lambda result, + node=node, + param_name=param_name: self.set_parameter( + node, + param_name, + result.sender.value, + ), + ) case "bool_array": - ui.input(param_text).bind_value(self.parameters, param_name).on('keydown.enter', lambda result, node=node, param_name=param_name: self.set_parameter(node, param_name, result.sender.value)) + ui.input(param_text).bind_value( + self.parameters, param_name + ).on( + "keydown.enter", + lambda result, + node=node, + param_name=param_name: self.set_parameter( + node, + param_name, + result.sender.value, + ), + ) case "int_array": - ui.input(param_text).bind_value(self.parameters, param_name).on('keydown.enter', lambda result, node=node, param_name=param_name: self.set_parameter(node, param_name, result.sender.value)) + ui.input(param_text).bind_value( + self.parameters, param_name + ).on( + "keydown.enter", + lambda result, + node=node, + param_name=param_name: self.set_parameter( + node, + param_name, + result.sender.value, + ), + ) case "double_array": - ui.input(param_text).bind_value(self.parameters, param_name).on('keydown.enter', lambda result, node=node, param_name=param_name: self.set_parameter(node, param_name, result.sender.value)) + ui.input(param_text).bind_value( + self.parameters, param_name + ).on( + "keydown.enter", + lambda result, + node=node, + param_name=param_name: self.set_parameter( + node, + param_name, + result.sender.value, + ), + ) case "string_array": - ui.input(param_text).bind_value(self.parameters, param_name).on('keydown.enter', lambda result, node=node, param_name=param_name: self.set_parameter(node, param_name, result.sender.value)) + ui.input(param_text).bind_value( + self.parameters, param_name + ).on( + "keydown.enter", + lambda result, + node=node, + param_name=param_name: self.set_parameter( + node, + param_name, + result.sender.value, + ), + ) case _: - ui.label(f"Unsupported type {param_type}").classes('text-red-500') + ui.label( + f"Unsupported type {param_type}" + ).classes("text-red-500") ui.tooltip(param_description) def load_json_schema(self, file_name) -> None: - """ Loads the parameter schema from the config folder. + """Loads the parameter schema from the config folder. Args: file_name (str): Name of the parameter schema file. @@ -107,15 +244,15 @@ def load_json_schema(self, file_name) -> None: """ schema_path = os.path.join(CONF_PATH, file_name) if not os.path.exists(schema_path): - ui.notify(f'Could not find parameter schema at {schema_path}') - self.get_logger().info(f'Could not find parameter schema at {schema_path}') + ui.notify(f"Could not find parameter schema at {schema_path}") + self.get_logger().info(f"Could not find parameter schema at {schema_path}") return with open(schema_path, "r") as f: json_schema = json.load(f) return json_schema def init_parameter_clients(self, active_node_list) -> dict: - """ Initializes the get and set parameter clients for all active nodes. + """Initializes the get and set parameter clients for all active nodes. Args: active_node_list (list): List of active nodes. @@ -125,14 +262,24 @@ def init_parameter_clients(self, active_node_list) -> dict: """ client_list = {} for module in active_node_list: - list_params_client = self.create_client(ListParameters, f'/atos/{module}/list_parameters') - get_param_value_client = self.create_client(GetParameters, f'/atos/{module}/get_parameters') - set_params_client = self.create_client(SetParametersAtomically, f'/atos/{module}/set_parameters_atomically') - client_list[module] = {"list_params_client": list_params_client, "get_param_value_client": get_param_value_client, "set_params_client": set_params_client} + list_params_client = self.create_client( + ListParameters, f"/atos/{module}/list_parameters" + ) + get_param_value_client = self.create_client( + GetParameters, f"/atos/{module}/get_parameters" + ) + set_params_client = self.create_client( + SetParametersAtomically, f"/atos/{module}/set_parameters_atomically" + ) + client_list[module] = { + "list_params_client": list_params_client, + "get_param_value_client": get_param_value_client, + "set_params_client": set_params_client, + } return client_list - + def service_is_available(self, client: Client) -> bool: - """ Waits for a service to become available and notifies the user if it times out. + """Waits for a service to become available and notifies the user if it times out. Args: client (Client): Client for the service. @@ -140,29 +287,34 @@ def service_is_available(self, client: Client) -> bool: service_timeout_counter = 0 while not client.wait_for_service(timeout_sec=1.0): service_timeout_counter += 1 - self.get_logger().debug('Service not available, waiting again...') + self.get_logger().debug("Service not available, waiting again...") if service_timeout_counter >= MAX_TIMEOUT: - timeout_message = f'Service {client.srv_name} not available after {MAX_TIMEOUT} seconds, please try again later' + timeout_message = f"Service {client.srv_name} not available after {MAX_TIMEOUT} seconds, please try again later" ui.notify(timeout_message) self.get_logger().info(timeout_message) return False return True def fetch_parameters_list(self, client_list) -> None: - """ Retrieves all parameters from all active nodes and saves them in a dictionary. + """Retrieves all parameters from all active nodes and saves them in a dictionary. First calls the list parameters service for each node to get all parameter names, then calls the get parameters service to get all parameter values. Args: client_list (dict): Dictionary of clients for all active nodes. - + """ - self.get_logger().debug(f'Retrieving all parameters in {[node for node in client_list.keys()]}') + self.get_logger().debug( + f"Retrieving all parameters in {[node for node in client_list.keys()]}" + ) for node in client_list.keys(): list_params_client = client_list[node]["list_params_client"] get_param_value_client = client_list[node]["get_param_value_client"] if not self.service_is_available(list_params_client): continue - threading.Thread(target=self.call_service, args=(list_params_client, get_param_value_client)).start() + threading.Thread( + target=self.call_service, + args=(list_params_client, get_param_value_client), + ).start() def call_service(self, list_params_client, get_param_value_client) -> None: """Helper function for get_parameters_list to make the actual service call. TODO: Retire this function @@ -171,12 +323,16 @@ def call_service(self, list_params_client, get_param_value_client) -> None: list_params_client (Client): Client for the list parameters service. get_param_value_client (Client): Client for the get parameters service. """ - self.get_logger().debug(f'Calling service {list_params_client.srv_name}, setting callback service: {get_param_value_client.srv_name}') - list_params_client.call_async(ListParameters.Request()).add_done_callback(lambda future: self.fetch_parameter_values(future, get_param_value_client)) + self.get_logger().debug( + f"Calling service {list_params_client.srv_name}, setting callback service: {get_param_value_client.srv_name}" + ) + list_params_client.call_async(ListParameters.Request()).add_done_callback( + lambda future: self.fetch_parameter_values(future, get_param_value_client) + ) def fetch_parameter_values(self, future, client) -> None: - """ Retrieves all parameter values from a node (specified by the client) and saves them in a dictionary. - + """Retrieves all parameter values from a node (specified by the client) and saves them in a dictionary. + Args: future (Future): Future object returned by the list parameters service call. client (Client): Client for the get parameters service (related to a specific node). @@ -187,53 +343,80 @@ def fetch_parameter_values(self, future, client) -> None: get_param_req = GetParameters.Request() param_names = response.result.names get_param_req.names = param_names - client.call_async(get_param_req).add_done_callback(lambda future: self.save_params_locally(future, param_names)) + client.call_async(get_param_req).add_done_callback( + lambda future: self.save_params_locally(future, param_names) + ) def save_params_locally(self, future, param_names) -> None: - """ Saves all parameter values in a dictionary. + """Saves all parameter values in a dictionary. Args: future (Future): Future object returned by the get parameters service call. param_names (list): List of parameter names. """ - self.get_logger().debug(f'Got value(s) {future.result().values} for parameter(s) {param_names}') + self.get_logger().debug( + f"Got value(s) {future.result().values} for parameter(s) {param_names}" + ) for idx, param_name in enumerate(param_names): param_type = future.result().values[idx].type match param_type: case ParameterType.PARAMETER_BOOL: self.parameters[param_name] = future.result().values[idx].bool_value case ParameterType.PARAMETER_INTEGER: - self.parameters[param_name] = future.result().values[idx].integer_value + self.parameters[param_name] = ( + future.result().values[idx].integer_value + ) case ParameterType.PARAMETER_DOUBLE: - self.parameters[param_name] = future.result().values[idx].double_value + self.parameters[param_name] = ( + future.result().values[idx].double_value + ) case ParameterType.PARAMETER_STRING: - self.parameters[param_name] = future.result().values[idx].string_value + self.parameters[param_name] = ( + future.result().values[idx].string_value + ) case ParameterType.PARAMETER_BYTE_ARRAY: - self.parameters[param_name] = future.result().values[idx].byte_array_value + self.parameters[param_name] = ( + future.result().values[idx].byte_array_value + ) case ParameterType.PARAMETER_BOOL_ARRAY: - self.parameters[param_name] = future.result().values[idx].bool_array_value + self.parameters[param_name] = ( + future.result().values[idx].bool_array_value + ) case ParameterType.PARAMETER_INTEGER_ARRAY: - self.parameters[param_name] = future.result().values[idx].integer_array_value + self.parameters[param_name] = ( + future.result().values[idx].integer_array_value + ) case ParameterType.PARAMETER_DOUBLE_ARRAY: - self.parameters[param_name] = future.result().values[idx].double_array_value + self.parameters[param_name] = ( + future.result().values[idx].double_array_value + ) case ParameterType.PARAMETER_STRING_ARRAY: - self.parameters[param_name] = future.result().values[idx].string_array_value + self.parameters[param_name] = ( + future.result().values[idx].string_array_value + ) case _: - self.get_logger().info(f'Parameter {param_name} has an unsupported type {param_type}') + self.get_logger().info( + f"Parameter {param_name} has an unsupported type {param_type}" + ) async def pick_file(self, node_name, param_name, multiple=False) -> None: - """ Opens a file picker and sets the selected files as the parameter value. + """Opens a file picker and sets the selected files as the parameter value. Args: node_name (str): Name of the node that the parameter belongs to. param_name (str): Name of the parameter. multiple (bool, optional): Whether to allow multiple files to be selected. Defaults to False. """ - result = await local_file_picker('~/.astazero/ATOS', upper_limit='~', multiple=multiple, show_hidden_files=True) - ui.notify(f'You selected {result}') + result = await local_file_picker( + "~/.astazero/ATOS", + upper_limit="~", + multiple=multiple, + show_hidden_files=True, + ) + ui.notify(f"You selected {result}") if not result: return - if multiple: + if multiple: param_value = result else: param_value = result[0] @@ -241,8 +424,8 @@ async def pick_file(self, node_name, param_name, multiple=False) -> None: self.set_parameter(node_name, param_name, param_value) def set_param_callback(self, future, param_name, param_value) -> None: - """ Callback function for the set parameters service call. Notifies the user if the call was successful or not. - + """Callback function for the set parameters service call. Notifies the user if the call was successful or not. + Args: future (Future): Future object returned by the set parameters service call (Bool). param_name (str): Name of the parameter. @@ -250,22 +433,24 @@ def set_param_callback(self, future, param_name, param_value) -> None: try: response = future.result() except Exception as e: - self.get_logger().info('Service call failed %r' % (e,)) + self.get_logger().info("Service call failed %r" % (e,)) return - response_text = f'Setting parameter \'{param_name}\' to \'{param_value}\' was {"successful" if response.result.successful else "unsuccesful"}' + response_text = f"Setting parameter '{param_name}' to '{param_value}' was {'successful' if response.result.successful else 'unsuccesful'}" self.get_logger().debug(response_text) with self.splitter: ui.notify(response_text) def set_parameter(self, node_name, param_name, param_value) -> None: - """ Sets the value for a specific parameter. + """Sets the value for a specific parameter. Args: node_name (str): Name of the node that the parameter belongs to. param_name (str): Name of the parameter. param_value (any): Value of the parameter. """ - self.get_logger().debug(f'Setting parameter {param_name} in {node_name} to {param_value}') + self.get_logger().debug( + f"Setting parameter {param_name} in {node_name} to {param_value}" + ) client = self.parameter_clients[node_name]["set_params_client"] if not self.service_is_available(client): @@ -273,16 +458,24 @@ def set_parameter(self, node_name, param_name, param_value) -> None: parameter = Parameter() parameter.name = param_name - param_type = self.json_schema["modules"][node_name]["ros__parameters"][param_name]["type"] - self.assign_value_and_type_to_parameter(parameter, param_type, param_value) # Checks param_value type and assigns it to the parameter + param_type = self.json_schema["modules"][node_name]["ros__parameters"][ + param_name + ]["type"] + self.assign_value_and_type_to_parameter( + parameter, param_type, param_value + ) # Checks param_value type and assigns it to the parameter param_req = SetParametersAtomically.Request() param_req.parameters.append(parameter) future = client.call_async(param_req) - future.add_done_callback(lambda future: self.set_param_callback(future, param_name, param_value)) + future.add_done_callback( + lambda future: self.set_param_callback(future, param_name, param_value) + ) - def assign_value_and_type_to_parameter(self, parameter, param_type, param_value) -> None: - """ Assigns a value and type to a parameter. Translates the parameter type from the json schema to the ROS2 parameter type. + def assign_value_and_type_to_parameter( + self, parameter, param_type, param_value + ) -> None: + """Assigns a value and type to a parameter. Translates the parameter type from the json schema to the ROS2 parameter type. Args: parameter (Parameter): Parameter object. @@ -292,84 +485,89 @@ def assign_value_and_type_to_parameter(self, parameter, param_type, param_value) match param_type: case "file": if type(param_value) != str: - ui.notify(f'Please select a file') + ui.notify("Please select a file") return parameter.value.type = ParameterType.PARAMETER_STRING parameter.value.string_value = param_value case "bool": if type(param_value) != bool: - ui.notify(f'Please select a boolean') + ui.notify("Please select a boolean") return parameter.value.type = ParameterType.PARAMETER_BOOL parameter.value.bool_value = param_value case "int": if type(param_value) != int: - ui.notify(f'Please select an integer') + ui.notify("Please select an integer") return parameter.value.type = ParameterType.PARAMETER_INTEGER parameter.value.integer_value = param_value case "double": if type(param_value) != float: - ui.notify(f'Please select a double') + ui.notify("Please select a double") return parameter.value.type = ParameterType.PARAMETER_DOUBLE parameter.value.double_value = param_value case "string": if type(param_value) != str: - ui.notify(f'Please select a string') + ui.notify("Please select a string") return parameter.value.type = ParameterType.PARAMETER_STRING parameter.value.string_value = param_value case "file_array": if type(param_value) != list or type(param_value[0]) != str: - ui.notify(f'Please select one or more files') + ui.notify("Please select one or more files") return parameter.value.type = ParameterType.PARAMETER_STRING_ARRAY parameter.value.string_array_value = param_value case "byte_array": if type(param_value) != list or type(param_value[0]) != int: - ui.notify(f'Please select one or more bytes') + ui.notify("Please select one or more bytes") return parameter.value.type = ParameterType.PARAMETER_BYTE_ARRAY parameter.value.byte_array_value = param_value case "bool_array": if type(param_value) != list or type(param_value[0]) != bool: - ui.notify(f'Please select one or more booleans') + ui.notify("Please select one or more booleans") return parameter.value.type = ParameterType.PARAMETER_BOOL_ARRAY parameter.value.bool_array_value = param_value case "int_array": if type(param_value) != list or type(param_value[0]) != int: - ui.notify(f'Please select one or more integers') + ui.notify("Please select one or more integers") return parameter.value.type = ParameterType.PARAMETER_INTEGER_ARRAY parameter.value.integer_array_value = param_value case "double_array": if type(param_value) != list or type(param_value[0]) != float: - ui.notify(f'Please select one or more doubles') + ui.notify("Please select one or more doubles") return parameter.value.type = ParameterType.PARAMETER_DOUBLE_ARRAY parameter.value.double_array_value = param_value case "string_array": if type(param_value) != list or type(param_value[0]) != str: - ui.notify(f'Please select one or more strings') + ui.notify("Please select one or more strings") return parameter.value.type = ParameterType.PARAMETER_STRING_ARRAY parameter.value.string_array_value = param_value case _: - self.get_logger().info(f'Unsupported type {param_type}') - ui.notify(f'Unsupported type {param_type}') + self.get_logger().info(f"Unsupported type {param_type}") + ui.notify(f"Unsupported type {param_type}") return def refresh(self) -> None: - """ Refreshes the config panel by fetching all active nodes and their parameters again. - """ + """Refreshes the config panel by fetching all active nodes and their parameters again.""" modules = self.json_schema["modules"].keys() self.active_nodes_and_namespaces = self.get_node_names_and_namespaces() - self.active_node_list = [node for node, namespace in self.active_nodes_and_namespaces if node in modules and "atos" in namespace] - self.active_node_list = list(set(self.active_node_list)) # Removes duplicates + self.active_node_list = [ + node + for node, namespace in self.active_nodes_and_namespaces + if node in modules and "atos" in namespace + ] + self.active_node_list = list(set(self.active_node_list)) # Removes duplicates self.parameter_clients = self.init_parameter_clients(self.active_node_list) self.parameters = {} - threading.Thread(target=self.fetch_parameters_list, args=(self.parameter_clients,)).start() - ui.open('/config') + threading.Thread( + target=self.fetch_parameters_list, args=(self.parameter_clients,) + ).start() + ui.open("/config") diff --git a/atos_gui/atos_gui/configpanel/local_file_picker.py b/atos_gui/atos_gui/configpanel/local_file_picker.py index c7fe5e6af..41b7a4c02 100644 --- a/atos_gui/atos_gui/configpanel/local_file_picker.py +++ b/atos_gui/atos_gui/configpanel/local_file_picker.py @@ -14,9 +14,14 @@ class local_file_picker(ui.dialog): - - def __init__(self, directory: str, *, - upper_limit: Optional[str] = ..., multiple: bool = False, show_hidden_files: bool = False) -> None: + def __init__( + self, + directory: str, + *, + upper_limit: Optional[str] = ..., + multiple: bool = False, + show_hidden_files: bool = False, + ) -> None: """Local File Picker This is a simple file picker that allows you to select a file from the local filesystem where NiceGUI is running. @@ -32,69 +37,85 @@ def __init__(self, directory: str, *, if upper_limit is None: self.upper_limit = None else: - self.upper_limit = Path(directory if upper_limit == ... else upper_limit).expanduser() + self.upper_limit = Path( + directory if upper_limit == ... else upper_limit + ).expanduser() self.show_hidden_files = show_hidden_files with self, ui.card(): self.add_drives_toggle() - self.grid = ui.aggrid({ - 'columnDefs': [{'field': 'name', 'headerName': 'File'}], - 'rowSelection': 'multiple' if multiple else 'single', - }, html_columns=[0]).classes('w-96').on('cellDoubleClicked', self.handle_double_click) - with ui.row().classes('w-full justify-end'): - ui.button('Cancel', on_click=self.close).props('outline') - ui.button('Ok', on_click=self._handle_ok) + self.grid = ( + ui.aggrid( + { + "columnDefs": [{"field": "name", "headerName": "File"}], + "rowSelection": "multiple" if multiple else "single", + }, + html_columns=[0], + ) + .classes("w-96") + .on("cellDoubleClicked", self.handle_double_click) + ) + with ui.row().classes("w-full justify-end"): + ui.button("Cancel", on_click=self.close).props("outline") + ui.button("Ok", on_click=self._handle_ok) self.update_grid() def add_drives_toggle(self): - """Add a toggle to select the drive on Windows. - """ - if platform.system() == 'Windows': + """Add a toggle to select the drive on Windows.""" + if platform.system() == "Windows": import win32api - drives = win32api.GetLogicalDriveStrings().split('\000')[:-1] - self.drives_toggle = ui.toggle(drives, value=drives[0], on_change=self.update_drive) + + drives = win32api.GetLogicalDriveStrings().split("\000")[:-1] + self.drives_toggle = ui.toggle( + drives, value=drives[0], on_change=self.update_drive + ) def update_drive(self): - """Update the grid to show the files of the selected drive. - """ + """Update the grid to show the files of the selected drive.""" self.path = Path(self.drives_toggle.value).expanduser() self.update_grid() def update_grid(self) -> None: - """Update the grid to show the files of the current directory. - """ - paths = list(self.path.glob('*')) + """Update the grid to show the files of the current directory.""" + paths = list(self.path.glob("*")) if not self.show_hidden_files: - paths = [p for p in paths if not p.name.startswith('.')] + paths = [p for p in paths if not p.name.startswith(".")] paths.sort(key=lambda p: p.name.lower()) paths.sort(key=lambda p: not p.is_dir()) - self.grid.options['rowData'] = [ + self.grid.options["rowData"] = [ { - 'name': f'📁 {p.name}' if p.is_dir() else p.name, - 'path': str(p), + "name": f"📁 {p.name}" if p.is_dir() else p.name, + "path": str(p), } for p in paths ] - if self.upper_limit is None and self.path != self.path.parent or \ - self.upper_limit is not None and self.path != self.upper_limit: - self.grid.options['rowData'].insert(0, { - 'name': '📁 ..', - 'path': str(self.path.parent), - }) + if ( + self.upper_limit is None + and self.path != self.path.parent + or self.upper_limit is not None + and self.path != self.upper_limit + ): + self.grid.options["rowData"].insert( + 0, + { + "name": "📁 ..", + "path": str(self.path.parent), + }, + ) self.grid.update() def handle_double_click(self, e: events.GenericEventArguments) -> None: - """Handle a double click on a file or directory. - """ - self.path = Path(e.args['data']['path']) + """Handle a double click on a file or directory.""" + self.path = Path(e.args["data"]["path"]) if self.path.is_dir(): self.update_grid() else: self.submit([str(self.path)]) async def _handle_ok(self): - """Handle the OK button click. - """ - rows = await ui.run_javascript(f'getElement({self.grid.id}).gridOptions.api.getSelectedRows()') - self.submit([r['path'] for r in rows]) \ No newline at end of file + """Handle the OK button click.""" + rows = await ui.run_javascript( + f"getElement({self.grid.id}).gridOptions.api.getSelectedRows()" + ) + self.submit([r["path"] for r in rows]) diff --git a/atos_gui/atos_gui/controlpanel/controlpanel.py b/atos_gui/atos_gui/controlpanel/controlpanel.py index 62a1d3e55..2fb6445ca 100644 --- a/atos_gui/atos_gui/controlpanel/controlpanel.py +++ b/atos_gui/atos_gui/controlpanel/controlpanel.py @@ -3,86 +3,134 @@ file, You can obtain one at https://mozilla.org/MPL/2.0/. """ -from atos_interfaces.srv import * - import rclpy -from std_msgs.msg import Empty +from nicegui import ui from rclpy.node import Node +from std_msgs.msg import Empty -from nicegui import ui +from atos_interfaces.srv import * QOS = rclpy.qos.QoSProfile(depth=10) OBC_STATES = { - 0: 'UNDEFINED', - 1: 'IDLE', - 2: 'INITIALIZED', - 3: 'CONNECTED', - 4: 'ARMED', - 5: 'DISARMING', - 6: 'RUNNING', - 7: 'REMOTECTRL', - 8: 'ERROR', - 9: 'ABORTING', - 10: 'CLEARING' - } + 0: "UNDEFINED", + 1: "IDLE", + 2: "INITIALIZED", + 3: "CONNECTED", + 4: "ARMED", + 5: "DISARMING", + 6: "RUNNING", + 7: "REMOTECTRL", + 8: "ERROR", + 9: "ABORTING", + 10: "CLEARING", +} + class ControlPanelNode(Node): def __init__(self) -> None: - super().__init__('control_panel') - self.initPub = self.create_publisher(Empty, '/atos/init', QOS) - self.connectPub = self.create_publisher(Empty, '/atos/connect', QOS) - self.disconnectPub = self.create_publisher(Empty, '/atos/disconnect', QOS) - self.armPub = self.create_publisher(Empty, '/atos/arm', QOS) - self.disarmPub = self.create_publisher(Empty, '/atos/disarm', QOS) - self.startPub = self.create_publisher(Empty, '/atos/start', QOS) - self.abortPub = self.create_publisher(Empty, '/atos/abort', QOS) - self.allClearPub = self.create_publisher(Empty, '/atos/all_clear', QOS) - self.resetTestObjectsPub = self.create_publisher(Empty, '/atos/reset_test_objects', QOS) - self.reloadObjectSettingsPub = self.create_publisher(Empty, '/atos/reload_object_settings', QOS) - - self.get_object_control_state_client = self.create_client(GetObjectControlState, '/atos/get_object_control_state') + super().__init__("control_panel") + self.initPub = self.create_publisher(Empty, "/atos/init", QOS) + self.connectPub = self.create_publisher(Empty, "/atos/connect", QOS) + self.disconnectPub = self.create_publisher(Empty, "/atos/disconnect", QOS) + self.armPub = self.create_publisher(Empty, "/atos/arm", QOS) + self.disarmPub = self.create_publisher(Empty, "/atos/disarm", QOS) + self.startPub = self.create_publisher(Empty, "/atos/start", QOS) + self.abortPub = self.create_publisher(Empty, "/atos/abort", QOS) + self.allClearPub = self.create_publisher(Empty, "/atos/all_clear", QOS) + self.resetTestObjectsPub = self.create_publisher( + Empty, "/atos/reset_test_objects", QOS + ) + self.reloadObjectSettingsPub = self.create_publisher( + Empty, "/atos/reload_object_settings", QOS + ) + + self.get_object_control_state_client = self.create_client( + GetObjectControlState, "/atos/get_object_control_state" + ) self.OBC_state_req = GetObjectControlState.Request() - self.get_object_control_state_timer = self.create_timer(0.5, self.get_object_control_state_callback) + self.get_object_control_state_timer = self.create_timer( + 0.5, self.get_object_control_state_callback + ) - self.OBC_state = {'state': "UNDEFINED"} + self.OBC_state = {"state": "UNDEFINED"} self.lost_connection = True - @ui.page(path='/control', title="ATOS Control Panel") + @ui.page(path="/control", title="ATOS Control Panel") def render_page(): - with ui.row().bind_visibility_from(self, 'lost_connection'): - ui.label('Lost connection to ATOS...').tailwind.text_color('red') + with ui.row().bind_visibility_from(self, "lost_connection"): + ui.label("Lost connection to ATOS...").tailwind.text_color("red") with ui.row(): - ui.button('Abort', on_click=lambda: self.abortPub.publish(Empty()), color='red').props('size=large ') + ui.button( + "Abort", + on_click=lambda: self.abortPub.publish(Empty()), + color="red", + ).props("size=large ") with ui.row(): - ui.button('Init', on_click=lambda: self.initPub.publish(Empty()), color='blue') - ui.button('Connect', on_click=lambda: self.connectPub.publish(Empty()), color='blue') - ui.button('Disconnect', on_click=lambda: self.disconnectPub.publish(Empty()), color='grey') - ui.button('Arm', on_click=lambda: self.armPub.publish(Empty()), color='orange') - ui.button('Disarm', on_click=lambda: self.disarmPub.publish(Empty()), color='orange') - ui.button('Start', on_click=lambda: self.startPub.publish(Empty()), color='green') - ui.button('All Clear', on_click=lambda: self.allClearPub.publish(Empty()), color='grey') + ui.button( + "Init", on_click=lambda: self.initPub.publish(Empty()), color="blue" + ) + ui.button( + "Connect", + on_click=lambda: self.connectPub.publish(Empty()), + color="blue", + ) + ui.button( + "Disconnect", + on_click=lambda: self.disconnectPub.publish(Empty()), + color="grey", + ) + ui.button( + "Arm", on_click=lambda: self.armPub.publish(Empty()), color="orange" + ) + ui.button( + "Disarm", + on_click=lambda: self.disarmPub.publish(Empty()), + color="orange", + ) + ui.button( + "Start", + on_click=lambda: self.startPub.publish(Empty()), + color="green", + ) + ui.button( + "All Clear", + on_click=lambda: self.allClearPub.publish(Empty()), + color="grey", + ) with ui.row(): - ui.label().bind_text_from(self.OBC_state, 'state', backward=lambda n: f'State: {n}').classes('text-lg') + ui.label().bind_text_from( + self.OBC_state, "state", backward=lambda n: f"State: {n}" + ).classes("text-lg") with ui.row(): - ui.button('Reset Test Objects', on_click=lambda: self.resetTestObjectsPub.publish(Empty()), color='grey') - ui.button('Reload Object Settings', on_click=lambda: self.reloadObjectSettingsPub.publish(Empty()), color='grey') - + ui.button( + "Reset Test Objects", + on_click=lambda: self.resetTestObjectsPub.publish(Empty()), + color="grey", + ) + ui.button( + "Reload Object Settings", + on_click=lambda: self.reloadObjectSettingsPub.publish(Empty()), + color="grey", + ) def get_object_control_state_callback(self): # Call the service - while not self.get_object_control_state_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') + while not self.get_object_control_state_client.wait_for_service( + timeout_sec=1.0 + ): + self.get_logger().info("service not available, waiting again...") self.lost_connection = True self.lost_connection = False future = self.get_object_control_state_client.call_async(self.OBC_state_req) - future.add_done_callback(lambda future: self.get_object_control_state_callback_done(future)) - + future.add_done_callback( + lambda future: self.get_object_control_state_callback_done(future) + ) def get_object_control_state_callback_done(self, future): try: response = future.result() except Exception as e: - self.get_logger().info('Service call failed %r' % (e,)) + self.get_logger().info("Service call failed %r" % (e,)) else: self.OBC_state["state"] = OBC_STATES[response.state] diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index e438f9707..dd9c3a9bf 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -3,31 +3,32 @@ file, You can obtain one at https://mozilla.org/MPL/2.0/. """ +import sys import threading from pathlib import Path -from atos_interfaces.srv import * -import sys +import nicegui import rclpy +from nicegui import app, ui, ui_run from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor -from atos_gui.controlpanel.controlpanel import ControlPanelNode + from atos_gui.configpanel.configpanel import ConfigPanelNode +from atos_gui.controlpanel.controlpanel import ControlPanelNode from atos_gui.objectpanel.objectpanel import ObjectPanelNode - -import nicegui -from nicegui import app, ui, ui_run +from atos_interfaces.srv import * USE_SSL = sys.argv[1] == "True" + def main() -> None: # NOTE: This function is defined as the ROS entry point in setup.py, but it's empty to enable NiceGUI auto-reloading pass def ros_main() -> None: - nicegui.ui.link('Control Panel', '/control') - nicegui.ui.link('Config Panel', '/config') - nicegui.ui.link('Object Panel', '/object') + nicegui.ui.link("Control Panel", "/control") + nicegui.ui.link("Config Panel", "/config") + nicegui.ui.link("Object Panel", "/object") rclpy.init() control_panel = ControlPanelNode() config_panel = ConfigPanelNode() @@ -46,29 +47,33 @@ def ros_main() -> None: config_panel.destroy_node() object_panel.destroy_node() -#Starting the ros node in a thread managed by nicegui. It will be restarted with "on_startup" after a reload. -#It has to be in a thread, since NiceGUI wants the main thread for itself. + +# Starting the ros node in a thread managed by nicegui. It will be restarted with "on_startup" after a reload. +# It has to be in a thread, since NiceGUI wants the main thread for itself. app.on_startup(lambda: threading.Thread(target=ros_main).start()) -ui_run.APP_IMPORT_STRING = f'{__name__}:app' # ROS2 uses a non-standard module name, so we need to specify it here +ui_run.APP_IMPORT_STRING = f"{__name__}:app" # ROS2 uses a non-standard module name, so we need to specify it here # Prepare the arguments for ui.run() uvicorn_args = { - 'uvicorn_reload_dirs': str(Path(__file__).parent.resolve()), + "uvicorn_reload_dirs": str(Path(__file__).parent.resolve()), # 'favicon': '/images/favicon.ico', - 'port': 3000, - 'show': False, # Disable auto-opening the browser - 'title': 'ATOS GUI' + "port": 3000, + "show": False, # Disable auto-opening the browser + "title": "ATOS GUI", } # If use_ssl is True, add the SSL arguments if USE_SSL: - uvicorn_args['ssl_keyfile'] = Path.home() / ".astazero/ATOS/certs/selfsigned.key" - uvicorn_args['ssl_certfile'] = Path.home() / ".astazero/ATOS/certs/selfsigned.crt" + uvicorn_args["ssl_keyfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.key" + uvicorn_args["ssl_certfile"] = Path.home() / ".astazero/ATOS/certs/selfsigned.crt" # Call ui.run() with the prepared arguments ui.run(**uvicorn_args) # If print is above ui.run(), it will be printed twice for some reason if USE_SSL: - print("ATTENTION: Using SSL, use https://localhost:3000 to access the GUI instead", flush=True) + print( + "ATTENTION: Using SSL, use https://localhost:3000 to access the GUI instead", + flush=True, + ) diff --git a/atos_gui/atos_gui/objectpanel/objectpanel.py b/atos_gui/atos_gui/objectpanel/objectpanel.py index e4548f8cd..86c256af1 100644 --- a/atos_gui/atos_gui/objectpanel/objectpanel.py +++ b/atos_gui/atos_gui/objectpanel/objectpanel.py @@ -1,127 +1,157 @@ -from atos_interfaces.srv import GetObjectIds, GetObjectIp, SetObjectIp - +from nicegui import Client, ui from rclpy.node import Node -from nicegui import Client, ui +from atos_interfaces.srv import GetObjectIds, GetObjectIp, SetObjectIp MAX_TIMEOUT = 3 + class ObjectPanelNode(Node): - """ This node is responsible for rendering the object panel and visualize the IP address for each object ID in the scenario. - """ + """This node is responsible for rendering the object panel and visualize the IP address for each object ID in the scenario.""" + def __init__(self) -> None: - """ Initializes the node, fetches all object IDs and their IPs in the current scenario and visualizes them. - """ - super().__init__('object_panel') - self.get_id_client = self.create_client(GetObjectIds, '/atos/get_object_ids') - self.get_ip_client = self.create_client(GetObjectIp, '/atos/get_object_ip') - self.set_ip_client = self.create_client(SetObjectIp, SetObjectIp.Request.SERVICE_NAME) - + """Initializes the node, fetches all object IDs and their IPs in the current scenario and visualizes them.""" + super().__init__("object_panel") + self.get_id_client = self.create_client(GetObjectIds, "/atos/get_object_ids") + self.get_ip_client = self.create_client(GetObjectIp, "/atos/get_object_ip") + self.set_ip_client = self.create_client( + SetObjectIp, SetObjectIp.Request.SERVICE_NAME + ) + self.object_ids_req = GetObjectIds.Request() self.object_id_ip_map = {} self.get_object_ids() with Client.auto_index_client: pass - - @ui.page(path='/object', title="ATOS Object Panel") + + @ui.page(path="/object", title="ATOS Object Panel") def render_objectpanel() -> None: - """ Render the object panel page. Generates a list of objects and their IPs. - """ + """Render the object panel page. Generates a list of objects and their IPs.""" with ui.row() as self.refresh_row: - with ui.button(text='Refresh', on_click=self.refresh): - ui.icon('refresh') + with ui.button(text="Refresh", on_click=self.refresh): + ui.icon("refresh") for object_id in self.object_id_ip_map.keys(): - with ui.element('div').classes('p-2 bg-blue-100'): + with ui.element("div").classes("p-2 bg-blue-100"): with ui.row(): - ui.label(f'Object {object_id}: ') - ui.label(self.object_id_ip_map[object_id]) + ui.label(f"Object {object_id}: ") + ui.label(self.object_id_ip_map[object_id]) # Will be used in the future # ui.input(label=f'Object {object_id}').bind_value(self.object_id_ip_map, object_id # ).on('keydown.enter', lambda result, object_id=object_id: self.update_object_ip(object_id, result.sender.value)) def get_object_ids(self): - """ Fetches all object IDs in the current scenario and calls the get_object_ips method for each object to fetch their IPs. - """ + """Fetches all object IDs in the current scenario and calls the get_object_ips method for each object to fetch their IPs.""" service_timeout_counter = 0 while not self.get_id_client.wait_for_service(timeout_sec=1.0): service_timeout_counter += 1 - self.get_logger().debug('Get object ID service not available, waiting again...') + self.get_logger().debug( + "Get object ID service not available, waiting again..." + ) if service_timeout_counter > MAX_TIMEOUT: - ui.notify(f'Get object ID service not available after {MAX_TIMEOUT} seconds') - self.get_logger().info(f'Get object ID service not available after {MAX_TIMEOUT} seconds') + ui.notify( + f"Get object ID service not available after {MAX_TIMEOUT} seconds" + ) + self.get_logger().info( + f"Get object ID service not available after {MAX_TIMEOUT} seconds" + ) return future = self.get_id_client.call_async(self.object_ids_req) - future.add_done_callback(lambda future: self.get_object_ips(future.result().ids)) + future.add_done_callback( + lambda future: self.get_object_ips(future.result().ids) + ) def get_object_ips(self, object_ids): - """ Fetches the IP address for each object ID in the current scenario. - + """Fetches the IP address for each object ID in the current scenario. + Args: object_ids (list): List of object IDs in the current scenario. - + """ service_timeout_counter = 0 while not self.get_ip_client.wait_for_service(timeout_sec=1.0): service_timeout_counter += 1 - self.get_logger().debug('Get object IP service not available, waiting again...') + self.get_logger().debug( + "Get object IP service not available, waiting again..." + ) if service_timeout_counter > MAX_TIMEOUT: - ui.notify(f'Get object IP service not available after {MAX_TIMEOUT} seconds') - self.get_logger().info(f'Get object IP service not available after {MAX_TIMEOUT} seconds') + ui.notify( + f"Get object IP service not available after {MAX_TIMEOUT} seconds" + ) + self.get_logger().info( + f"Get object IP service not available after {MAX_TIMEOUT} seconds" + ) return for object_id in object_ids: self.object_id_ip_map[object_id] = "" future = self.get_ip_client.call_async(GetObjectIp.Request(id=object_id)) - future.add_done_callback(lambda future: self.save_ips_locally(future.result())) + future.add_done_callback( + lambda future: self.save_ips_locally(future.result()) + ) def save_ips_locally(self, object_ip): - """ Saves the IP address for each object ID in the current scenario locally. + """Saves the IP address for each object ID in the current scenario locally. Args: object_ip (GetObjectIp.Response): IP address for the given object ID. - + """ self.object_id_ip_map[object_ip.id] = object_ip.ip - self.get_logger().debug(f'Object {object_ip.id} IP: {object_ip.ip}') + self.get_logger().debug(f"Object {object_ip.id} IP: {object_ip.ip}") def update_object_ip(self, object_id, object_ip): - """ Updates the IP address for a given object ID in the current scenario. - + """Updates the IP address for a given object ID in the current scenario. + Args: object_id (int): Object ID to update the IP address for. object_ip (str): New IP address for the given object ID. - + """ - ui.notify(f'Setting object {object_id} IP: {object_ip}') + ui.notify(f"Setting object {object_id} IP: {object_ip}") service_timeout_counter = 0 while not self.set_ip_client.wait_for_service(timeout_sec=1.0): - self.get_logger().debug('Set object IP service not available, waiting again...') + self.get_logger().debug( + "Set object IP service not available, waiting again..." + ) if service_timeout_counter > MAX_TIMEOUT: - ui.notify(f'Set object IP service not available after {MAX_TIMEOUT} seconds') - self.get_logger().info(f'Set object IP service not available after {MAX_TIMEOUT} seconds') + ui.notify( + f"Set object IP service not available after {MAX_TIMEOUT} seconds" + ) + self.get_logger().info( + f"Set object IP service not available after {MAX_TIMEOUT} seconds" + ) return - future = self.set_ip_client.call_async(SetObjectIp.Request(id=object_id, ip=object_ip)) - future.add_done_callback(lambda future: self.update_object_ip_result(future.result())) - + future = self.set_ip_client.call_async( + SetObjectIp.Request(id=object_id, ip=object_ip) + ) + future.add_done_callback( + lambda future: self.update_object_ip_result(future.result()) + ) + def update_object_ip_result(self, result): - """ Updates the IP address for a given object ID in the current scenario. + """Updates the IP address for a given object ID in the current scenario. Args: result (SetObjectIp.Response): Response from the set object IP service. - + """ if result.success: with self.refresh_row: - ui.notify(f'IP set to {result.ip} for object {result.id} was successful') - self.get_logger().info(f'IP set to {result.ip} for object {result.id} was successful') + ui.notify( + f"IP set to {result.ip} for object {result.id} was successful" + ) + self.get_logger().info( + f"IP set to {result.ip} for object {result.id} was successful" + ) self.object_id_ip_map[result.id] = result.ip else: with self.refresh_row: - ui.notify(f'Failed to set object {result.id} IP to {result.ip}') - self.get_logger().info(f'Failed to set object {result.id} IP to {result.ip}') + ui.notify(f"Failed to set object {result.id} IP to {result.ip}") + self.get_logger().info( + f"Failed to set object {result.id} IP to {result.ip}" + ) def refresh(self): - """ Refreshes the object panel by fetching all object IDs and their IPs in the current scenario. - """ + """Refreshes the object panel by fetching all object IDs and their IPs in the current scenario.""" self.get_object_ids() - ui.open('/object') + ui.open("/object") diff --git a/atos_gui/launch/gui.py b/atos_gui/launch/gui.py index 3165f1046..2248ceced 100644 --- a/atos_gui/launch/gui.py +++ b/atos_gui/launch/gui.py @@ -1,29 +1,33 @@ from launch import LaunchDescription -from launch_ros.actions import Node from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PythonExpression from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node -def generate_launch_description(): - insecure_launch_arg = DeclareLaunchArgument('insecure', default_value='False') +def generate_launch_description(): + insecure_launch_arg = DeclareLaunchArgument("insecure", default_value="False") - return LaunchDescription([ - insecure_launch_arg, - Node( - condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('insecure')])), - package='atos_gui', - namespace='atos', - executable='gui', - output='screen', - arguments = ["True"] # Use ssl - ), - Node( - condition=IfCondition(LaunchConfiguration('insecure')), - package='atos_gui', - namespace='atos', - executable='gui', - output='screen', - arguments = ["False"] # Don't use ssl - ) - ]) \ No newline at end of file + return LaunchDescription( + [ + insecure_launch_arg, + Node( + condition=IfCondition( + PythonExpression(["not ", LaunchConfiguration("insecure")]) + ), + package="atos_gui", + namespace="atos", + executable="gui", + output="screen", + arguments=["True"], # Use ssl + ), + Node( + condition=IfCondition(LaunchConfiguration("insecure")), + package="atos_gui", + namespace="atos", + executable="gui", + output="screen", + arguments=["False"], # Don't use ssl + ), + ] + ) diff --git a/atos_gui/setup.py b/atos_gui/setup.py index 44ae0fab7..ea3f8726a 100644 --- a/atos_gui/setup.py +++ b/atos_gui/setup.py @@ -1,27 +1,25 @@ import xml.etree.ElementTree as ET from pathlib import Path -from setuptools import setup, find_packages +from setuptools import find_packages, setup -package_xml = ET.parse('package.xml').getroot() -package_name = package_xml.find('name').text -data = Path('share') / package_name +package_xml = ET.parse("package.xml").getroot() +package_name = package_xml.find("name").text +data = Path("share") / package_name setup( name=package_name, - version=package_xml.find('version').text, + version=package_xml.find("version").text, packages=find_packages(), - maintainer=package_xml.find('license').text, - maintainer_email=package_xml.find('maintainer').attrib['email'], - license=package_xml.find('license').text, + maintainer=package_xml.find("license").text, + maintainer_email=package_xml.find("maintainer").attrib["email"], + license=package_xml.find("license").text, data_files=[ - (str(data), ['package.xml']), - (str(data / 'launch'), ['launch/gui.py']), + (str(data), ["package.xml"]), + (str(data / "launch"), ["launch/gui.py"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, entry_points={ - 'console_scripts': [ - 'gui = atos_gui.main:main' - ], + "console_scripts": ["gui = atos_gui.main:main"], }, -) \ No newline at end of file +) diff --git a/plugins/rviz2/include/object_monitor_display.hpp b/plugins/rviz2/include/object_monitor_display.hpp index 839f45a02..3d2b848b0 100644 --- a/plugins/rviz2/include/object_monitor_display.hpp +++ b/plugins/rviz2/include/object_monitor_display.hpp @@ -5,8 +5,6 @@ */ #pragma once -#include -#include #include "rviz_common/interaction/forwards.hpp" #include "rviz_common/properties/color_property.hpp" #include "rviz_common/properties/enum_property.hpp" @@ -15,19 +13,21 @@ #include "rviz_rendering/objects/arrow.hpp" #include "rviz_rendering/objects/axes.hpp" #include "rviz_rendering/objects/shape.hpp" +#include +#include namespace atos_rviz_plugins { class MonitorDisplay : public rviz_common::RosTopicDisplay { Q_OBJECT - public: +public: enum Shape { Arrow, Axes, }; MonitorDisplay(); - private: +private: void processMessage(atos_interfaces::msg::Monitor::ConstSharedPtr msg); void onInitialize() override; void reset() override; @@ -37,7 +37,7 @@ class MonitorDisplay : public rviz_common::RosTopicDisplay -#include #include "rclcpp/logging.hpp" +#include +#include namespace atos_rviz_plugins { -MonitorDisplay::MonitorDisplay() -: arrow_(nullptr), axes_(nullptr), pose_valid_(false) { +MonitorDisplay::MonitorDisplay() : + arrow_(nullptr), + axes_(nullptr), + pose_valid_(false) { shape_property_ = new rviz_common::properties::EnumProperty( - "Shape", "Arrow", "Shape to display the pose as.", this, SLOT(updateShapeChoice())); + "Shape", "Arrow", "Shape to display the pose as.", this, SLOT(updateShapeChoice())); shape_property_->addOption("Arrow", Arrow); shape_property_->addOption("Axes", Axes); color_property_ = new rviz_common::properties::ColorProperty( - "Color", QColor(255, 25, 0), "Color to draw the arrow.", this, SLOT(updateColorAndAlpha())); + "Color", QColor(255, 25, 0), "Color to draw the arrow.", this, SLOT(updateColorAndAlpha())); alpha_property_ = new rviz_common::properties::FloatProperty( - "Alpha", 1, "Amount of transparency to apply to the arrow.", this, SLOT(updateColorAndAlpha())); + "Alpha", 1, "Amount of transparency to apply to the arrow.", this, SLOT(updateColorAndAlpha())); alpha_property_->setMin(0); alpha_property_->setMax(1); - shaft_length_property_ = new rviz_common::properties::FloatProperty( - "Shaft Length", 1, "Length of the arrow's shaft, in meters.", - this, SLOT(updateArrowGeometry())); + shaft_length_property_ = new rviz_common::properties::FloatProperty( + "Shaft Length", 1, "Length of the arrow's shaft, in meters.", this, SLOT(updateArrowGeometry())); - shaft_radius_property_ = new rviz_common::properties::FloatProperty( - "Shaft Radius", 0.05f, "Radius of the arrow's shaft, in meters.", - this, SLOT(updateArrowGeometry())); + shaft_radius_property_ = new rviz_common::properties::FloatProperty( + "Shaft Radius", 0.05f, "Radius of the arrow's shaft, in meters.", this, SLOT(updateArrowGeometry())); - head_length_property_ = new rviz_common::properties::FloatProperty( - "Head Length", 0.3f, "Length of the arrow's head, in meters.", - this, SLOT(updateArrowGeometry())); + head_length_property_ = new rviz_common::properties::FloatProperty( + "Head Length", 0.3f, "Length of the arrow's head, in meters.", this, SLOT(updateArrowGeometry())); - head_radius_property_ = new rviz_common::properties::FloatProperty( - "Head Radius", 0.1f, "Radius of the arrow's head, in meters.", - this, SLOT(updateArrowGeometry())); + head_radius_property_ = new rviz_common::properties::FloatProperty( + "Head Radius", 0.1f, "Radius of the arrow's head, in meters.", this, SLOT(updateArrowGeometry())); - axes_length_property_ = new rviz_common::properties::FloatProperty( - "Axes Length", 1, "Length of each axis, in meters.", - this, SLOT(updateAxisGeometry())); + axes_length_property_ = new rviz_common::properties::FloatProperty( + "Axes Length", 1, "Length of each axis, in meters.", this, SLOT(updateAxisGeometry())); - axes_radius_property_ = new rviz_common::properties::FloatProperty( - "Axes Radius", 0.1f, "Radius of each axis, in meters.", - this, SLOT(updateAxisGeometry())); + axes_radius_property_ = new rviz_common::properties::FloatProperty( + "Axes Radius", 0.1f, "Radius of each axis, in meters.", this, SLOT(updateAxisGeometry())); } MonitorDisplay::~MonitorDisplay() = default; void MonitorDisplay::processMessage(atos_interfaces::msg::Monitor::ConstSharedPtr msg) { - - using rviz_common::validateFloats; - if (!validateFloats(msg->pose) || !validateFloats(msg->velocity.twist)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - - if ( - !context_->getFrameManager()->transform( - msg->pose.header, msg->pose.pose, position, orientation)) - { - setMissingTransformToFixedFrame(msg->pose.header.frame_id); - return; - } - setTransformOk(); - pose_valid_ = true; - updateShapeVisibility(); - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - context_->queueRender(); + + using rviz_common::validateFloats; + if (!validateFloats(msg->pose) || !validateFloats(msg->velocity.twist)) { + setStatus(rviz_common::properties::StatusProperty::Error, + "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + + if (!context_->getFrameManager()->transform(msg->pose.header, msg->pose.pose, position, orientation)) { + setMissingTransformToFixedFrame(msg->pose.header.frame_id); + return; + } + setTransformOk(); + pose_valid_ = true; + updateShapeVisibility(); + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + + context_->queueRender(); } void MonitorDisplay::onInitialize() { - RTDClass::onInitialize(); - - arrow_ = std::make_unique( - scene_manager_, scene_node_, - shaft_length_property_->getFloat(), - shaft_radius_property_->getFloat(), - head_length_property_->getFloat(), - head_radius_property_->getFloat()); - arrow_->setDirection(Ogre::Vector3::UNIT_X); - - axes_ = std::make_unique( - scene_manager_, scene_node_, - axes_length_property_->getFloat(), - axes_radius_property_->getFloat()); - - updateShapeChoice(); - updateColorAndAlpha(); + RTDClass::onInitialize(); + + arrow_ = std::make_unique(scene_manager_, + scene_node_, + shaft_length_property_->getFloat(), + shaft_radius_property_->getFloat(), + head_length_property_->getFloat(), + head_radius_property_->getFloat()); + arrow_->setDirection(Ogre::Vector3::UNIT_X); + + axes_ = std::make_unique( + scene_manager_, scene_node_, axes_length_property_->getFloat(), axes_radius_property_->getFloat()); + + updateShapeChoice(); + updateColorAndAlpha(); } -void MonitorDisplay::updateColorAndAlpha() -{ - Ogre::ColourValue color = color_property_->getOgreColor(); - color.a = alpha_property_->getFloat(); +void MonitorDisplay::updateColorAndAlpha() { + Ogre::ColourValue color = color_property_->getOgreColor(); + color.a = alpha_property_->getFloat(); - arrow_->setColor(color); + arrow_->setColor(color); - context_->queueRender(); + context_->queueRender(); } -void MonitorDisplay::updateArrowGeometry() -{ - arrow_->set( - shaft_length_property_->getFloat(), - shaft_radius_property_->getFloat(), - head_length_property_->getFloat(), - head_radius_property_->getFloat()); - context_->queueRender(); +void MonitorDisplay::updateArrowGeometry() { + arrow_->set(shaft_length_property_->getFloat(), + shaft_radius_property_->getFloat(), + head_length_property_->getFloat(), + head_radius_property_->getFloat()); + context_->queueRender(); } -void MonitorDisplay::updateAxisGeometry() -{ - axes_->set( - axes_length_property_->getFloat(), - axes_radius_property_->getFloat()); - context_->queueRender(); +void MonitorDisplay::updateAxisGeometry() { + axes_->set(axes_length_property_->getFloat(), axes_radius_property_->getFloat()); + context_->queueRender(); } -void MonitorDisplay::updateShapeChoice() -{ - bool use_arrow = (shape_property_->getOptionInt() == Arrow); +void MonitorDisplay::updateShapeChoice() { + bool use_arrow = (shape_property_->getOptionInt() == Arrow); - color_property_->setHidden(!use_arrow); - alpha_property_->setHidden(!use_arrow); - shaft_length_property_->setHidden(!use_arrow); - shaft_radius_property_->setHidden(!use_arrow); - head_length_property_->setHidden(!use_arrow); - head_radius_property_->setHidden(!use_arrow); + color_property_->setHidden(!use_arrow); + alpha_property_->setHidden(!use_arrow); + shaft_length_property_->setHidden(!use_arrow); + shaft_radius_property_->setHidden(!use_arrow); + head_length_property_->setHidden(!use_arrow); + head_radius_property_->setHidden(!use_arrow); - axes_length_property_->setHidden(use_arrow); - axes_radius_property_->setHidden(use_arrow); + axes_length_property_->setHidden(use_arrow); + axes_radius_property_->setHidden(use_arrow); - updateShapeVisibility(); + updateShapeVisibility(); - context_->queueRender(); + context_->queueRender(); } -void MonitorDisplay::updateShapeVisibility() -{ - if (!pose_valid_) { - arrow_->getSceneNode()->setVisible(false); - axes_->getSceneNode()->setVisible(false); - } else { - bool use_arrow = (shape_property_->getOptionInt() == Arrow); - arrow_->getSceneNode()->setVisible(use_arrow); - axes_->getSceneNode()->setVisible(!use_arrow); - } +void MonitorDisplay::updateShapeVisibility() { + if (!pose_valid_) { + arrow_->getSceneNode()->setVisible(false); + axes_->getSceneNode()->setVisible(false); + } else { + bool use_arrow = (shape_property_->getOptionInt() == Arrow); + arrow_->getSceneNode()->setVisible(use_arrow); + axes_->getSceneNode()->setVisible(!use_arrow); + } } -void MonitorDisplay::onEnable() -{ - RTDClass::onEnable(); - updateShapeVisibility(); +void MonitorDisplay::onEnable() { + RTDClass::onEnable(); + updateShapeVisibility(); } -void MonitorDisplay::onDisable() -{ - RTDClass::onDisable(); +void MonitorDisplay::onDisable() { + RTDClass::onDisable(); } -void MonitorDisplay::reset() -{ - RTDClass::reset(); - pose_valid_ = false; - updateShapeVisibility(); +void MonitorDisplay::reset() { + RTDClass::reset(); + pose_valid_ = false; + updateShapeVisibility(); } - -} // namespace atos_rviz_plugins +} // namespace atos_rviz_plugins #include PLUGINLIB_EXPORT_CLASS(atos_rviz_plugins::MonitorDisplay, rviz_common::Display) - - - diff --git a/scripts/integration_testing/run_scenario_test.py b/scripts/integration_testing/run_scenario_test.py index c5188132e..74576d579 100644 --- a/scripts/integration_testing/run_scenario_test.py +++ b/scripts/integration_testing/run_scenario_test.py @@ -1,11 +1,13 @@ import os +import re import signal -import pytest + import launch import launch_pytest -from launch_pytest.tools import process as process_tools -import re import psutil +import pytest +from launch_pytest.tools import process as process_tools + def kill_process_by_name(name, signal): """Kill process by its name. @@ -28,10 +30,10 @@ def integration_test_proc(): """ # Launch a process to test return launch.actions.ExecuteProcess( - cmd=['ros2', 'launch', 'atos', 'launch_integration_testing.py'], + cmd=["ros2", "launch", "atos", "launch_integration_testing.py"], shell=True, cached_output=True, - output='screen' + output="screen", ) @@ -45,11 +47,12 @@ def launch_description(integration_test_proc): Yields: LaunchDescription: Launch description for our test """ - yield launch.LaunchDescription([ - integration_test_proc, - launch_pytest.actions.ReadyToTest() - ]) - kill_process_by_name("ros2", signal.SIGINT) # TODO: Is there a better way to do this? + yield launch.LaunchDescription( + [integration_test_proc, launch_pytest.actions.ReadyToTest()] + ) + kill_process_by_name( + "ros2", signal.SIGINT + ) # TODO: Is there a better way to do this? @pytest.mark.launch(fixture=launch_description) @@ -60,10 +63,18 @@ def test_scenario_execution(integration_test_proc, launch_context): integration_test_proc (Any): Integration test process launch_context (Any): Launch context """ + def validate_scenario_execution(output): # this function can use assertions to validate the output or return a boolean. # pytest generates easier to understand failures when assertions are used. - assert any(re.search('State change result: OK', line) for line in output.splitlines()), 'State change test failed' - assert any(re.search('Trajectory following result: OK', line) for line in output.splitlines()), 'Trajectory check test failed' + assert any( + re.search("State change result: OK", line) for line in output.splitlines() + ), "State change test failed" + assert any( + re.search("Trajectory following result: OK", line) + for line in output.splitlines() + ), "Trajectory check test failed" + process_tools.assert_output_sync( - launch_context, integration_test_proc, validate_scenario_execution, timeout=30) \ No newline at end of file + launch_context, integration_test_proc, validate_scenario_execution, timeout=30 + ) diff --git a/scripts/openscenario/garage_plan_test_scenario.py b/scripts/openscenario/garage_plan_test_scenario.py index 2c4557663..fa7275bd0 100644 --- a/scripts/openscenario/garage_plan_test_scenario.py +++ b/scripts/openscenario/garage_plan_test_scenario.py @@ -1,9 +1,7 @@ #!/usr/bin/env python # coding: utf-8 -import json from scenariogeneration import xosc -import copy # Fixed parameters MONDEO_ID = "1"