diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index eb89d7cf5..a1153220c 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -83,15 +83,15 @@ include_directories( add_library(ur_robot_driver src/comm/tcp_socket.cpp src/comm/server.cpp - #src/ros/service_stopper.cpp - #src/ur/commander.cpp - #src/ur/master_board.cpp - #src/ur/messages.cpp - #src/ur/robot_mode.cpp + src/primary/primary_client.cpp src/primary/primary_package.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp + src/primary/robot_message/error_code_message.cpp + src/primary/robot_message/runtime_exception_message.cpp src/primary/robot_message/version_message.cpp + src/primary/robot_message/text_message.cpp + src/primary/robot_message/key_message.cpp src/primary/robot_state/kinematics_info.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp diff --git a/ur_robot_driver/include/ur_robot_driver/comm/producer.h b/ur_robot_driver/include/ur_robot_driver/comm/producer.h index 32ba2ed3d..050080fed 100644 --- a/ur_robot_driver/include/ur_robot_driver/comm/producer.h +++ b/ur_robot_driver/include/ur_robot_driver/comm/producer.h @@ -57,6 +57,8 @@ class URProducer : public IProducer { } + virtual ~URProducer() = default; + /*! * \brief Triggers the stream to connect to the robot. */ diff --git a/ur_robot_driver/include/ur_robot_driver/primary/abstract_primary_consumer.h b/ur_robot_driver/include/ur_robot_driver/primary/abstract_primary_consumer.h index 39cc2cb84..7dbe5323d 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/abstract_primary_consumer.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/abstract_primary_consumer.h @@ -30,6 +30,10 @@ #include "ur_robot_driver/log.h" #include "ur_robot_driver/comm/pipeline.h" +#include "ur_robot_driver/primary/robot_message/error_code_message.h" +#include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_robot_driver/primary/robot_message/runtime_exception_message.h" +#include "ur_robot_driver/primary/robot_message/text_message.h" #include "ur_robot_driver/primary/robot_message/version_message.h" #include "ur_robot_driver/primary/robot_state/kinematics_info.h" @@ -70,6 +74,10 @@ class AbstractPrimaryConsumer : public comm::IConsumer // To be implemented in specific consumers virtual bool consume(RobotMessage& pkg) = 0; virtual bool consume(RobotState& pkg) = 0; + virtual bool consume(ErrorCodeMessage& pkg) = 0; + virtual bool consume(KeyMessage& pkg) = 0; + virtual bool consume(RuntimeExceptionMessage& pkg) = 0; + virtual bool consume(TextMessage& pkg) = 0; virtual bool consume(VersionMessage& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; diff --git a/ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h b/ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h new file mode 100644 index 000000000..0e48846ee --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h @@ -0,0 +1,60 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED +#define UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace ur_driver +{ +namespace primary_interface +{ +class KeyMessageHandler : public IPrimaryPackageHandler +{ +public: + KeyMessageHandler() = default; + virtual ~KeyMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(KeyMessage& pkg) override + { + LOG_INFO("---KeyMessage---\n%s", pkg.toString().c_str()); + } + +private: + /* data */ +}; +} // namespace primary_interface +} // namespace ur_driver +#endif // ifndef UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_client.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_client.h new file mode 100644 index 000000000..d01f7cd71 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/primary/primary_client.h @@ -0,0 +1,79 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- +#ifndef UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED +#define UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace ur_driver +{ +namespace primary_interface +{ +class PrimaryClient +{ +public: + PrimaryClient() = delete; + PrimaryClient(const std::string& robot_ip, const std::string& calibration_checksum); + virtual ~PrimaryClient() = default; + + /*! + * \brief Sends a custom script program to the robot. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param script_code URScript code that shall be executed by the robot. + * + * \returns true on successful upload, false otherwise. + */ + bool sendScript(const std::string& script_code); + + /*! + * \brief Checks if the kinematics information in the used model fits the actual robot. + * + * \param checksum Hash of the used kinematics information + */ + void checkCalibration(const std::string& checksum); + +private: + std::string robot_ip_; + PrimaryParser parser_; + std::unique_ptr consumer_; + comm::INotifier notifier_; + std::unique_ptr> producer_; + std::unique_ptr> stream_; + std::unique_ptr> pipeline_; +}; + +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h new file mode 100644 index 000000000..fafe07501 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h @@ -0,0 +1,149 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED +#define UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED + +#include "ur_robot_driver/log.h" +#include "ur_robot_driver/comm/pipeline.h" +#include "ur_robot_driver/primary/primary_package_handler.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" +#include "ur_robot_driver/primary/key_message_handler.h" +#include "ur_robot_driver/primary/error_code_message_handler.h" +#include "ur_robot_driver/primary/robot_message/error_code_message.h" +#include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_robot_driver/primary/robot_message/runtime_exception_message.h" +#include "ur_robot_driver/primary/robot_message/text_message.h" +#include "ur_robot_driver/primary/robot_message/version_message.h" +#include "ur_robot_driver/primary/robot_state/kinematics_info.h" + +namespace ur_driver +{ +namespace primary_interface +{ +/*! + * \brief Primary consumer implementation + * + * This class implements am AbstractPrimaryConsumer such that it can consume all incoming primary + * messages. However, actual work will be done by workers for each specific type. + */ +class PrimaryConsumer : public AbstractPrimaryConsumer +{ +public: + PrimaryConsumer() + { + LOG_INFO("Constructing primary consumer"); + key_message_worker_.reset(new KeyMessageHandler()); + error_code_message_worker_.reset(new ErrorCodeMessageHandler()); + LOG_INFO("Constructed primary consumer"); + } + virtual ~PrimaryConsumer() = default; + + virtual bool consume(RobotMessage& msg) override + { + LOG_INFO("---RobotMessage:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RobotState& msg) override + { + // LOG_INFO("---RobotState:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RuntimeExceptionMessage& msg) override + { + LOG_INFO("---RuntimeExceptionMessage---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(TextMessage& msg) override + { + LOG_INFO("---TextMessage---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(VersionMessage& msg) override + { + LOG_INFO("---VersionMessage---\n%s", msg.toString().c_str()); + return true; + } + + /*! + * \brief Handle a KinematicsInfo + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(KinematicsInfo& pkg) override + { + if (kinematics_info_message_worker_ != nullptr) + { + kinematics_info_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a KeyMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(KeyMessage& pkg) override + { + if (key_message_worker_ != nullptr) + { + key_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a ErrorCodeMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(ErrorCodeMessage& pkg) override + { + if (error_code_message_worker_ != nullptr) + { + error_code_message_worker_->handle(pkg); + return true; + } + return false; + } + void setKinematicsInfoHandler(const std::shared_ptr>& handler) + { + kinematics_info_message_worker_ = handler; + } + +private: + std::shared_ptr> key_message_worker_; + std::shared_ptr> error_code_message_worker_; + std::shared_ptr> kinematics_info_message_worker_; +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_package_handler.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_package_handler.h new file mode 100644 index 000000000..25b3fb624 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/primary/primary_package_handler.h @@ -0,0 +1,58 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED +#define UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED + +namespace ur_driver +{ +namespace primary_interface +{ +/*! + * \brief Interface for a class handling a primary interface package. Classes that implement this + * interface with a specific package type will be able to handle packages of this type. + */ +template +class IPrimaryPackageHandler +{ +public: + IPrimaryPackageHandler() = default; + virtual ~IPrimaryPackageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(PackageT& pkg) = 0; + +private: + /* data */ +}; +} // namespace primary_interface +} // namespace ur_driver +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h index c609c0f88..ec0b6158a 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h @@ -27,6 +27,10 @@ #include "ur_robot_driver/primary/robot_state.h" #include "ur_robot_driver/primary/robot_message.h" #include "ur_robot_driver/primary/robot_state/kinematics_info.h" +#include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_robot_driver/primary/robot_message/error_code_message.h" +#include "ur_robot_driver/primary/robot_message/runtime_exception_message.h" +#include "ur_robot_driver/primary/robot_message/text_message.h" #include "ur_robot_driver/primary/robot_message/version_message.h" namespace ur_driver @@ -115,7 +119,7 @@ class PrimaryParser : public comm::Parser case RobotPackageType::ROBOT_MESSAGE: { uint64_t timestamp; - uint8_t source; + int8_t source; RobotMessagePackageType message_type; bp.parse(timestamp); @@ -174,8 +178,16 @@ class PrimaryParser : public comm::Parser return new MBD;*/ case RobotMessagePackageType::ROBOT_MESSAGE_VERSION: return new VersionMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_TEXT: + return new TextMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_KEY: + return new KeyMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE: + return new ErrorCodeMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION: + return new RuntimeExceptionMessage(timestamp, source); default: - return new RobotMessage(timestamp, source); + return new RobotMessage(timestamp, source, type); } } }; diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_shell_consumer.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_shell_consumer.h new file mode 100644 index 000000000..f829437e0 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/primary/primary_shell_consumer.h @@ -0,0 +1,92 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED +#define UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED + +#include "ur_robot_driver/log.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" + +namespace ur_driver +{ +namespace primary_interface +{ +class PrimaryShellConsumer : public AbstractPrimaryConsumer +{ +public: + PrimaryShellConsumer() = default; + virtual ~PrimaryShellConsumer() = default; + + virtual bool consume(RobotMessage& msg) override + { + LOG_INFO("---RobotMessage:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RobotState& msg) override + { + //LOG_INFO("---RobotState:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(ErrorCodeMessage& msg) override + { + LOG_INFO("---ErrorCodeMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(KeyMessage& msg) override + { + LOG_INFO("---KeyMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RuntimeExceptionMessage& msg) override + { + LOG_INFO("---RuntimeExceptionMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(TextMessage& msg) override + { + LOG_INFO("---TextMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(VersionMessage& msg) override + { + LOG_INFO("---VersionMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(KinematicsInfo& msg) override + { + LOG_INFO("%s", msg.toString().c_str()); + return true; + } + +private: + /* data */ +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h index a27f9cc69..a1224df76 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h @@ -37,7 +37,7 @@ namespace primary_interface /*! * \brief Possible RobotMessage types */ -enum class RobotMessagePackageType : uint8_t +enum class RobotMessagePackageType : int8_t { ROBOT_MESSAGE_TEXT = 0, ROBOT_MESSAGE_PROGRAM_LABEL = 1, @@ -62,7 +62,19 @@ class RobotMessage : public PrimaryPackage * \param timestamp Timestamp of the package * \param source The package's source */ - RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source) + RobotMessage(const uint64_t timestamp, const int8_t source) : timestamp_(timestamp), source_(source) + { + } + + /*! + * \brief Creates a new RobotMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + * \param message_type The package's message type + */ + RobotMessage(const uint64_t timestamp, const int8_t source, const RobotMessagePackageType message_type) + : timestamp_(timestamp), source_(source), message_type_(message_type) { } virtual ~RobotMessage() = default; @@ -94,7 +106,7 @@ class RobotMessage : public PrimaryPackage virtual std::string toString() const; uint64_t timestamp_; - uint8_t source_; + int8_t source_; RobotMessagePackageType message_type_; }; diff --git a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h new file mode 100644 index 000000000..f2ca2b6e3 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h @@ -0,0 +1,106 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED +#define UR_RTDE_DRIVER_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED + +#include "ur_robot_driver/primary/robot_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +enum class ReportLevel : int32_t +{ + DEBUG = 0, + INFO = 1, + WARNING = 2, + VIOLATION = 3, + FAULT = 4, + DEVL_DEBUG = 128, + DEVL_INFO = 129, + DEVL_WARNING = 130, + DEVL_VIOLATION = 131, + DEVL_FAULT = 132 +}; + +/*! + * \brief The ErrorCodeMessage class handles the error code messages sent via the primary UR interface. + */ +class ErrorCodeMessage : public RobotMessage +{ +public: + ErrorCodeMessage() = delete; + /*! + * \brief Creates a new ErrorCodeMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + ErrorCodeMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE) + { + } + virtual ~ErrorCodeMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t message_code_; + int32_t message_argument_; + ReportLevel report_level_; + uint8_t data_type_; + uint32_t data_; + std::string text_; +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/key_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/key_message.h new file mode 100644 index 000000000..590f8abeb --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/key_message.h @@ -0,0 +1,91 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_PRIMARY_KEY_MESSAGE_H_INCLUDED +#define UR_RTDE_DRIVER_PRIMARY_KEY_MESSAGE_H_INCLUDED + +#include "ur_robot_driver/primary/robot_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +/*! + * \brief The KeyMessage class handles the key messages sent via the primary UR interface. + */ +class KeyMessage : public RobotMessage +{ +public: + KeyMessage() = delete; + /*! + * \brief Creates a new KeyMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + KeyMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_KEY) + { + } + virtual ~KeyMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t message_code_; + int32_t message_argument_; + uint8_t title_size_; + std::string title_; + std::string text_; +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/runtime_exception_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/runtime_exception_message.h new file mode 100644 index 000000000..d03295b26 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/runtime_exception_message.h @@ -0,0 +1,89 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED +#define UR_RTDE_DRIVER_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED + +#include "ur_robot_driver/primary/robot_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +/*! + * \brief The RuntimeExceptionMessage class handles the runtime exception messages sent via the primary UR interface. + */ +class RuntimeExceptionMessage : public RobotMessage +{ +public: + RuntimeExceptionMessage() = delete; + /*! + * \brief Creates a new RuntimeExceptionMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + RuntimeExceptionMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION) + { + } + virtual ~RuntimeExceptionMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t line_number_; + int32_t column_number_; + std::string text_; +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/text_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/text_message.h new file mode 100644 index 000000000..70e264712 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/text_message.h @@ -0,0 +1,87 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#define UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED + +#include "ur_robot_driver/primary/robot_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +/*! + * \brief The TextMessage class handles the text messages sent via the primary UR interface. + */ +class TextMessage : public RobotMessage +{ +public: + TextMessage() = delete; + /*! + * \brief Creates a new TextMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + TextMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_TEXT) + { + } + virtual ~TextMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + std::string text_; +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h index 73abc09ad..e166cbd6a 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h @@ -47,7 +47,8 @@ class VersionMessage : public RobotMessage * \param timestamp Timestamp of the package * \param source The package's source */ - VersionMessage(uint64_t timestamp, uint8_t source) : RobotMessage(timestamp, source) + VersionMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_VERSION) { } virtual ~VersionMessage() = default; diff --git a/ur_robot_driver/include/ur_robot_driver/ur/calibration_checker.h b/ur_robot_driver/include/ur_robot_driver/ur/calibration_checker.h index 559f0f624..f1e49077f 100644 --- a/ur_robot_driver/include/ur_robot_driver/ur/calibration_checker.h +++ b/ur_robot_driver/include/ur_robot_driver/ur/calibration_checker.h @@ -27,18 +27,17 @@ #ifndef UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED #define UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED -#include - +#include #include namespace ur_driver { /*! - * \brief The CalibrationChecker class consumes primary packages ignoring all but KinematicsInfo - * packages. These are then checked against the used kinematics to see if the correct calibration - * is used. + * \brief The CalibrationChecker checks a received KinematicsInfo package against a registered calibration hash + * value. This way we know whether the robot that sent the KinematicsInfo package matches the + * expected calibration. */ -class CalibrationChecker : public comm::IConsumer +class CalibrationChecker : public primary_interface::IPrimaryPackageHandler { public: /*! @@ -50,31 +49,6 @@ class CalibrationChecker : public comm::IConsumer product); + virtual void handle(primary_interface::KinematicsInfo& kin_info) override; /*! * \brief Used to make sure the calibration check is not performed several times. diff --git a/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h b/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h index 2bd06449a..2e991a5df 100644 --- a/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h +++ b/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h @@ -33,6 +33,7 @@ #include "ur_robot_driver/ur/tool_communication.h" #include "ur_robot_driver/ur/version_information.h" #include "ur_robot_driver/primary/robot_message/version_message.h" +#include "ur_robot_driver/primary/primary_client.h" #include "ur_robot_driver/rtde/rtde_writer.h" namespace ur_driver @@ -165,7 +166,7 @@ class UrDriver * * \param checksum Hash of the used kinematics information */ - void checkCalibration(const std::string& checksum); + //void checkCalibration(const std::string& checksum); /*! * \brief Getter for the RTDE writer used to write to the robot's RTDE interface. @@ -211,8 +212,10 @@ class UrDriver std::unique_ptr rtde_client_; std::unique_ptr reverse_interface_; std::unique_ptr script_sender_; - std::unique_ptr> primary_stream_; - std::unique_ptr> secondary_stream_; + + primary_interface::PrimaryClient primary_client_; + //std::unique_ptr> primary_stream_; + //std::unique_ptr> secondary_stream_; double servoj_time_; uint32_t servoj_gain_; diff --git a/ur_robot_driver/src/primary/primary_client.cpp b/ur_robot_driver/src/primary/primary_client.cpp new file mode 100644 index 000000000..c7e7679da --- /dev/null +++ b/ur_robot_driver/src/primary/primary_client.cpp @@ -0,0 +1,99 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include +#include + +#include +#include + +namespace ur_driver +{ +namespace primary_interface +{ +PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& calibration_checksum) : robot_ip_(robot_ip) +{ + stream_.reset(new comm::URStream(robot_ip_, UR_PRIMARY_PORT)); + producer_.reset(new comm::URProducer(*stream_, parser_)); + producer_->setupProducer(); + + consumer_.reset(new PrimaryConsumer()); + std::shared_ptr calibration_checker(new CalibrationChecker(calibration_checksum)); + consumer_->setKinematicsInfoHandler(calibration_checker); + + pipeline_.reset(new comm::Pipeline(*producer_, consumer_.get(), "primary pipeline", notifier_)); + pipeline_->run(); + + calibration_checker->isChecked(); + while (!calibration_checker->isChecked()) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + LOG_DEBUG("Got calibration information from robot."); +} + +bool PrimaryClient::sendScript(const std::string& script_code) +{ + if (stream_ == nullptr) + { + throw std::runtime_error("Sending script to robot requested while there is no primary interface established. This " + "should not happen."); + } + + // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will + // not execute them. To avoid problems, we always just append a newline here, even if + // there may already be one. + auto program_with_newline = script_code + '\n'; + + size_t len = program_with_newline.size(); + const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); + size_t written; + + if (stream_->write(data, len, written)) + { + LOG_INFO("Sent program to robot:\n%s", program_with_newline.c_str()); + return true; + } + LOG_ERROR("Could not send program to robot"); + return false; +} + +void PrimaryClient::checkCalibration(const std::string& checksum) +{ + // if (stream_ == nullptr) + //{ + // throw std::runtime_error("checkCalibration() called without a primary interface connection being established."); + //} + + // while (!consumer.isChecked()) + //{ + // ros::Duration(1).sleep(); + //} + // ROS_DEBUG_STREAM("Got calibration information from robot."); +} +} // namespace primary_interface +} // namespace ur_driver diff --git a/ur_robot_driver/src/primary/robot_message/error_code_message.cpp b/ur_robot_driver/src/primary/robot_message/error_code_message.cpp new file mode 100644 index 000000000..11668f31f --- /dev/null +++ b/ur_robot_driver/src/primary/robot_message/error_code_message.cpp @@ -0,0 +1,70 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include "ur_robot_driver/log.h" +#include "ur_robot_driver/primary/robot_message/error_code_message.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" + +namespace ur_driver +{ +namespace primary_interface +{ +bool ErrorCodeMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(message_code_); + bp.parse(message_argument_); + int32_t report_level; + bp.parse(report_level); + report_level_ = static_cast(report_level); + bp.parse(data_type_); + bp.parse(data_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool ErrorCodeMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string ErrorCodeMessage::toString() const +{ + std::stringstream ss; + ss << "Message code: " << message_code_ << std::endl; + ss << "Message argument: " << message_argument_ << std::endl; + ss << "Report level: " << static_cast(report_level_) << std::endl; + ss << "Datatype: " << static_cast(data_type_) << std::endl; + ss << "Data: " << data_ << std::endl; + ss << "Text: " << text_; + return ss.str(); +} +} // namespace primary_interface +} // namespace ur_driver diff --git a/ur_robot_driver/src/primary/robot_message/key_message.cpp b/ur_robot_driver/src/primary/robot_message/key_message.cpp new file mode 100644 index 000000000..bf9f58920 --- /dev/null +++ b/ur_robot_driver/src/primary/robot_message/key_message.cpp @@ -0,0 +1,64 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-04-08 + * + */ +//---------------------------------------------------------------------- + +#include "ur_robot_driver/log.h" +#include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" + +namespace ur_driver +{ +namespace primary_interface +{ +bool KeyMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(message_code_); + bp.parse(message_argument_); + bp.parse(title_size_); + bp.parse(title_, title_size_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool KeyMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string KeyMessage::toString() const +{ + std::stringstream ss; + ss << "Message code: " << message_code_ << std::endl; + ss << "Title: " << title_ << std::endl; + ss << "Text: " << text_; + return ss.str(); +} +} // namespace primary_interface +} // namespace ur_driver diff --git a/ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp b/ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp new file mode 100644 index 000000000..ecf3b6bbd --- /dev/null +++ b/ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp @@ -0,0 +1,62 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include "ur_robot_driver/log.h" +#include "ur_robot_driver/primary/robot_message/runtime_exception_message.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" + +namespace ur_driver +{ +namespace primary_interface +{ +bool RuntimeExceptionMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(line_number_); + bp.parse(column_number_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool RuntimeExceptionMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string RuntimeExceptionMessage::toString() const +{ + std::stringstream ss; + ss << "Runtime error in line " << line_number_; + ss << ", column " << column_number_ << std::endl; + ss << "Error: " << text_; + return ss.str(); +} +} // namespace primary_interface +} // namespace ur_driver diff --git a/ur_robot_driver/src/primary/robot_message/text_message.cpp b/ur_robot_driver/src/primary/robot_message/text_message.cpp new file mode 100644 index 000000000..b05e4719d --- /dev/null +++ b/ur_robot_driver/src/primary/robot_message/text_message.cpp @@ -0,0 +1,56 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-04-08 + * + */ +//---------------------------------------------------------------------- + +#include "ur_robot_driver/log.h" +#include "ur_robot_driver/primary/robot_message/text_message.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" + +namespace ur_driver +{ +namespace primary_interface +{ +bool TextMessage::parseWith(comm::BinParser& bp) +{ + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool TextMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string TextMessage::toString() const +{ + return text_; +} +} // namespace primary_interface +} // namespace ur_driver diff --git a/ur_robot_driver/src/primary/robot_message/version_message.cpp b/ur_robot_driver/src/primary/robot_message/version_message.cpp index f7aa80685..8b11bbbb4 100644 --- a/ur_robot_driver/src/primary/robot_message/version_message.cpp +++ b/ur_robot_driver/src/primary/robot_message/version_message.cpp @@ -49,7 +49,7 @@ bool VersionMessage::parseWith(comm::BinParser& bp) return true; // not really possible to check dynamic size packets } -bool VersionMessage ::consumeWith(AbstractPrimaryConsumer& consumer) +bool VersionMessage::consumeWith(AbstractPrimaryConsumer& consumer) { return consumer.consume(*this); } diff --git a/ur_robot_driver/src/ur/calibration_checker.cpp b/ur_robot_driver/src/ur/calibration_checker.cpp index a6dcaee93..b1268024a 100644 --- a/ur_robot_driver/src/ur/calibration_checker.cpp +++ b/ur_robot_driver/src/ur/calibration_checker.cpp @@ -20,30 +20,23 @@ CalibrationChecker::CalibrationChecker(const std::string& expected_hash) : expected_hash_(expected_hash), checked_(false) { } -bool CalibrationChecker::consume(std::shared_ptr product) +void CalibrationChecker::handle(primary_interface::KinematicsInfo& kin_info) { - auto kin_info = std::dynamic_pointer_cast(product); - if (kin_info != nullptr) + if (kin_info.toHash() != expected_hash_) { - // LOG_INFO("%s", product->toString().c_str()); - // - if (kin_info->toHash() != expected_hash_) - { - LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics " - "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the " - "ur_calibration tool to extract the correct calibration from the robot and pass that into the " - "description. See " - "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] for " - "details."); - } - else - { - LOG_INFO("Calibration checked successfully."); - } - - checked_ = true; + LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics " + "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the " + "ur_calibration tool to extract the correct calibration from the robot and pass that into the " + "description. See " + "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] for " + "details."); + } + else + { + LOG_INFO("Calibration checked successfully."); } - return true; + checked_ = true; } + } // namespace ur_driver diff --git a/ur_robot_driver/src/ur/ur_driver.cpp b/ur_robot_driver/src/ur/ur_driver.cpp index 216a0dd4c..d4a23ea98 100644 --- a/ur_robot_driver/src/ur/ur_driver.cpp +++ b/ur_robot_driver/src/ur/ur_driver.cpp @@ -53,7 +53,8 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read) - : servoj_time_(0.008) + : primary_client_(robot_ip, calibration_checksum) + , servoj_time_(0.008) , servoj_gain_(servoj_gain) , servoj_lookahead_time_(servoj_lookahead_time) , reverse_interface_active_(false) @@ -65,13 +66,14 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc LOG_DEBUG("Initializing RTDE client"); rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_file, input_recipe_file)); - primary_stream_.reset( - new comm::URStream(robot_ip_, ur_driver::primary_interface::UR_PRIMARY_PORT)); - secondary_stream_.reset(new comm::URStream( - robot_ip_, ur_driver::primary_interface::UR_SECONDARY_PORT)); - secondary_stream_->connect(); - LOG_INFO("Checking if calibration data matches connected robot."); - checkCalibration(calibration_checksum); + // primary_stream_.reset( + // new comm::URStream(robot_ip_, ur_driver::primary_interface::UR_PRIMARY_PORT)); + // secondary_stream_.reset(new comm::URStream( + // robot_ip_, ur_driver::primary_interface::UR_SECONDARY_PORT)); + // secondary_stream_->connect(); + // LOG_INFO("Checking if calibration data matches connected robot."); + // checkCalibration(calibration_checksum); + // non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -254,30 +256,6 @@ std::string UrDriver::readKeepalive() } } -void UrDriver::checkCalibration(const std::string& checksum) -{ - if (primary_stream_ == nullptr) - { - throw std::runtime_error("checkCalibration() called without a primary interface connection being established."); - } - primary_interface::PrimaryParser parser; - comm::URProducer prod(*primary_stream_, parser); - prod.setupProducer(); - - CalibrationChecker consumer(checksum); - - comm::INotifier notifier; - - comm::Pipeline pipeline(prod, &consumer, "Pipeline", notifier); - pipeline.run(); - - while (!consumer.isChecked()) - { - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - LOG_DEBUG("Got calibration information from robot."); -} - rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() { return rtde_client_->getWriter(); @@ -285,28 +263,7 @@ rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() bool UrDriver::sendScript(const std::string& program) { - if (secondary_stream_ == nullptr) - { - throw std::runtime_error("Sending script to robot requested while there is no primary interface established. This " - "should not happen."); - } - - // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will - // not execute them. To avoid problems, we always just append a newline here, even if - // there may already be one. - auto program_with_newline = program + '\n'; - - size_t len = program_with_newline.size(); - const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); - size_t written; - - if (secondary_stream_->write(data, len, written)) - { - LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); - return true; - } - LOG_ERROR("Could not send program to robot"); - return false; + return primary_client_.sendScript(program); } bool UrDriver::sendRobotProgram()