From 83125e63c1c2ddcfbc51e96fee43d7232c076637 Mon Sep 17 00:00:00 2001 From: Lea Steffen Date: Mon, 8 Apr 2019 13:57:04 +0200 Subject: [PATCH] Implemented parser functionality --- include/ur_rtde_driver/comm/parser.h | 17 +- .../ur_rtde_driver/primary/package_header.h | 4 +- .../ur_rtde_driver/primary/primary_parser.h | 171 ++++++++++++++++++ .../ur_rtde_driver/primary/robot_message.h | 15 +- include/ur_rtde_driver/primary/robot_state.h | 2 +- include/ur_rtde_driver/rtde/rtde_parser.h | 60 ++++++ 6 files changed, 259 insertions(+), 10 deletions(-) create mode 100644 include/ur_rtde_driver/primary/primary_parser.h create mode 100644 include/ur_rtde_driver/rtde/rtde_parser.h diff --git a/include/ur_rtde_driver/comm/parser.h b/include/ur_rtde_driver/comm/parser.h index 97bc7d6..825c261 100644 --- a/include/ur_rtde_driver/comm/parser.h +++ b/include/ur_rtde_driver/comm/parser.h @@ -18,19 +18,24 @@ #pragma once #include -#include "ur_rtde_driver/bin_parser.h" -#include "ur_rtde_driver/comm/pipeline.h" +#include "ur_rtde_driver/comm/bin_parser.h" +#include "ur_rtde_driver/comm/package.h" namespace ur_driver { namespace comm { -template -class URParser +template +class Parser { public: - virtual bool parse(BinParser& bp, std::vector>& results) = 0; -}; // namespace commtemplateclassURParser + virtual bool parse(BinParser& bp, std::vector>>& results) = 0; + using _header_type = HeaderT; + +private: + HeaderT header_; + // URProducer producer_; +}; } // namespace comm } // namespace ur_driver diff --git a/include/ur_rtde_driver/primary/package_header.h b/include/ur_rtde_driver/primary/package_header.h index 1ab0e1c..55f5844 100644 --- a/include/ur_rtde_driver/primary/package_header.h +++ b/include/ur_rtde_driver/primary/package_header.h @@ -38,7 +38,7 @@ namespace ur_driver { namespace primary_interface { -enum class robot_message_type : int8_t +enum class RobotPackageType : int8_t { DISCONNECT = -1, ROBOT_STATE = 16, @@ -64,7 +64,7 @@ public: private: _package_size_type package_size_; - int8_t package_type_; + RobotPackageType package_type_; }; } // namespace primary_interface } // namespace ur_driver diff --git a/include/ur_rtde_driver/primary/primary_parser.h b/include/ur_rtde_driver/primary/primary_parser.h new file mode 100644 index 0000000..7a91965 --- /dev/null +++ b/include/ur_rtde_driver/primary/primary_parser.h @@ -0,0 +1,171 @@ +/* + * Copyright 2017, 2018 Simon Rasmussen (refactor) + * + * Copyright 2015, 2016 Thomas Timm Andersen (original version) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once +#include +#include "ur_rtde_driver/comm/bin_parser.h" +#include "ur_rtde_driver/comm/pipeline.h" +#include "ur_rtde_driver/comm/parser.h" +#include "ur_rtde_driver/primary/package_header.h" +#include "ur_rtde_driver/primary/robot_state.h" +#include "ur_rtde_driver/primary/robot_message.h" +#include "ur_rtde_driver/primary/robot_state/robot_mode_data.h" +#include "ur_rtde_driver/primary/robot_state/kinematics_info.h" +#include "ur_rtde_driver/primary/robot_state/master_board.h" +#include "ur_rtde_driver/primary/robot_message/version_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +using namespace comm; +class PrimaryParser : comm::Parser +{ +private: + RobotState* state_from_type(robot_state_type type) + { + switch (type) + { + /*case robot_state_type::ROBOT_MODE_DATA: + // SharedRobotModeData* rmd = new SharedRobotModeData(); + + //return new rmd; + case robot_state_type::MASTERBOARD_DATA: + return new MBD;*/ + case robot_state_type::KINEMATICS_INFO: + return new KinematicsInfo; + default: + return nullptr; + } + } + + RobotMessage* message_from_type(message_type type, uint64_t timestamp, uint8_t source) + { + switch (type) + { + /*case robot_state_type::ROBOT_MODE_DATA: + // SharedRobotModeData* rmd = new SharedRobotModeData(); + + //return new rmd; + case robot_state_type::MASTERBOARD_DATA: + return new MBD;*/ + case message_type::ROBOT_MESSAGE_VERSION: + return new VersionMessage(timestamp, source); + default: + return nullptr; + } + } + +public: + bool parse(BinParser& bp, std::vector>& results) + { + int32_t packet_size; + robot_message_type type; + bp.parse(packet_size); + bp.parse(type); + + switch (type) + { + case robot_message_type::ROBOT_STATE: + { + while (!bp.empty()) + { + if (!bp.checkSize(sizeof(uint32_t))) + { + LOG_ERROR("Failed to read sub-package length, there's likely a parsing error"); + return false; + } + uint32_t sub_size = bp.peek(); + if (!bp.checkSize(static_cast(sub_size))) + { + LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size); + return false; + } + + // deconstruction of a sub parser will increment the position of the parent parser + BinParser sbp(bp, sub_size); + sbp.consume(sizeof(sub_size)); + robot_state_type type; + sbp.parse(type); + + std::unique_ptr packet(state_from_type(type)); + + if (packet == nullptr) + { + sbp.consume(); + + // TODO: create robot state type here + continue; + } + + if (!packet->parseWith(sbp)) + { + LOG_ERROR("Sub-package parsing of type %d failed!", static_cast(type)); + return false; + } + + results.push_back(std::move(packet)); + + if (!sbp.empty()) + { + LOG_ERROR("Sub-package of type %d was not parsed completely!", static_cast(type)); + sbp.debug(); + return false; + } + } + + break; + } + + case robot_message_type::ROBOT_MESSAGE: + { + uint64_t timestamp; + uint8_t source; + message_type message_type; + + bp.parse(timestamp); + bp.parse(source); + bp.parse(message_type); + + bool parsed = false; + + std::unique_ptr packet(message_from_type(message_type, timestamp, source)); + if (!packet->parseWith(bp)) + { + LOG_ERROR("Package parsing of type %d failed!", static_cast(message_type)); + return false; + } + + results.push_back(std::move(packet)); + return parsed; + break; + } + + default: + { + LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); + bp.consume(); + return true; + } + } + return true; + } +}; + +} // namespace primary_interface +} // namespace ur_driver diff --git a/include/ur_rtde_driver/primary/robot_message.h b/include/ur_rtde_driver/primary/robot_message.h index d462958..59f84f1 100644 --- a/include/ur_rtde_driver/primary/robot_message.h +++ b/include/ur_rtde_driver/primary/robot_message.h @@ -34,7 +34,20 @@ namespace ur_driver { namespace primary_interface { +enum class RobotMessagePackageType : uint8_t +{ + ROBOT_MESSAGE_TEXT = 0, + ROBOT_MESSAGE_PROGRAM_LABEL = 1, + PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, + ROBOT_MESSAGE_VERSION = 3, + ROBOT_MESSAGE_SAFETY_MODE = 5, + ROBOT_MESSAGE_ERROR_CODE = 6, + ROBOT_MESSAGE_KEY = 7, + ROBOT_MESSAGE_REQUEST_VALUE = 9, + ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 +}; class RobotMessage : public PrimaryPackage + { public: RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source) @@ -47,7 +60,7 @@ public: uint64_t timestamp_; uint8_t source_; - uint8_t message_type_; + RobotMessagePackageType message_type_; }; } // namespace primary_interface diff --git a/include/ur_rtde_driver/primary/robot_state.h b/include/ur_rtde_driver/primary/robot_state.h index fa44bad..f829337 100644 --- a/include/ur_rtde_driver/primary/robot_state.h +++ b/include/ur_rtde_driver/primary/robot_state.h @@ -32,7 +32,7 @@ #include #include "ur_rtde_driver/primary/primary_package.h" -#include "ur_rtde_driver/primary/primary_header.h" +#include "ur_rtde_driver/primary/package_header.h" namespace ur_driver { diff --git a/include/ur_rtde_driver/rtde/rtde_parser.h b/include/ur_rtde_driver/rtde/rtde_parser.h new file mode 100644 index 0000000..c5109e2 --- /dev/null +++ b/include/ur_rtde_driver/rtde/rtde_parser.h @@ -0,0 +1,60 @@ +/* + * Copyright 2017, 2018 Simon Rasmussen (refactor) + * + * Copyright 2015, 2016 Thomas Timm Andersen (original version) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once +#include +#include "ur_rtde_driver/comm/parser.h" +#include "ur_rtde_driver/comm/bin_parser.h" +#include "ur_rtde_driver/comm/pipeline.h" +#include "ur_rtde_driver/rtde/package_header.h" + +namespace ur_driver +{ +namespace rtde_interface +{ +using namespace comm; +template +class RTDEParser : comm::Parser +{ +public: + bool parse(BinParser& bp, std::vector>& results) + + { + int32_t packet_size = bp.peek(); + + if (!bp.checkSize(packet_size)) + { + LOG_ERROR("Buffer len shorter than expected packet length"); + return false; + } + + bp.parse(packet_size); // consumes the peeked data + + std::unique_ptr packet(new T); + + if (!packet->parseWith(bp)) + return false; + + results.push_back(std::move(packet)); + + return true; + } +}; + +} // namespace rtde_interface +} // namespace ur_driver