From 0d355f36041a9551ac873390fed2edc86aa4153e Mon Sep 17 00:00:00 2001 From: Shahrukh <82996646+smartSRA@users.noreply.github.com> Date: Fri, 16 Dec 2022 11:00:06 +0100 Subject: [PATCH] Fix pcl anomaly and timestamp (#27) --- CHANGELOG.md | 6 +- Readme.md | 8 +- simulator/config_umrr9d/com_lib_config.json | 2 +- simulator/simulation/CMakeLists.txt | 4 +- smart_extract.sh | 4 +- umrr_ros2_driver/CMakeLists.txt | 6 +- .../smartmicro_radar_node.hpp | 6 +- umrr_ros2_driver/package.xml | 2 +- .../smartmicro/include/DataServicesIface.h | 16 +- .../smartmicro/include/InstructionBuffer.h | 319 ++---- umrr_ros2_driver/smartmicro/include/Types.h | 1014 +++++++---------- .../src/smartmicro_radar_node.cpp | 12 +- 12 files changed, 597 insertions(+), 802 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a9e0af3..048a45b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,4 +26,8 @@ This release includes the new smartmicro sensor DRVEGRD 152. The driver offers m ## v3.2.0 - 2022-11-11 -This minor release introduces signal-to-noise field in the pointclouds and also fixes the max number of sensors that could be connected at once. \ No newline at end of file +This minor release introduces signal-to-noise field in the pointclouds and also fixes the max number of sensors that could be connected at once. + +## v3.2.1 - 2022-12-16 + +This release fixes the offset which causes anomaly in pointclouds from DRVEGRD 152 sensor. It also fixes the timestamp calculation bug which causes rviz to crash and updates the simulator source files. diff --git a/Readme.md b/Readme.md index 26a1b96..0d4fae8 100644 --- a/Readme.md +++ b/Readme.md @@ -26,18 +26,18 @@ A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVE required to run this node. This code is bundled with a version of Smart Access API. Please make sure the version used to publish the data is compatible with this version: -- Date of release: `November 11, 2022` -- Smart Access Automotive version: `v2.1.0` +- Date of release: `December 16, 2022` +- Smart Access Automotive version: `v2.2.3` - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.1` - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.1` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v1.1.1` -- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.1` +- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.2` ### Sensor Firmwares This ROS2 driver release is compatible with the following sensor firmwares: - UMRR11 Type 132: V5.1.4 - UMRR96 Type 153: V5.2.4 -- UMRR9D Type 152: V1.0.0 +- UMRR9D Type 152: V2.1.0 - UMRR9F Type 169: V1.3.0 ### Point cloud message wrapper library diff --git a/simulator/config_umrr9d/com_lib_config.json b/simulator/config_umrr9d/com_lib_config.json index 069ef60..d71df6d 100644 --- a/simulator/config_umrr9d/com_lib_config.json +++ b/simulator/config_umrr9d/com_lib_config.json @@ -8,7 +8,7 @@ "user_interface_name": "umrr9d_t152_automotive", "user_interface_major_v": 1, "user_interface_minor_v": 0, - "user_interface_patch_v": 1, + "user_interface_patch_v": 2, "download_path": "", "instruction_serialization_type": "port_based", "data_serialization_type": "port_based", diff --git a/simulator/simulation/CMakeLists.txt b/simulator/simulation/CMakeLists.txt index beb6a47..fbd282d 100644 --- a/simulator/simulation/CMakeLists.txt +++ b/simulator/simulation/CMakeLists.txt @@ -18,7 +18,8 @@ target_include_directories(simulator PUBLIC ../../umrr_ros2_driver/smartmicro/include umrr_ros2_driver/smartmicro/include/umrr11_t132_automotive_v1_1_1 umrr_ros2_driver/smartmicro/include/umrr96_t153_automotive_v1_2_1 - umrr_ros2_driver/smartmicro/include/umrr9f_t169_automotive_v1_1_1) + umrr_ros2_driver/smartmicro/include/umrr9f_t169_automotive_v1_1_1 + umrr_ros2_driver/smartmicro/include/umrr9d_t152_automotive_v1_0_2) target_link_libraries(simulator com_lib @@ -26,6 +27,7 @@ target_link_libraries(simulator umrr11_t132_automotivev1.1.1_user_interface umrr96_t153_automotivev1.2.1_user_interface umrr9f_t169_automotivev1.1.1_user_interface + umrr9d_t152_automotivev1.0.2_user_interface dl) install(TARGETS simulator DESTINATION bin) diff --git a/smart_extract.sh b/smart_extract.sh index 04bb9f5..0048d89 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_2_1_0.tar.gz +smart_pack=SmartAccessAutomotive_2_2_3.tgz URL_smartbinaries=https://www.smartmicro.com/fileadmin/media/Downloads/Automotive_Radar/Software/${smart_pack} cat << EOF @@ -42,7 +42,7 @@ echo function getSmartaccessBinaries { wget -c $URL_smartbinaries echo "extracting smart access" - tar xfz $smart_pack -C umrr_ros2_driver/smartmicro/ + tar xfz $smart_pack --strip-components=1 -C umrr_ros2_driver/smartmicro/ } function cleanup { diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 5398087..b33bd5f 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -56,7 +56,7 @@ install(FILES "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr11_t132_automotivev1.1.1_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr96_t153_automotivev1.2.1_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr9f_t169_automotivev1.1.1_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr9d_t152_automotivev1.0.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr9d_t152_automotivev1.0.2_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -81,7 +81,7 @@ smartmicro/include/ smartmicro/include/umrr11_t132_automotive_v1_1_1 smartmicro/include/umrr96_t153_automotive_v1_2_1 smartmicro/include/umrr9f_t169_automotive_v1_1_1 -smartmicro/include/umrr9d_t152_automotive_v1_0_1) +smartmicro/include/umrr9d_t152_automotive_v1_0_2) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -92,7 +92,7 @@ target_link_libraries(smartmicro_radar_node umrr11_t132_automotivev1.1.1_user_interface umrr96_t153_automotivev1.2.1_user_interface umrr9f_t169_automotivev1.1.1_user_interface - umrr9d_t152_automotivev1.0.1_user_interface) + umrr9d_t152_automotivev1.0.2_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp index e462f66..2b4d920 100644 --- a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp +++ b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include #include @@ -136,7 +136,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node void targetlist_callback_umrr9d( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::ComTargetListPort> & + com::master::umrr9d_t152_automotive_v1_0_2::comtargetlistport::ComTargetListPort> & targetlist_port_umrr9d, const com::types::ClientId client_id); @@ -198,7 +198,7 @@ std::shared_ptr m_services{}; std::shared_ptr data_umrr11{}; std::shared_ptr data_umrr96{}; std::shared_ptr data_umrr9f{}; -std::shared_ptr data_umrr9d{}; +std::shared_ptr data_umrr9d{}; } // namespace radar } // namespace drivers diff --git a/umrr_ros2_driver/package.xml b/umrr_ros2_driver/package.xml index 4b02a9b..df0bc97 100644 --- a/umrr_ros2_driver/package.xml +++ b/umrr_ros2_driver/package.xml @@ -2,7 +2,7 @@ umrr_ros2_driver - 3.2.0 + 3.2.1 A node to publish data from a smartmicro radar. s.m.s, smart microwave sensors GmbH. Apache 2.0 License diff --git a/umrr_ros2_driver/smartmicro/include/DataServicesIface.h b/umrr_ros2_driver/smartmicro/include/DataServicesIface.h index 99ddb3b..08525c2 100644 --- a/umrr_ros2_driver/smartmicro/include/DataServicesIface.h +++ b/umrr_ros2_driver/smartmicro/include/DataServicesIface.h @@ -99,9 +99,23 @@ class EXPORT_COM_LIB DataServicesIface : public com::VersionedIface { * Return: ErrorCode * Description: Streams a data port to input client id */ - virtual ErrorCode StreamDataPort(IN ClientId clientId, + virtual ErrorCode StreamDataPort(IN ClientId clientId, IN PortId portId, IN BufferDescriptor& buffer) = 0; + + /* + * Function: StreamInternalDataPort + * Arguments: ClientId clientId - destination client id. + * PortId portId - id of the streamed port. + * BufferDescriptor& buffer - data buffer + * Return: ErrorCode + * Description: Streams a internal data port to input client id. + * This will return with an error code if the port is + * content of client user interface + */ + virtual ErrorCode StreamInternalDataPort( + ClientId clientId, PortId portId, BufferDescriptor& buffer) = 0; + /* * Function: Init * Arguments: none diff --git a/umrr_ros2_driver/smartmicro/include/InstructionBuffer.h b/umrr_ros2_driver/smartmicro/include/InstructionBuffer.h index 7270a3f..36346e7 100644 --- a/umrr_ros2_driver/smartmicro/include/InstructionBuffer.h +++ b/umrr_ros2_driver/smartmicro/include/InstructionBuffer.h @@ -12,8 +12,8 @@ #ifndef COM_COMMON_INSTRUCTION_BUFFER_H #define COM_COMMON_INSTRUCTION_BUFFER_H - #include + #include using namespace com::types; @@ -23,238 +23,169 @@ namespace common { /** @} */ -#define MAX_DIM_ELEMENT 2U /**< @brief Max. numbers of available dimension elements */ -#define INSTR_HANDLER_CMD_DATA_TYPE 6U +#define MAX_DIM_ELEMENT \ + 2U /**< @brief Max. numbers of available dimension elements */ +#define INSTR_HANDLER_CMD_DATA_TYPE 6U enum RequestType { - COM_INSTR_PORT_INVALID = 0U, /**< @brief Instruction Requests is invalid */ - COM_INSTR_PORT_PARAM_SET_PARAMETER = 1U, /**< @brief Instruction Requests for setting a parameter */ - COM_INSTR_PORT_PARAM_GET_PARAMETER = 2U, /**< @brief Instruction Requests for reading a parameter */ - COM_INSTR_PORT_STATUS_GET_STATUS = 3U, /**< @brief Instruction Requests for reading a status */ - COM_INSTR_PORT_CMD = 4U, /**< @brief Instruction Requests for command */ - COM_INSTR_PORT_EXPORT = 5U, /**< @brief Instruction Requests for export param/status */ - COM_INSTR_PORT_RESERVED_0 = 248U, /**< @brief Reserved 0 */ - COM_INSTR_PORT_RESERVED_1 = 249U, /**< @brief Reserved 1 */ - COM_INSTR_PORT_RESERVED_2 = 250U, /**< @brief Reserved 2 */ - COM_INSTR_PORT_RESERVED_3 = 251U, /**< @brief Reserved 3 */ - COM_INSTR_PORT_RESERVED_4 = 252U, /**< @brief Reserved 4 */ - COM_INSTR_PORT_RESERVED_5 = 253U, /**< @brief Reserved 5 */ - COM_INSTR_PORT_RESERVED_6 = 254U, /**< @brief Reserved 6 */ - COM_INSTR_PORT_RESERVED_7 = 255U, /**< @brief Reserved 7 */ - }; - - enum ResposeType { - COM_INSTR_PORT_NO_RESPONSE = 0U, /**< @brief No instruction Response */ - COM_INSTR_PORT_SUCCESS = 1U, /**< @brief Instruction Response was processed successfully */ - COM_INSTR_PORT_ERROR = 2U, /**< @brief General error */ - COM_INSTR_PORT_ERROR_REQUEST = 3U, /**< @brief Invalid request */ - COM_INSTR_PORT_ERROR_SECTION = 4U, /**< @brief Invalid section */ - COM_INSTR_PORT_ERROR_ID = 5U, /**< @brief Invalid id */ - COM_INSTR_PORT_ERROR_PROT = 6U, /**< @brief Invalid protection */ - COM_INSTR_PORT_ERROR_MIN = 7U, /**< @brief Value out of minimal bounds */ - COM_INSTR_PORT_ERROR_MAX = 8U, /**< @brief Value out of maximal bounds */ - COM_INSTR_PORT_ERROR_NAN = 9U, /**< @brief Value is not a number */ - COM_INSTR_PORT_ERROR_TYPE = 10U, /**< @brief Type of Instruction is not valid */ - COM_INSTR_PORT_ERROR_DIM = 11U, /**< @brief Dim of Instruction is not valid */ - COM_INSTR_PORT_ERROR_ELEMENT = 12U, /**< @brief Element of Instruction is not valid */ - COM_INSTR_PORT_ERROR_SIGNATURE = 13U, /**< @brief Signature of Instruction is not valid */ - COM_INSTR_PORT_ERROR_ACCESS_LVL = 14U, /**< @brief Access level is not valid */ - COM_INSTR_PORT_ERROR_INTERNAL = 15U, /**< @brief Internal error shall not be sent */ + COM_INSTR_PORT_INVALID = 0U, /**< @brief Instruction Requests is invalid */ + COM_INSTR_PORT_PARAM_SET_PARAMETER = + 1U, /**< @brief Instruction Requests for setting a parameter */ + COM_INSTR_PORT_PARAM_GET_PARAMETER = + 2U, /**< @brief Instruction Requests for reading a parameter */ + COM_INSTR_PORT_STATUS_GET_STATUS = + 3U, /**< @brief Instruction Requests for reading a status */ + COM_INSTR_PORT_CMD = 4U, /**< @brief Instruction Requests for command */ + COM_INSTR_PORT_EXPORT = + 5U, /**< @brief Instruction Requests for export param/status */ + COM_INSTR_PORT_RESERVED_0 = 248U, /**< @brief Reserved 0 */ + COM_INSTR_PORT_RESERVED_1 = 249U, /**< @brief Reserved 1 */ + COM_INSTR_PORT_RESERVED_2 = 250U, /**< @brief Reserved 2 */ + COM_INSTR_PORT_RESERVED_3 = 251U, /**< @brief Reserved 3 */ + COM_INSTR_PORT_RESERVED_4 = 252U, /**< @brief Reserved 4 */ + COM_INSTR_PORT_RESERVED_5 = 253U, /**< @brief Reserved 5 */ + COM_INSTR_PORT_RESERVED_6 = 254U, /**< @brief Reserved 6 */ + COM_INSTR_PORT_RESERVED_7 = 255U, /**< @brief Reserved 7 */ +}; + +enum ResposeType { + COM_INSTR_PORT_NO_RESPONSE = 0U, /**< @brief No instruction Response */ + COM_INSTR_PORT_SUCCESS = + 1U, /**< @brief Instruction Response was processed successfully */ + COM_INSTR_PORT_ERROR = 2U, /**< @brief General error */ + COM_INSTR_PORT_ERROR_REQUEST = 3U, /**< @brief Invalid request */ + COM_INSTR_PORT_ERROR_SECTION = 4U, /**< @brief Invalid section */ + COM_INSTR_PORT_ERROR_ID = 5U, /**< @brief Invalid id */ + COM_INSTR_PORT_ERROR_PROT = 6U, /**< @brief Invalid protection */ + COM_INSTR_PORT_ERROR_MIN = 7U, /**< @brief Value out of minimal bounds */ + COM_INSTR_PORT_ERROR_MAX = 8U, /**< @brief Value out of maximal bounds */ + COM_INSTR_PORT_ERROR_NAN = 9U, /**< @brief Value is not a number */ + COM_INSTR_PORT_ERROR_TYPE = + 10U, /**< @brief Type of Instruction is not valid */ + COM_INSTR_PORT_ERROR_DIM = 11U, /**< @brief Dim of Instruction is not valid */ + COM_INSTR_PORT_ERROR_ELEMENT = + 12U, /**< @brief Element of Instruction is not valid */ + COM_INSTR_PORT_ERROR_SIGNATURE = + 13U, /**< @brief Signature of Instruction is not valid */ + COM_INSTR_PORT_ERROR_ACCESS_LVL = + 14U, /**< @brief Access level is not valid */ + COM_INSTR_PORT_ERROR_INTERNAL = + 15U, /**< @brief Internal error shall not be sent */ }; class Instruction { -public: - Instruction(uint32_t sectionId, uint32_t id) - : _request(0), - _response(0), - _sectionId(sectionId), - _id(id), - _dataType(0), - _dimCount(0), - _value(0), - _signature(0) - {} - - ~Instruction() - {} - - inline uint8_t GetRequest() const - { - return _request; - } + public: + Instruction(uint32_t sectionId, uint32_t id) + : _request(0), + _response(0), + _sectionId(sectionId), + _id(id), + _dataType(0), + _dimCount(0), + _value(0), + _signature(0) {} - inline void SetRequest(IN uint8_t request) - { - _request = request; - } + ~Instruction() {} - inline uint8_t GetResponse() const - { - return _response; - } + inline uint8_t GetRequest() const { return _request; } - inline void SetResponse(IN uint8_t response) - { - _response = response; - } + inline void SetRequest(IN uint8_t request) { _request = request; } - inline uint16_t GetSectionId() const - { - return _sectionId; - } + inline uint8_t GetResponse() const { return _response; } - inline void SetSectionId(IN uint16_t sectionId) - { - _sectionId = sectionId; - } + inline void SetResponse(IN uint8_t response) { _response = response; } - inline uint16_t GetId() const - { - return _id; - } + inline uint16_t GetSectionId() const { return _sectionId; } - inline void SetId(IN uint16_t id) - { - _id = id; - } + inline void SetSectionId(IN uint16_t sectionId) { _sectionId = sectionId; } - inline uint8_t GetDataType() const - { - return _dataType; - } + inline uint16_t GetId() const { return _id; } - inline void SetDataType(IN uint8_t dataType) - { - _dataType = dataType; - } + inline void SetId(IN uint16_t id) { _id = id; } - inline uint8_t GetDimCount() const - { - return _dimCount; - } + inline uint8_t GetDataType() const { return _dataType; } - inline bool SetDimCount(IN uint8_t dimCount) - { - if(dimCount <= MAX_DIM_ELEMENT) - { - _dimCount = dimCount; - return true; - } - return false; - } + inline void SetDataType(IN uint8_t dataType) { _dataType = dataType; } - inline uint16_t GetDimElement(IN uint8_t index) const - { - return (index < _dimCount ? _dimElement[index] : 0); - } + inline uint8_t GetDimCount() const { return _dimCount; } - inline bool SetDimElement(IN uint8_t index, IN uint16_t element) - { - if(index < _dimCount) - { - _dimElement[index] = element; - return true; - } - return false; + inline bool SetDimCount(IN uint8_t dimCount) { + if (dimCount <= MAX_DIM_ELEMENT) { + _dimCount = dimCount; + return true; } + return false; + } - inline uint32_t GetSignature() const - { - return _signature; - } + inline uint16_t GetDimElement(IN uint8_t index) const { + return (index < _dimCount ? _dimElement[index] : 0); + } - inline void SetSignature(IN uint32_t signature) - { - _signature = signature; + inline bool SetDimElement(IN uint8_t index, IN uint16_t element) { + if (index < _dimCount) { + _dimElement[index] = element; + return true; } + return false; + } - inline uint64_t GetValue() const - { - return _value; - } + inline uint32_t GetSignature() const { return _signature; } - inline void SetValue(IN uint64_t value) - { - _value = value; - } - - -private: - - uint8_t _request; /**< @brief Instruction request command */ - uint8_t _response; /**< @brief Instruction response */ - uint16_t _sectionId; /**< @brief Section number. */ - uint16_t _id; /**< @brief Each instruction has a unique ID */ - uint8_t _dataType; /**< @brief The data type. */ - uint8_t _dimCount; /**< @brief Number of dimensions. - Value 0 for non-array structures. */ - uint16_t _dimElement[MAX_DIM_ELEMENT]; /**< @brief Status element indices addressing. */ - uint32_t _signature; /**< @brief Instruction signature. */ - uint64_t _value; /**< @brief Instruction value. */ - -}; + inline void SetSignature(IN uint32_t signature) { _signature = signature; } + inline uint64_t GetValue() const { return _value; } + inline void SetValue(IN uint64_t value) { _value = value; } + private: + uint8_t _request; /**< @brief Instruction request command */ + uint8_t _response; /**< @brief Instruction response */ + uint16_t _sectionId; /**< @brief Section number. */ + uint16_t _id; /**< @brief Each instruction has a unique ID */ + uint8_t _dataType; /**< @brief The data type. */ + uint8_t _dimCount; /**< @brief Number of dimensions. + Value 0 for non-array structures. */ + uint16_t _dimElement[MAX_DIM_ELEMENT]; /**< @brief Status element indices + addressing. */ + uint32_t _signature; /**< @brief Instruction signature. */ + uint64_t _value; /**< @brief Instruction value. */ +}; class InstructionBuffer { + public: + InstructionBuffer() : _seqCount(0) {} -public: + InstructionBuffer(uint32_t seqCount) : _seqCount(seqCount) {} - InstructionBuffer() - :_seqCount(0) - {} + ~InstructionBuffer() {} - InstructionBuffer(uint32_t seqCount) - :_seqCount(seqCount) - {} + inline void AddInstruction(IN std::shared_ptr inst) { + _instructions.push_back(inst); + } - ~InstructionBuffer() {} + inline std::vector>& GetInstructions() { + return _instructions; + } - inline void AddInstruction(IN std::shared_ptr inst) - { - _instructions.push_back(inst); - } - - inline std::vector>& GetInstructions() - { - return _instructions; - } + inline size_t GetNumOfInstructions() const { return _instructions.size(); } - inline size_t GetNumOfInstructions() const - { - return _instructions.size(); - } - - inline void SetSeqCount(IN SequenceNumber seqCount) - { - _seqCount = seqCount; - } - - inline SequenceNumber GetSeqCount() const - { - return _seqCount; - } + inline void SetSeqCount(IN SequenceNumber seqCount) { _seqCount = seqCount; } - inline void Reset() - { - _instructions.clear(); - } - - void operator=(const InstructionBuffer& rhs) - { - _instructions = rhs._instructions; - _seqCount = rhs._seqCount; - } + inline SequenceNumber GetSeqCount() const { return _seqCount; } + inline void Reset() { _instructions.clear(); } -private: + void operator=(const InstructionBuffer& rhs) { + _instructions = rhs._instructions; + _seqCount = rhs._seqCount; + } - std::vector> _instructions; - uint32_t _seqCount; + private: + std::vector> _instructions; + uint32_t _seqCount; }; -} // common -} // com +} // namespace common +} // namespace com -#endif //COM_COMMON_INSTRUCTION_BUFFER_H +#endif // COM_COMMON_INSTRUCTION_BUFFER_H diff --git a/umrr_ros2_driver/smartmicro/include/Types.h b/umrr_ros2_driver/smartmicro/include/Types.h index 55e2b2a..81076e0 100644 --- a/umrr_ros2_driver/smartmicro/include/Types.h +++ b/umrr_ros2_driver/smartmicro/include/Types.h @@ -12,11 +12,12 @@ #ifndef COM_TYPES_H #define COM_TYPES_H +#include #include #include + #include #include -#include namespace com { @@ -25,701 +26,544 @@ namespace types { const uint8_t MAX_NUM_OF_INST = 255; #ifdef _WIN32 - #define USER_IF_LIB_PREFIX "" - #define USER_IF_LIB_EXT "_user_interface.dll" +#define USER_IF_LIB_PREFIX "" +#define USER_IF_LIB_EXT "_user_interface.dll" #else - #define USER_IF_LIB_PREFIX "lib" - #define USER_IF_LIB_EXT "_user_interface.so" +#define USER_IF_LIB_PREFIX "lib" +#define USER_IF_LIB_EXT "_user_interface.so" #endif -#define CAN_MAX_DATA_BYTES 8 +#define CAN_MAX_DATA_BYTES 8 enum SerializationType { - SERIALIZATION_TYPE_CAN_SPEC = 0, - SERIALIZATION_TYPE_PORT_BASED, - SERIALIZATION_TYPE_UNKNOWN + SERIALIZATION_TYPE_CAN_SPEC = 0, + SERIALIZATION_TYPE_PORT_BASED, + SERIALIZATION_TYPE_UNKNOWN }; -typedef struct -{ - uint16_t u16_identifier; - uint8_t u8_dlc; - uint8_t au8_data[CAN_MAX_DATA_BYTES]; -}CanFormat; +typedef struct { + uint16_t u16_identifier; + uint8_t u8_dlc; + uint8_t au8_data[CAN_MAX_DATA_BYTES]; +} CanFormat; enum LinkType { - LINK_TYPE_UDP = 0, - LINK_TYPE_UDP_DISCOVERY, - LINK_TYPE_CAN, - LINK_TYPE_CAN_DISCOVERY, - LINK_TYPE_RS485, - LIMK_TYPE_UNKNOWN + LINK_TYPE_UDP = 0, + LINK_TYPE_UDP_DISCOVERY, + LINK_TYPE_CAN, + LINK_TYPE_CAN_DISCOVERY, + LINK_TYPE_RS485, + LIMK_TYPE_UNKNOWN }; enum ProtocolType { - PROTOCOL_TYPE_UNKNOWN = 0, - PROTOCOL_TYPE_SMS_CAN_BASE_DATA_V1, - PROTOCOL_TYPE_ATXMEGA_SDLC, - PROTOCOL_TYPE_STG, - PROTOCOL_TYPE_SMS_CAN_BASE_DATA_V2, - PROTOCOL_TYPE_DEBUG, - PROTOCOL_TYPE_LOG_MSG, - PROTOCOL_TYPE_ALIVE, - PROTOCOL_TYPE_PORT, - PROTOCOL_TYPE_INTERVIEW, - PROTOCOL_TYPE_DOWNLOAD, - PROTOCOL_TYPE_TIME_SYNC, - PROTOCOL_TYPE_DATA_STREAM, - PROTOCOL_TYPE_INSTRUCTION + PROTOCOL_TYPE_UNKNOWN = 0, + PROTOCOL_TYPE_SMS_CAN_BASE_DATA_V1, + PROTOCOL_TYPE_ATXMEGA_SDLC, + PROTOCOL_TYPE_STG, + PROTOCOL_TYPE_SMS_CAN_BASE_DATA_V2, + PROTOCOL_TYPE_DEBUG, + PROTOCOL_TYPE_LOG_MSG, + PROTOCOL_TYPE_ALIVE, + PROTOCOL_TYPE_PORT, + PROTOCOL_TYPE_INTERVIEW, + PROTOCOL_TYPE_DOWNLOAD, + PROTOCOL_TYPE_TIME_SYNC, + PROTOCOL_TYPE_DATA_STREAM, + PROTOCOL_TYPE_INSTRUCTION }; enum LibraryRole { - LIBRARY_ROLE_MASTER, - LIBRARY_ROLE_SLAVE, - LIBRARY_ROLE_UNKNOWN + LIBRARY_ROLE_MASTER, + LIBRARY_ROLE_SLAVE, + LIBRARY_ROLE_UNKNOWN }; enum TimeSyncRole { - TIME_SYNC_ROLE_MASTER, - TIME_SYNC_ROLE_SLAVE, - TIME_SYNC_ROLE_UNKNOWN + TIME_SYNC_ROLE_MASTER, + TIME_SYNC_ROLE_SLAVE, + TIME_SYNC_ROLE_UNKNOWN }; - -typedef uint16_t LinkId; -typedef uint32_t SequenceNumber; -typedef uint8_t CanNetId; -typedef uint16_t CanId; -typedef uint16_t UdtId; -typedef uint8_t PhyDeviceId; +typedef uint16_t LinkId; +typedef uint32_t SequenceNumber; +typedef uint8_t CanNetId; +typedef uint16_t CanId; +typedef uint16_t UdtId; +typedef uint8_t PhyDeviceId; const PortId INSTRUCTION_PORT_ID = 46; - class SharedLibDescriptor { -public: - SharedLibDescriptor(const std::string& libName); - virtual ~SharedLibDescriptor(); + public: + SharedLibDescriptor(const std::string& libName); + virtual ~SharedLibDescriptor(); - bool Link(); + bool Link(); - inline void* GetHandle() - { - return _handle; - } + inline void* GetHandle() { return _handle; } -protected: - std::string _libName; - void* _handle; + protected: + std::string _libName; + void* _handle; }; - class ReceiverKey { -public: - ReceiverKey(ClientId clientId,PortId portId); - ReceiverKey(); - ~ReceiverKey(); + public: + ReceiverKey(ClientId clientId, PortId portId); + ReceiverKey(); + ~ReceiverKey(); - ClientId GetClientId() const; - void SetClientId(const ClientId& clientId); + ClientId GetClientId() const; + void SetClientId(const ClientId& clientId); - PortId GetPortId() const; - void SetPortId(const PortId& portId); + PortId GetPortId() const; + void SetPortId(const PortId& portId); - -private: + private: + ClientId _clientId; + PortId _portId; +}; - ClientId _clientId; - PortId _portId; +inline bool operator<(const ReceiverKey& lhs, const ReceiverKey& rhs) { + if ((lhs.GetClientId() < rhs.GetClientId()) || + ((lhs.GetClientId() == rhs.GetClientId()) && + (lhs.GetPortId() < rhs.GetPortId()))) { + return true; + } + return false; +} +class ResponseKey { + public: + ResponseKey(ClientId clientId, SequenceNumber seqNum); + ResponseKey(); + ~ResponseKey(); + + ClientId GetClientId() const; + void SetClientId(IN const ClientId& clientId); + + SequenceNumber GetSequenceNumber() const; + void SetSequenceNumber(IN const SequenceNumber& seqNum); + + private: + ClientId _clientId; + SequenceNumber _seqNum; }; -inline bool operator<(const ReceiverKey& lhs,const ReceiverKey& rhs) -{ - if((lhs.GetClientId() < rhs.GetClientId()) || - ((lhs.GetClientId() == rhs.GetClientId()) && (lhs.GetPortId() < rhs.GetPortId()))) - { - return true; - } - return false; +inline bool operator<(IN const ResponseKey& lhs, IN const ResponseKey& rhs) { + if ((lhs.GetClientId() < rhs.GetClientId()) || + ((lhs.GetClientId() == rhs.GetClientId()) && + (lhs.GetSequenceNumber() < rhs.GetSequenceNumber()))) { + return true; + } + return false; } -class ResponseKey { -public: - ResponseKey(ClientId clientId,SequenceNumber seqNum); - ResponseKey(); - ~ResponseKey(); +class EthNetDescriptor { + public: + EthNetDescriptor(const std::string& ip, uint32_t port, EthTransportType type); - ClientId GetClientId() const; - void SetClientId(IN const ClientId& clientId); + EthNetDescriptor() {} + ~EthNetDescriptor() {} + // ACCESS + void GetIp(OUT std::string& ip) const; + void SetIp(IN const std::string& ip); - SequenceNumber GetSequenceNumber() const; - void SetSequenceNumber(IN const SequenceNumber& seqNum); + uint32_t GetPort() const; + void SetPort(IN uint32_t port); - -private: + EthTransportType GetType() const; + void SetType(IN EthTransportType type); - ClientId _clientId; - SequenceNumber _seqNum; + private: + std::string _ip; + uint32_t _port; + EthTransportType _type; +}; +enum ClientPhyType { + CLIENT_PHY_CAN = 0, + CLIENT_PHY_ETH, + CLIENT_PHY_RS485, + CLIENT_PHY_UNKNOWN }; -inline bool operator<(IN const ResponseKey& lhs,IN const ResponseKey& rhs) -{ - if((lhs.GetClientId() < rhs.GetClientId()) || - ((lhs.GetClientId() == rhs.GetClientId()) && (lhs.GetSequenceNumber() < rhs.GetSequenceNumber()))) - { - return true; - } - return false; -} +class ClientDescriptor { + public: + ClientDescriptor(ClientId clientId, ClientPhyType phyType, + SerializationType instSerialType, + SerializationType dataSerialType, uint8_t phyDevId); + virtual ~ClientDescriptor() {} -class EthNetDescriptor { -public: - EthNetDescriptor(const std::string& ip, - uint32_t port, - EthTransportType type); + inline ClientPhyType GetPhyType() const { return _phyType; } + inline void SetPhyType(IN ClientPhyType phyType) { _phyType = phyType; } - EthNetDescriptor(){} - ~EthNetDescriptor(){} - // ACCESS - void GetIp(OUT std::string& ip) const; - void SetIp(IN const std::string& ip); + inline ClientId GetId() const { return _clientId; } - uint32_t GetPort() const; - void SetPort(IN uint32_t port); + inline void SetId(IN ClientId clientId) { _clientId = clientId; } - EthTransportType GetType() const; - void SetType(IN EthTransportType type); + inline void SetPhyDevId(IN PhyDeviceId phyDevId) { _phyDevId = phyDevId; } + inline SerializationType GetInstSerialType() const { return _instSerialType; } -private: + inline void SetInstSerialType(IN SerializationType type) { + _instSerialType = type; + } - std::string _ip; - uint32_t _port; - EthTransportType _type; + inline SerializationType GetDataSerialType() const { return _dataSerialType; } -}; + inline void SetDataSerialType(IN SerializationType type) { + _dataSerialType = type; + } + inline PhyDeviceId GetPhyDevId() const { return _phyDevId; } -enum ClientPhyType { - CLIENT_PHY_CAN = 0, - CLIENT_PHY_ETH, - CLIENT_PHY_RS485, - CLIENT_PHY_UNKNOWN -}; + inline void SetUserIfName(IN const std::string userIfName) { + _userIfName = userIfName; + } + inline std::string GetUserIfName() const { return _userIfName; } -class ClientDescriptor { -public: - ClientDescriptor(ClientId clientId, - ClientPhyType phyType, - SerializationType instSerialType, - SerializationType dataSerialType, - uint8_t phyDevId); - - virtual ~ClientDescriptor() {} - - inline ClientPhyType GetPhyType() const - { - return _phyType; - } - inline void SetPhyType(IN ClientPhyType phyType) - { - _phyType = phyType; - } - - inline ClientId GetId() const - { - return _clientId; - } - - inline void SetId(IN ClientId clientId) - { - _clientId = clientId; - } - - inline void SetPhyDevId(IN PhyDeviceId phyDevId) - { - _phyDevId = phyDevId; - } - - inline SerializationType GetInstSerialType() const - { - return _instSerialType; - } - - inline void SetInstSerialType(IN SerializationType type) - { - _instSerialType = type; - } - - inline SerializationType GetDataSerialType() const - { - return _dataSerialType; - } - - inline void SetDataSerialType(IN SerializationType type) - { - _dataSerialType = type; - } - - inline PhyDeviceId GetPhyDevId() const - { - return _phyDevId; - } - - inline void SetUserIfName(IN const std::string userIfName) - { - _userIfName = userIfName; - } - - inline std::string GetUserIfName() const - { - return _userIfName; - } - - inline void SetUserIfMajorVer(IN uint32_t majorVer) - { - _userIfMajorVer = majorVer; - } - - inline uint32_t GetUserIfMajorVer() const - { - return _userIfMajorVer; - } - - inline void SetUserIfMinorVer(IN uint32_t minorVer) - { - _userIfMinorVer = minorVer; - } - - inline uint32_t GetUserIfMinorVer() const - { - return _userIfMinorVer; - } - - inline void SetUserIfPatchVer(IN uint32_t patchVer) - { - _userIfPatchVer = patchVer; - } - - inline uint32_t GetUserIfPatchVer() const - { - return _userIfPatchVer; - } - -private: - - ClientId _clientId; - ClientPhyType _phyType; - SerializationType _instSerialType; - SerializationType _dataSerialType; - PhyDeviceId _phyDevId; - std::string _userIfName; - uint32_t _userIfMajorVer; - uint32_t _userIfMinorVer; - uint32_t _userIfPatchVer; + inline void SetUserIfMajorVer(IN uint32_t majorVer) { + _userIfMajorVer = majorVer; + } -}; + inline uint32_t GetUserIfMajorVer() const { return _userIfMajorVer; } + inline void SetUserIfMinorVer(IN uint32_t minorVer) { + _userIfMinorVer = minorVer; + } -class CanClientDescriptor : public ClientDescriptor{ -public: - CanClientDescriptor(ClientId clientId, CanNetId canNetId, PhyDeviceId phyDevId); - virtual ~CanClientDescriptor() {} + inline uint32_t GetUserIfMinorVer() const { return _userIfMinorVer; } - inline CanNetId GetCanNetId() const - { - return _canNetId; - } + inline void SetUserIfPatchVer(IN uint32_t patchVer) { + _userIfPatchVer = patchVer; + } - inline void SetCanNetId(IN CanNetId canNetId) - { - _canNetId = canNetId; - } - -private: - CanNetId _canNetId; + inline uint32_t GetUserIfPatchVer() const { return _userIfPatchVer; } + + private: + ClientId _clientId; + ClientPhyType _phyType; + SerializationType _instSerialType; + SerializationType _dataSerialType; + PhyDeviceId _phyDevId; + std::string _userIfName; + uint32_t _userIfMajorVer; + uint32_t _userIfMinorVer; + uint32_t _userIfPatchVer; }; -class Rs485ClientDescriptor : public ClientDescriptor{ -public: - Rs485ClientDescriptor(ClientId clientId, - SerializationType instSerialType, - SerializationType dataSerialType, - PhyDeviceId phyDevId); - virtual ~Rs485ClientDescriptor() {} +class CanClientDescriptor : public ClientDescriptor { + public: + CanClientDescriptor(ClientId clientId, CanNetId canNetId, + PhyDeviceId phyDevId); + virtual ~CanClientDescriptor() {} -}; + inline CanNetId GetCanNetId() const { return _canNetId; } + inline void SetCanNetId(IN CanNetId canNetId) { _canNetId = canNetId; } -class EthClientDescriptor : public ClientDescriptor{ -public: - EthClientDescriptor(ClientId clientId, - SerializationType instSerialType, - SerializationType dataSerialType); - virtual ~EthClientDescriptor() {} + private: + CanNetId _canNetId; +}; +class Rs485ClientDescriptor : public ClientDescriptor { + public: + Rs485ClientDescriptor(ClientId clientId, SerializationType instSerialType, + SerializationType dataSerialType, PhyDeviceId phyDevId); + virtual ~Rs485ClientDescriptor() {} }; +class EthClientDescriptor : public ClientDescriptor { + public: + EthClientDescriptor(ClientId clientId, SerializationType instSerialType, + SerializationType dataSerialType); + virtual ~EthClientDescriptor() {} +}; +class SerializerConfig { + public: + SerializerConfig(ClientId clientId, SerializationType serialType) + : _clientId(clientId), _serialType(serialType) {} + virtual ~SerializerConfig() {} + inline ClientId GetClientId() const { return _clientId; } -class SerializerConfig { -public: - SerializerConfig(ClientId clientId, SerializationType serialType) - : _clientId(clientId), - _serialType(serialType) - {} + inline SerializationType GetType() const { return _serialType; } - virtual ~SerializerConfig() {} + private: + ClientId _clientId; + SerializationType _serialType; +}; + +class CanSerializerConfig : public SerializerConfig { + public: + CanSerializerConfig(ClientId clientId, LibraryRole role, CanNetId canNetId) + : SerializerConfig(clientId, SERIALIZATION_TYPE_CAN_SPEC), + _canNetId(canNetId), + _role(role) {} - inline ClientId GetClientId() const - { - return _clientId; - } + virtual ~CanSerializerConfig() {} - inline SerializationType GetType() const - { - return _serialType; - } + inline CanNetId GetCanNetId() const { return _canNetId; } -private: + inline LibraryRole GetLibraryRole() const { return _role; } - ClientId _clientId; - SerializationType _serialType; + private: + CanNetId _canNetId; + LibraryRole _role; }; +class PortBasedSerializerConfig : public SerializerConfig { + public: + PortBasedSerializerConfig(ClientId clientId, LibraryRole role) + : SerializerConfig(clientId, SERIALIZATION_TYPE_PORT_BASED), + _role(role) {} + virtual ~PortBasedSerializerConfig() {} + inline LibraryRole GetLibraryRole() const { return _role; } -class CanSerializerConfig : public SerializerConfig { -public: - CanSerializerConfig(ClientId clientId, - LibraryRole role, - CanNetId canNetId) - : SerializerConfig(clientId,SERIALIZATION_TYPE_CAN_SPEC), - _role(role), - _canNetId(canNetId) - {} - - virtual ~CanSerializerConfig() {} - - inline CanNetId GetCanNetId() const - { - return _canNetId; - } - - inline LibraryRole GetLibraryRole() const - { - return _role; - } - -private: - CanNetId _canNetId; - LibraryRole _role; - + private: + LibraryRole _role; }; -class PortBasedSerializerConfig : public SerializerConfig { -public: - PortBasedSerializerConfig(ClientId clientId, - LibraryRole role) - : SerializerConfig(clientId,SERIALIZATION_TYPE_PORT_BASED), - _role(role) - {} - virtual ~PortBasedSerializerConfig() {} - - inline LibraryRole GetLibraryRole() const - { - return _role; - } - -private: - - LibraryRole _role; - +class InterfaceConfig { + public: + InterfaceConfig(LinkType linkType); + virtual ~InterfaceConfig() {} + + LinkType GetLinkType() const; + + private: + LinkType _linkType; }; +class CanInterfaceConfig : public InterfaceConfig { + public: + CanInterfaceConfig(CanNetId canNetId, CanDevId canDevId); + virtual ~CanInterfaceConfig() {} -class InterfaceConfig { -public: - InterfaceConfig(LinkType linkType); - virtual ~InterfaceConfig() {} - - LinkType GetLinkType() const; + CanNetId GetNetId() const; + CanDevId GetDevId() const; -private: + inline void SetUserIfName(IN const std::string& userIfName) { + _userIfName = userIfName; + } - LinkType _linkType; - -}; + inline std::string GetUserIfName() const { return _userIfName; } + inline void SetUserIfMajorVer(IN uint32_t majorVer) { + _userIfMajorVer = majorVer; + } -class CanInterfaceConfig : public InterfaceConfig { -public: - CanInterfaceConfig(CanNetId canNetId, CanDevId canDevId); - virtual ~CanInterfaceConfig() {} - - CanNetId GetNetId() const; - CanDevId GetDevId() const; - - inline void SetUserIfName(IN const std::string& userIfName) - { - _userIfName = userIfName; - } - - inline std::string GetUserIfName() const - { - return _userIfName; - } - - inline void SetUserIfMajorVer(IN uint32_t majorVer) - { - _userIfMajorVer = majorVer; - } - - inline uint32_t GetUserIfMajorVer() const - { - return _userIfMajorVer; - } - - inline void SetUserIfMinorVer(IN uint32_t minorVer) - { - _userIfMinorVer = minorVer; - } - - inline uint32_t GetUserIfMinorVer() const - { - return _userIfMinorVer; - } - - inline void SetUserIfPatchVer(IN uint32_t patchVer) - { - _userIfPatchVer = patchVer; - } - - inline uint32_t GetUserIfPatchVer() const - { - return _userIfPatchVer; - } - - -private: - CanNetId _canNetId; - CanDevId _canDevId; - std::string _userIfName; - uint32_t _userIfMajorVer; - uint32_t _userIfMinorVer; - uint32_t _userIfPatchVer; + inline uint32_t GetUserIfMajorVer() const { return _userIfMajorVer; } + + inline void SetUserIfMinorVer(IN uint32_t minorVer) { + _userIfMinorVer = minorVer; + } + + inline uint32_t GetUserIfMinorVer() const { return _userIfMinorVer; } + + inline void SetUserIfPatchVer(IN uint32_t patchVer) { + _userIfPatchVer = patchVer; + } + + inline uint32_t GetUserIfPatchVer() const { return _userIfPatchVer; } + + private: + CanNetId _canNetId; + CanDevId _canDevId; + std::string _userIfName; + uint32_t _userIfMajorVer; + uint32_t _userIfMinorVer; + uint32_t _userIfPatchVer; }; class Rs485InterfaceConfig : public InterfaceConfig { -public: - Rs485InterfaceConfig(SerialDevId devId); - virtual ~Rs485InterfaceConfig() {} + public: + Rs485InterfaceConfig(SerialDevId devId); + virtual ~Rs485InterfaceConfig() {} + + SerialDevId GetDevId() const; - SerialDevId GetDevId() const; + private: + SerialDevId _devId; +}; -private: +class IpConfig : public InterfaceConfig { + public: + IpConfig(const std::string& ip, uint16_t port, LinkType type) + : InterfaceConfig(type), _ip(ip), _port(port) {} + virtual ~IpConfig() {} - SerialDevId _devId; + inline std::string GetIp() const { return _ip; } + + inline uint16_t GetPort() const { return _port; } + + private: + std::string _ip; + uint16_t _port; }; +class UdpConfig : public IpConfig { + public: + UdpConfig(const std::string& ip, uint16_t port) + : IpConfig(ip, port, LINK_TYPE_UDP) {} + virtual ~UdpConfig() {} +}; -class IpConfig : public InterfaceConfig { -public: - IpConfig(const std::string& ip, uint16_t port, LinkType type) - : InterfaceConfig(type), - _ip(ip), - _port(port) - {} - virtual ~IpConfig() {} - - inline std::string GetIp() const - { - return _ip; - } - - inline uint16_t GetPort() const - { - return _port; - } -private: - - std::string _ip; - uint16_t _port; +class MulticastConfig : public IpConfig { + public: + MulticastConfig(const std::string& ip, uint16_t port) + : IpConfig(ip, port, LINK_TYPE_UDP_DISCOVERY) {} + virtual ~MulticastConfig() {} }; +class ComLibConfig { + public: + ComLibConfig(); + + ComLibConfig(uint16_t majorVer, uint16_t minorVer, uint16_t patchVer); + + ~ComLibConfig(); + // ACCESS + + LibraryRole GetRole() const; + ClientId GetClientId() const; + const std::string& GetConfigPath() const; + const std::string& GetSerialLibPath() const; + const std::string& GetUserInterfaceName() const; + uint16_t GetUserInterfaceMajorVersion() const; + uint16_t GetUserInterfaceMinorVersion() const; + uint16_t GetUserInterfacePatchVersion() const; + const std::string& GetDownloadPath() const; + SerializationType GetInstSerializationType() const; + SerializationType GetDataSerializationType() const; + ClientPhyType GetTimeSyncHwIfaceType() const; + uint8_t GetTimeSyncHwDeviceId() const; + bool IsTimeSyncSupported() const; + bool IsAliveSupported() const; + TimeSyncRole GetTimeSyncRole() const; + uint8_t GetTimeSyncNumOfIter() const; + uint16_t GetMajorVersion() const; + uint16_t GetMinorVersion() const; + uint16_t GetPatchVersion() const; + + void SetRole(IN LibraryRole role); + void SetClientId(IN ClientId clientId); + void SetConfigPath(IN const std::string& path); + void SetSerialLibPath(IN const std::string& path); + void SetUserInterfaceName(IN const std::string& name); + void SetDownloadPath(IN const std::string& path); + void SetDataSerializationType(IN SerializationType type); + void SetInstSerializationType(IN SerializationType type); + void SetTimeSyncHwIfaceType(IN ClientPhyType hwIfType); + void SetTimeSyncHwDeviceId(IN uint8_t deviceId); + void SetTimeSyncSupport(IN bool isSupported); + void SetTimeSyncRole(IN TimeSyncRole role); + void SetTimeSyncNumOfIter(IN uint8_t numOfIter); + void SetAliveSupport(IN bool support); + void SetUserInterfaceMajorVersion(uint16_t majorVer); + void SetUserInterfaceMinorVersion(uint16_t minorVer); + void SetUserInterfacePatchVersion(uint16_t patchVer); + void SetComLibConfigMajorVersion(uint16_t majorVer); + void SetComLibConfigMinorVersion(uint16_t minorVer); + void SetComLibConfigPatchVersion(uint16_t patchVer); + + private: + uint16_t _majorVer; + uint16_t _minorVer; + uint16_t _patchVer; + std::string _serialLibPath; + std::string _configFilesPath; + ClientId _clientId; + LibraryRole _role; + std::string _userIfName; + std::string _downloadPath; + SerializationType _instSerialization; + SerializationType _dataSerialization; + ClientPhyType _timeSyncHwIfaceType; + uint8_t _timeSyncHwDevId; + bool _isTimeSyncSupported; + TimeSyncRole _timeSyncRole; + uint8_t _numOfIter; + bool _isAliveSupported; + uint16_t _userIfMajorVer; + uint16_t _userIfMinorVer; + uint16_t _userIfPatchVer; +}; -class UdpConfig : public IpConfig -{ -public: - UdpConfig(const std::string& ip, uint16_t port) - : IpConfig(ip,port,LINK_TYPE_UDP) - {} - virtual ~UdpConfig() - {} - }; +class SWUpdateConfigData { + public: + SWUpdateConfigData() + : _pathAvailableFlag(false), + _dataPtrValidFlag(false), + _segmentBufferSize(0), + _segmentDataPtr((uint8_t*)0) {} + virtual ~SWUpdateConfigData() {} -class MulticastConfig : public IpConfig -{ -public: - MulticastConfig(const std::string& ip, uint16_t port) - : IpConfig(ip,port,LINK_TYPE_UDP_DISCOVERY) - {} - virtual ~MulticastConfig() - {} - }; + inline void SetData(IN bool pathFlag, IN std::string pathStr, + IN bool dataFlag, IN int32_t dataSize, + IN uint8_t dataEncrypt, IN uint8_t* dataPtr) { + _pathAvailableFlag = pathFlag; + _segmentPath = pathStr; + _dataPtrValidFlag = dataFlag; + _segmentBufferSize = dataSize; + _segmentEncryption = dataEncrypt; + _segmentDataPtr = dataPtr; + } + void cleanData(); -class ComLibConfig { -public: - ComLibConfig(); - - ComLibConfig(uint16_t majorVer, uint16_t minorVer, uint16_t patchVer); - - ~ComLibConfig(); - //ACCESS - - LibraryRole GetRole() const; - ClientId GetClientId() const; - const std::string& GetConfigPath() const; - const std::string& GetSerialLibPath() const; - const std::string& GetUserInterfaceName() const; - uint16_t GetUserInterfaceMajorVersion() const; - uint16_t GetUserInterfaceMinorVersion() const; - uint16_t GetUserInterfacePatchVersion() const; - const std::string& GetDownloadPath() const; - SerializationType GetInstSerializationType() const; - SerializationType GetDataSerializationType() const; - ClientPhyType GetTimeSyncHwIfaceType() const; - uint8_t GetTimeSyncHwDeviceId() const; - bool IsTimeSyncSupported() const; - bool IsAliveSupported() const; - TimeSyncRole GetTimeSyncRole() const; - uint8_t GetTimeSyncNumOfIter() const; - uint16_t GetMajorVersion() const; - uint16_t GetMinorVersion() const; - uint16_t GetPatchVersion() const; - - - void SetRole(IN LibraryRole role); - void SetClientId(IN ClientId clientId); - void SetConfigPath(IN const std::string& path); - void SetSerialLibPath(IN const std::string& path); - void SetUserInterfaceName(IN const std::string& name); - void SetDownloadPath(IN const std::string& path); - void SetDataSerializationType(IN SerializationType type); - void SetInstSerializationType(IN SerializationType type); - void SetTimeSyncHwIfaceType(IN ClientPhyType hwIfType); - void SetTimeSyncHwDeviceId(IN uint8_t deviceId); - void SetTimeSyncSupport(IN bool isSupported); - void SetTimeSyncRole(IN TimeSyncRole role); - void SetTimeSyncNumOfIter(IN uint8_t numOfIter); - void SetAliveSupport(IN bool support); - void SetUserInterfaceMajorVersion(uint16_t majorVer); - void SetUserInterfaceMinorVersion(uint16_t minorVer); - void SetUserInterfacePatchVersion(uint16_t patchVer); - void SetComLibConfigMajorVersion(uint16_t majorVer); - void SetComLibConfigMinorVersion(uint16_t minorVer); - void SetComLibConfigPatchVersion(uint16_t patchVer); - -private: - uint16_t _majorVer; - uint16_t _minorVer; - uint16_t _patchVer; - std::string _serialLibPath; - std::string _configFilesPath; - ClientId _clientId; - LibraryRole _role; - std::string _userIfName; - std::string _downloadPath; - SerializationType _instSerialization; - SerializationType _dataSerialization; - ClientPhyType _timeSyncHwIfaceType; - uint8_t _timeSyncHwDevId; - bool _isTimeSyncSupported; - TimeSyncRole _timeSyncRole; - uint8_t _numOfIter; - bool _isAliveSupported; - uint16_t _userIfMajorVer; - uint16_t _userIfMinorVer; - uint16_t _userIfPatchVer; + inline bool GetPathAvailableFlag() const { return _pathAvailableFlag; } + + inline std::string GetSegmentFilePath() const { return _segmentPath; } + inline bool GetDataPtrValidFlag() const { return _dataPtrValidFlag; } + + inline uint8_t GetSegmentEncryption() const { return _segmentEncryption; } + + inline int32_t GetSegmentBufferSize() const { return _segmentBufferSize; } + + inline uint8_t* GetSegmentDataPtr() { return _segmentDataPtr; } + + private: + bool _pathAvailableFlag; + std::string _segmentPath; + bool _dataPtrValidFlag; + int32_t _segmentBufferSize; + uint8_t _segmentEncryption; + uint8_t* _segmentDataPtr; }; -class SWUpdateConfigData -{ -public: - SWUpdateConfigData() - : - _pathAvailableFlag(false), - _dataPtrValidFlag(false), - _segmentBufferSize(0), - _segmentDataPtr((uint8_t *)0) - { - } - virtual ~SWUpdateConfigData() - { - } - - inline void SetData(IN bool pathFlag, IN std::string pathStr, - IN bool dataFlag, IN int32_t dataSize, - IN uint8_t dataEncrypt, - IN uint8_t *dataPtr) - { - _pathAvailableFlag = pathFlag; - _segmentPath = pathStr; - _dataPtrValidFlag = dataFlag; - _segmentBufferSize = dataSize; - _segmentEncryption = dataEncrypt; - _segmentDataPtr = dataPtr; - } - - void cleanData(); - - inline bool GetPathAvailableFlag() const - { - return _pathAvailableFlag; - } - - inline std::string GetSegmentFilePath() const - { - return _segmentPath; - } - - inline bool GetDataPtrValidFlag() const - { - return _dataPtrValidFlag; - } - - inline uint8_t GetSegmentEncryption() const - { - return _segmentEncryption; - } - - inline int32_t GetSegmentBufferSize() const - { - return _segmentBufferSize; - } - - inline uint8_t *GetSegmentDataPtr() - { - return _segmentDataPtr; - } - -private: - bool _pathAvailableFlag; - std::string _segmentPath; - bool _dataPtrValidFlag; - int32_t _segmentBufferSize; - uint8_t _segmentEncryption; - uint8_t *_segmentDataPtr; +class InternalTransportHeader { + public: + InternalTransportHeader(); + ~InternalTransportHeader(); + + com::types::ProtocolType GetProtocolType() const; + void SetProtocolType(IN com::types::ProtocolType protoType); + + bool HasSrcClientId() const; + ClientId GetSrcClientId() const; + void SetSrcClientId(IN ClientId clientId); + + bool HasDstClientId() const; + ClientId GetDstClientId() const; + void SetDstClientId(IN ClientId clientId); + + private: + com::types::ProtocolType _protoType; + ClientId _srcClientId; + bool _srcClientIdValid; + ClientId _dstClientId; + bool _dstClientIdValid; }; -} // types -} // com +} // namespace types +} // namespace com -#endif //COM_TYPES_H +#endif // COM_TYPES_H diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index cd8943d..425ec41 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -23,8 +23,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -143,7 +143,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option data_umrr11 = com::master::umrr11_t132_automotive_v1_1_1::DataStreamServiceIface::Get(); data_umrr96 = com::master::umrr96_t153_automotive_v1_2_1::DataStreamServiceIface::Get(); data_umrr9f = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); - data_umrr9d = com::master::umrr9d_t152_automotive_v1_0_1::DataStreamServiceIface::Get(); + data_umrr9d = com::master::umrr9d_t152_automotive_v1_0_2::DataStreamServiceIface::Get(); RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); // Wait init time @@ -498,20 +498,20 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f( void SmartmicroRadarNode::targetlist_callback_umrr9d( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::ComTargetListPort> & + com::master::umrr9d_t152_automotive_v1_0_2::comtargetlistport::ComTargetListPort> & targetlist_port_umrr9d, const com::types::ClientId client_id) { std::cout << "Targetlist callback is being called for umrr9d" << std::endl; if (!check_signal) { std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::GenericPortHeader> + com::master::umrr9d_t152_automotive_v1_0_2::comtargetlistport::GenericPortHeader> port_header; port_header = targetlist_port_umrr9d->GetGenericPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; - const auto sec = std::chrono::duration_cast(timestamp); + const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count();