From 6699c1facb960fb268fb265f9c980c0bf7d09e73 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 9 Apr 2019 11:51:07 +0200 Subject: [PATCH] Added parse and toString functions for higher-level primary functions --- CMakeLists.txt | 2 + .../ur_rtde_driver/primary/primary_package.h | 8 ++- .../ur_rtde_driver/primary/robot_message.h | 19 ++----- .../primary/robot_message/version_message.h | 1 + include/ur_rtde_driver/primary/robot_state.h | 9 ++++ .../primary/robot_state/kinematics_info.h | 2 +- include/ur_rtde_driver/rtde/package_header.h | 3 ++ src/primary/primary_package.cpp | 46 ++++++++++++++++ src/primary/robot_message.cpp | 53 +++++++++++++++++++ src/primary/robot_message/version_message.cpp | 4 +- 10 files changed, 125 insertions(+), 22 deletions(-) create mode 100644 src/primary/primary_package.cpp create mode 100644 src/primary/robot_message.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 7594443..2c7e203 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,6 +75,8 @@ add_library(ur_rtde_driver #src/ur/robot_mode.cpp #src/ur/rt_state.cpp #src/ur/server.cpp + src/primary/primary_package.cpp + src/primary/robot_message.cpp src/primary/robot_message/version_message.cpp src/primary/robot_state/kinematics_info.cpp ) diff --git a/include/ur_rtde_driver/primary/primary_package.h b/include/ur_rtde_driver/primary/primary_package.h index 3936e2e..cf44c00 100644 --- a/include/ur_rtde_driver/primary/primary_package.h +++ b/include/ur_rtde_driver/primary/primary_package.h @@ -35,16 +35,20 @@ namespace ur_driver { namespace primary_interface { -class PrimaryPackage : comm::URPackage +class PrimaryPackage : public comm::URPackage { public: PrimaryPackage() = default; virtual ~PrimaryPackage() = default; + virtual bool parseWith(comm::BinParser& bp); + virtual std::string toString() const; + private: - uint8_t* data_buffer_; + std::string buffer_; }; + } // 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 c54a51b..d462958 100644 --- a/include/ur_rtde_driver/primary/robot_message.h +++ b/include/ur_rtde_driver/primary/robot_message.h @@ -34,20 +34,7 @@ namespace ur_driver { namespace primary_interface { -enum class message_type : 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 : PrimaryPackage +class RobotMessage : public PrimaryPackage { public: RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source) @@ -55,8 +42,8 @@ public: } virtual ~RobotMessage() = default; - virtual bool parseWith(comm::BinParser& bp) = 0; - virtual std::string toString() const = 0; + virtual bool parseWith(comm::BinParser& bp); + virtual std::string toString() const; uint64_t timestamp_; uint8_t source_; diff --git a/include/ur_rtde_driver/primary/robot_message/version_message.h b/include/ur_rtde_driver/primary/robot_message/version_message.h index 92ea25f..9f7a128 100644 --- a/include/ur_rtde_driver/primary/robot_message/version_message.h +++ b/include/ur_rtde_driver/primary/robot_message/version_message.h @@ -40,6 +40,7 @@ public: VersionMessage(uint64_t timestamp, uint8_t source) : RobotMessage(timestamp, source) { } + virtual ~VersionMessage() = default; virtual bool parseWith(comm::BinParser& bp); diff --git a/include/ur_rtde_driver/primary/robot_state.h b/include/ur_rtde_driver/primary/robot_state.h index 48c3f29..fa44bad 100644 --- a/include/ur_rtde_driver/primary/robot_state.h +++ b/include/ur_rtde_driver/primary/robot_state.h @@ -48,6 +48,15 @@ public: RobotState() = default; virtual ~RobotState() = default; + virtual bool parseWith(comm::BinParser& bp) + { + return true; + } + virtual std::string toString() const + { + return std::string(); + } + private: }; diff --git a/include/ur_rtde_driver/primary/robot_state/kinematics_info.h b/include/ur_rtde_driver/primary/robot_state/kinematics_info.h index f8aa4eb..660fb8d 100644 --- a/include/ur_rtde_driver/primary/robot_state/kinematics_info.h +++ b/include/ur_rtde_driver/primary/robot_state/kinematics_info.h @@ -26,7 +26,7 @@ namespace primary_interface * a combination between the perfect model parameters and the correction deltas as noted in the * configuration files on the robot controller. */ -class KinematicsInfo : RobotState +class KinematicsInfo : public RobotState { public: KinematicsInfo() = default; diff --git a/include/ur_rtde_driver/rtde/package_header.h b/include/ur_rtde_driver/rtde/package_header.h index 4fcbdb9..88c268d 100644 --- a/include/ur_rtde_driver/rtde/package_header.h +++ b/include/ur_rtde_driver/rtde/package_header.h @@ -43,6 +43,9 @@ public: virtual ~PackageHeader() = default; using _package_size_type = uint16_t; + PackageHeader(PackageType& type) : package_type_(type){}; + PackageHeader(_package_size_type& size, PackageType& type) : package_size_(size), package_type_(type){}; + static size_t getPackageLength(uint8_t* buf) { return be16toh(*(reinterpret_cast<_package_size_type*>(buf))); diff --git a/src/primary/primary_package.cpp b/src/primary/primary_package.cpp new file mode 100644 index 0000000..0b22f50 --- /dev/null +++ b/src/primary/primary_package.cpp @@ -0,0 +1,46 @@ +// 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-09 + * + */ +//---------------------------------------------------------------------- + +#include "ur_rtde_driver/primary/primary_package.h" + +namespace ur_driver +{ +namespace primary_interface +{ +bool PrimaryPackage::parseWith(comm::BinParser& bp) +{ + bp.parseRemainder(buffer_); + return true; +} + +std::string PrimaryPackage::toString() const +{ + return buffer_; +} + +} // namespace primary_interface +} // namespace ur_driver diff --git a/src/primary/robot_message.cpp b/src/primary/robot_message.cpp new file mode 100644 index 0000000..41c868e --- /dev/null +++ b/src/primary/robot_message.cpp @@ -0,0 +1,53 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_rtde_driver) +// +// 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-09 + * + */ +//---------------------------------------------------------------------- +#include "ur_rtde_driver/primary/robot_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +bool RobotMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(timestamp_); + bp.parse(source_); + bp.parse(message_type_); + + return true; +} + +std::string RobotMessage::toString() const +{ + std::stringstream ss; + ss << "timestamp: " << timestamp_ << std::endl; + ss << "source: " << static_cast(source_) << std::endl; + ss << "message_type: " << static_cast(message_type_) << std::endl; + + return ss.str(); +} + +} // namespace primary_interface +} // namespace ur_driver diff --git a/src/primary/robot_message/version_message.cpp b/src/primary/robot_message/version_message.cpp index 391f472..1ac94c1 100644 --- a/src/primary/robot_message/version_message.cpp +++ b/src/primary/robot_message/version_message.cpp @@ -37,9 +37,7 @@ namespace primary_interface { bool VersionMessage::parseWith(comm::BinParser& bp) { - bp.parse(timestamp_); - bp.parse(source_); - bp.parse(message_type_); + RobotMessage::parseWith(bp); bp.parse(project_name_); bp.parse(major_version_); bp.parse(minor_version_);