From 4a12a8c9ce9884bf94004e79c10aeec73a7bde69 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 10 Apr 2019 17:21:45 +0200 Subject: [PATCH] Implemented RTDEParser --- CMakeLists.txt | 15 ++--- include/ur_rtde_driver/rtde/rtde_package.h | 9 +++ include/ur_rtde_driver/rtde/rtde_parser.h | 68 +++++++++++++++++++--- src/rtde/rtde_package.cpp | 45 ++++++++++++++ 4 files changed, 121 insertions(+), 16 deletions(-) create mode 100644 src/rtde/rtde_package.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 725bcef..9a499a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,14 +82,15 @@ add_library(ur_rtde_driver src/primary/robot_message.cpp src/primary/robot_message/version_message.cpp src/primary/robot_state/kinematics_info.cpp - src/rtde/data_package.cpp - src/rtde/request_protocol_version.cpp - src/rtde/get_urcontrol_version.cpp - src/rtde/text_message.cpp - src/rtde/control_package_setup_outputs.cpp - src/rtde/control_package_setup_inputs.cpp - src/rtde/control_package_start.cpp src/rtde/control_package_pause.cpp + src/rtde/control_package_setup_inputs.cpp + src/rtde/control_package_setup_outputs.cpp + src/rtde/control_package_start.cpp + src/rtde/data_package.cpp + src/rtde/get_urcontrol_version.cpp + src/rtde/request_protocol_version.cpp + src/rtde/rtde_package.cpp + src/rtde/text_message.cpp ) target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES}) add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/include/ur_rtde_driver/rtde/rtde_package.h b/include/ur_rtde_driver/rtde/rtde_package.h index 3d3d70d..10e0bd4 100644 --- a/include/ur_rtde_driver/rtde/rtde_package.h +++ b/include/ur_rtde_driver/rtde/rtde_package.h @@ -46,7 +46,16 @@ public: * \brief Creates a new RTDEPackage object. */ RTDEPackage() = default; + RTDEPackage(const PackageType type) : type_(type) + { + } virtual ~RTDEPackage() = default; + + virtual bool parseWith(comm::BinParser& bp); + virtual std::string toString() const; + +protected: + PackageType type_; }; } // namespace rtde_interface diff --git a/include/ur_rtde_driver/rtde/rtde_parser.h b/include/ur_rtde_driver/rtde/rtde_parser.h index c5109e2..373986b 100644 --- a/include/ur_rtde_driver/rtde/rtde_parser.h +++ b/include/ur_rtde_driver/rtde/rtde_parser.h @@ -21,39 +21,89 @@ #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" +#include "ur_rtde_driver/rtde/data_package.h" +#include "ur_rtde_driver/rtde/text_message.h" +#include "ur_rtde_driver/rtde/request_protocol_version.h" +#include "ur_rtde_driver/rtde/get_urcontrol_version.h" namespace ur_driver { namespace rtde_interface { -using namespace comm; template class RTDEParser : comm::Parser { public: - bool parse(BinParser& bp, std::vector>& results) + RTDEParser() = default; + virtual ~RTDEParser() = default; + + bool parse(comm::BinParser& bp, std::vector>>& results) { - int32_t packet_size = bp.peek(); + PackageHeader::_package_size_type size; + PackageType type; + bp.parse(size); + bp.parse(type); - if (!bp.checkSize(packet_size)) + if (!bp.checkSize(size)) { LOG_ERROR("Buffer len shorter than expected packet length"); return false; } - bp.parse(packet_size); // consumes the peeked data + switch (type) + { + case PackageType::RTDE_DATA_PACKAGE: + { + // TODO: get proper recipe here + std::vector recipe; + std::unique_ptr package(new DataPackage(recipe)); - std::unique_ptr packet(new T); + if (!package->parseWith(bp)) + { + return false; + } + results.push_back(std::move(package)); + break; + } + default: + { + std::unique_ptr package(package_from_type(type)); + if (!package->parseWith(bp)) + { + LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); + return false; + } - if (!packet->parseWith(bp)) - return false; + results.push_back(std::move(package)); + break; + } + } - results.push_back(std::move(packet)); return true; } + +private: + RTDEPackage* package_from_type(PackageType type) + { + switch (type) + { + case PackageType::RTDE_TEXT_MESSAGE: + return new TextMessage; + break; + case PackageType::RTDE_GET_URCONTROL_VERSION: + return new GetUrcontrolVersion; + break; + case PackageType::RTDE_REQUEST_PROTOCOL_VERSION: + return new RequestProtocolVersion; + break; + default: + return new RTDEPackage(type); + } + } }; } // namespace rtde_interface diff --git a/src/rtde/rtde_package.cpp b/src/rtde/rtde_package.cpp new file mode 100644 index 0000000..13e8231 --- /dev/null +++ b/src/rtde/rtde_package.cpp @@ -0,0 +1,45 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-04-10 + * + */ +//---------------------------------------------------------------------- +#include "ur_rtde_driver/rtde/rtde_package.h" + +namespace ur_driver +{ +namespace rtde_interface +{ +bool RTDEPackage::parseWith(comm::BinParser& bp) +{ + return true; +} + +std::string rtde_interface::RTDEPackage ::toString() const +{ + std::stringstream ss; + ss << "Type: " << static_cast(type_); + return ss.str(); +} +} // namespace rtde_interface +} // namespace ur_driver