diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ebf3264..fe2d2193 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,11 +23,24 @@ add_library(urcl SHARED src/control/script_sender.cpp src/control/trajectory_point_interface.cpp src/control/script_command_interface.cpp + src/primary/primary_client.cpp src/primary/primary_package.cpp + src/primary/program_state_message.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp + src/primary/program_state_message/global_variables_update_message.cpp + src/primary/program_state_message/global_variables_setup_message.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/primary/robot_state/robot_mode_data.cpp + src/primary/robot_state/joint_data.cpp + src/primary/robot_state/cartesian_info.cpp + src/primary/robot_state/force_mode_data.cpp + src/primary/robot_state/additional_info.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp src/rtde/control_package_setup_outputs.cpp diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 3a2135ab..186fe765 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -44,6 +44,8 @@ const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; +const bool SIMULATED_ROBOT = true; +bool g_program_running = false; std::unique_ptr g_my_driver; std::unique_ptr g_my_dashboard; @@ -54,6 +56,7 @@ void handleRobotProgramState(bool program_running) { // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; + g_program_running = program_running; } int main(int argc, char* argv[]) @@ -109,12 +112,18 @@ int main(int argc, char* argv[]) std::unique_ptr tool_comm_setup; const bool HEADLESS = true; g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + std::move(tool_comm_setup), CALIBRATION_CHECKSUM, SIMULATED_ROBOT)); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main + // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main // loop. + // Wait for the program to run on the robot + while (!g_program_running) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + g_my_driver->startRTDECommunication(); double increment = 0.01; @@ -157,7 +166,7 @@ int main(int argc, char* argv[]) URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); return 1; } - URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString()); + URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); } else { diff --git a/examples/primary_pipeline.cpp b/examples/primary_pipeline.cpp index e4390d8b..2c587c79 100644 --- a/examples/primary_pipeline.cpp +++ b/examples/primary_pipeline.cpp @@ -27,18 +27,18 @@ #include #include -#include #include +#include using namespace urcl; // In a real-world example it would be better to get those values from command line parameters / a better configuration // system such as Boost.Program_options const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; +const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; int main(int argc, char* argv[]) { - // Set the loglevel to info get print out the DH parameters urcl::setLogLevel(urcl::LogLevel::INFO); // Parse the ip arguments if given @@ -48,40 +48,41 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // Parse how many seconds to run - int second_to_run = -1; - if (argc > 2) - { - second_to_run = std::stoi(argv[2]); - } - - // First of all, we need a stream that connects to the robot - comm::URStream primary_stream(robot_ip, urcl::primary_interface::UR_PRIMARY_PORT); - - // This will parse the primary packages - primary_interface::PrimaryParser parser; - - // The producer needs both, the stream and the parser to fully work - comm::URProducer prod(primary_stream, parser); - prod.setupProducer(); + // First of all, we need to create a primary client that connects to the robot + primary_interface::PrimaryClient primary_client(robot_ip, CALIBRATION_CHECKSUM); - // The shell consumer will print the package contents to the shell - std::unique_ptr> consumer; - consumer.reset(new comm::ShellConsumer()); + // Give time to get the client to connect + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - // The notifer will be called at some points during connection setup / loss. This isn't fully - // implemented atm. - comm::INotifier notifier; - - // Now that we have all components, we can create and start the pipeline to run it all. - comm::Pipeline pipeline(prod, consumer.get(), "Pipeline", notifier); - pipeline.run(); - - // Package contents will be printed while not being interrupted - // Note: Packages for which the parsing isn't implemented, will only get their raw bytes printed. - do + for (int i = 0; i < 10; ++i) { - std::this_thread::sleep_for(std::chrono::seconds(second_to_run)); - } while (second_to_run < 0); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + // Create script program to send through client + std::stringstream cmd; + cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' + cmd << "sec setup():" << std::endl + << " textmsg(\"Command through primary interface complete " << i << "\")" << std::endl + << "end"; + std::string script_code = cmd.str(); + auto program_with_newline = script_code + '\n'; + // Send script + primary_client.sendScript(program_with_newline); + + try + { + URCL_LOG_INFO("Cartesian Information:\n%s", primary_client.getCartesianInfo()->toString().c_str()); + URCL_LOG_INFO("Calibration Hash:\n%s", primary_client.getCalibrationChecker()->getData()->toHash().c_str()); + URCL_LOG_INFO("Build Date:\n%s", primary_client.getVersionMessage()->build_date_.c_str()); + std::stringstream os; + os << primary_client.getJointData()->q_actual_; + URCL_LOG_INFO("Joint Angles:\n%s", os.str().c_str()); + // getGlobalVariablesSetupMessage() will throw an exception if a program on the robot has not been started + URCL_LOG_INFO("Global Variables:\n%s", primary_client.getGlobalVariablesSetupMessage()->variable_names_.c_str()); + } + catch (const UrException& e) + { + URCL_LOG_WARN(e.what()); + } + } return 0; } diff --git a/include/ur_client_library/comm/bin_parser.h b/include/ur_client_library/comm/bin_parser.h index cad7c632..52846c26 100644 --- a/include/ur_client_library/comm/bin_parser.h +++ b/include/ur_client_library/comm/bin_parser.h @@ -207,6 +207,19 @@ class BinParser } } + /*! + * \brief Parses the next bytes as a vector of 6 floats. + * + * \param val Reference to write the parsed value to + */ + void parse(vector6f_t& val) + { + for (size_t i = 0; i < val.size(); ++i) + { + parse(val[i]); + } + } + /*! * \brief Parses the next bytes as a vector of 6 32 bit integers. * diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index 5120be36..09d701e7 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -303,6 +303,8 @@ class Pipeline return; URCL_LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str()); + URCL_LOG_DEBUG("Producer thread joinable?! <%i>", pThread_.joinable()); + URCL_LOG_DEBUG("Consumer thread joinable?! <%i>", cThread_.joinable()); running_ = false; @@ -315,6 +317,7 @@ class Pipeline { cThread_.join(); } + URCL_LOG_DEBUG("Joined pipeline threads"); notifier_.stopped(name_); } @@ -352,7 +355,7 @@ class Pipeline void runProducer() { - URCL_LOG_DEBUG("Starting up producer"); + URCL_LOG_DEBUG("Starting up producer <%s>", name_.c_str()); std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in); bool has_realtime; realtime_file >> has_realtime; diff --git a/include/ur_client_library/comm/producer.h b/include/ur_client_library/comm/producer.h index 71d36f22..22443efb 100644 --- a/include/ur_client_library/comm/producer.h +++ b/include/ur_client_library/comm/producer.h @@ -42,7 +42,7 @@ class URProducer : public IProducer private: URStream& stream_; Parser& parser_; - std::chrono::seconds timeout_; + std::chrono::milliseconds timeout_; bool running_; @@ -57,6 +57,8 @@ class URProducer : public IProducer { } + virtual ~URProducer() = default; + /*! * \brief Triggers the stream to connect to the robot. */ @@ -83,6 +85,7 @@ class URProducer : public IProducer */ void stopProducer() override { + URCL_LOG_DEBUG("Stopping producer"); running_ = false; } @@ -105,24 +108,24 @@ class URProducer : public IProducer // 4KB should be enough to hold any packet received from UR uint8_t buf[4096]; size_t read = 0; - // expoential backoff reconnects + // exponential backoff reconnects while (true) { + if (!running_) + return true; + if (stream_.read(buf, sizeof(buf), read)) { // reset sleep amount - timeout_ = std::chrono::seconds(1); + timeout_ = std::chrono::milliseconds(100); BinParser bp(buf, read); return parser_.parse(bp, products); } - if (!running_) - return true; - if (stream_.closed()) return false; - URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count()); + URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld milliseconds...", timeout_.count()); std::this_thread::sleep_for(timeout_); if (stream_.connect()) diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index b2c13c6d..c6fb2156 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -26,13 +26,24 @@ */ //---------------------------------------------------------------------- -#ifndef UR_ROBOT_DRIVER_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED -#define UR_ROBOT_DRIVER_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED +#define UR_CLIENT_LIBRARY_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED #include "ur_client_library/log.h" #include "ur_client_library/comm/pipeline.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/robot_state/joint_data.h" +#include "ur_client_library/primary/robot_state/cartesian_info.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" +#include "ur_client_library/primary/robot_state/force_mode_data.h" +#include "ur_client_library/primary/robot_state/additional_info.h" +#include "ur_client_library/primary/program_state_message/global_variables_update_message.h" +#include "ur_client_library/primary/program_state_message/global_variables_setup_message.h" namespace urcl { @@ -51,7 +62,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual ~AbstractPrimaryConsumer() = default; /*! - * \brief This consume method is usally being called by the Pipeline structure. We don't + * \brief This consume method is usually being called by the Pipeline structure. We don't * necessarily need to know the specific package type here, as the packages themselves will take * care to be consumed with the correct function (visitor pattern). * @@ -71,8 +82,20 @@ 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(RobotModeData& pkg) = 0; + virtual bool consume(JointData& pkg) = 0; + virtual bool consume(CartesianInfo& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; + virtual bool consume(ForceModeData& pkg) = 0; + virtual bool consume(AdditionalInfo& pkg) = 0; + virtual bool consume(ProgramStateMessage& pkg) = 0; + virtual bool consume(GlobalVariablesUpdateMessage& pkg) = 0; + virtual bool consume(GlobalVariablesSetupMessage& pkg) = 0; private: /* data */ @@ -80,4 +103,4 @@ class AbstractPrimaryConsumer : public comm::IConsumer } // namespace primary_interface } // namespace urcl -#endif // ifndef UR_ROBOT_DRIVER_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/addtional_info_message_handler.h b/include/ur_client_library/primary/message_handlers/addtional_info_message_handler.h new file mode 100644 index 00000000..1cf56c2f --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/addtional_info_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_ADDITIONAL_INFO_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_ADDITIONAL_INFO_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class AdditionalInfoHandler : public IPrimaryPackageHandler +{ +public: + AdditionalInfoHandler() = default; + virtual ~AdditionalInfoHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(AdditionalInfo& pkg) override + { + // URCL_LOG_INFO("---AdditionalInfo---\n%s", pkg.toString().c_str()); + data_.reset(new AdditionalInfo(pkg)); + } + + /*! + * \brief Get latest AdditionalInfo + * + * \return latest AdditionalInfo + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A AdditionalInfo package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_ADDITIONAL_INFO_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/cartesian_info_message_handler.h b/include/ur_client_library/primary/message_handlers/cartesian_info_message_handler.h new file mode 100644 index 00000000..7ca72698 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/cartesian_info_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_CARTESIAN_INFO_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_CARTESIAN_INFO_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class CartesianInfoHandler : public IPrimaryPackageHandler +{ +public: + CartesianInfoHandler() = default; + virtual ~CartesianInfoHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(CartesianInfo& pkg) override + { + // URCL_LOG_INFO("---CartesianInfo---\n%s", pkg.toString().c_str()); + data_.reset(new CartesianInfo(pkg)); + } + + /*! + * \brief Get latest CartesianInfo + * + * \return latest CartesianInfo + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A CartesianInfo package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_CARTESIAN_INFO_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/error_code_message_handler.h b/include/ur_client_library/primary/message_handlers/error_code_message_handler.h new file mode 100644 index 00000000..e5bd596b --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/error_code_message_handler.h @@ -0,0 +1,108 @@ +// 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_CLIENT_LIBRARY_ERROR_CODE_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_ERROR_CODE_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class ErrorCodeMessageHandler : public IPrimaryPackageHandler +{ +public: + ErrorCodeMessageHandler() = default; + virtual ~ErrorCodeMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(ErrorCodeMessage& pkg) override + { + std::stringstream out_ss; + out_ss << "---ErrorCodeMessage---\n" << pkg.toString().c_str() << std::endl; + switch (pkg.report_level_) + { + case ReportLevel::DEBUG: + case ReportLevel::DEVL_DEBUG: + { + URCL_LOG_DEBUG("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::INFO: + case ReportLevel::DEVL_INFO: + { + URCL_LOG_INFO("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::WARNING: + { + URCL_LOG_WARN("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::VIOLATION: + case ReportLevel::FAULT: + case ReportLevel::DEVL_VIOLATION: + case ReportLevel::DEVL_FAULT: + { + URCL_LOG_ERROR("%s", out_ss.str().c_str()); + break; + } + default: + { + std::stringstream ss; + ss << "Unknown report level: " << static_cast(pkg.report_level_) << std::endl << out_ss.str(); + URCL_LOG_ERROR("%s", ss.str().c_str()); + } + } + data_.reset(new ErrorCodeMessage(pkg)); + } + + /*! + * \brief Get latest ErrorCodeMessage + * + * \return latest ErrorCodeMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A ErrorCodeMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_ERROR_CODE_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/force_mode_data_message_handler.h b/include/ur_client_library/primary/message_handlers/force_mode_data_message_handler.h new file mode 100644 index 00000000..8f6e02c0 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/force_mode_data_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_FORCE_MODE_DATA_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_FORCE_MODE_DATA_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class ForceModeDataHandler : public IPrimaryPackageHandler +{ +public: + ForceModeDataHandler() = default; + virtual ~ForceModeDataHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(ForceModeData& pkg) override + { + // URCL_LOG_INFO("---ForceModeData---\n%s", pkg.toString().c_str()); + data_.reset(new ForceModeData(pkg)); + } + + /*! + * \brief Get latest ForceModeData + * + * \return latest ForceModeData + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A ForceModeData package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_FORCE_MODE_DATA_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/global_variables_setup_message_handler.h b/include/ur_client_library/primary/message_handlers/global_variables_setup_message_handler.h new file mode 100644 index 00000000..e545fbe2 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/global_variables_setup_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_SETUP_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_SETUP_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class GlobalVariablesSetupMessageHandler : public IPrimaryPackageHandler +{ +public: + GlobalVariablesSetupMessageHandler() = default; + virtual ~GlobalVariablesSetupMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(GlobalVariablesSetupMessage& pkg) override + { + // URCL_LOG_INFO("---GlobalVariablesSetupMessage---\n%s", pkg.toString().c_str()); + data_.reset(new GlobalVariablesSetupMessage(pkg)); + } + + /*! + * \brief Get latest GlobalVariablesSetupMessage + * + * \return latest GlobalVariablesSetupMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A GlobalVariablesSetupMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_SETUP_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/global_variables_update_message_handler.h b/include/ur_client_library/primary/message_handlers/global_variables_update_message_handler.h new file mode 100644 index 00000000..5737d420 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/global_variables_update_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class GlobalVariablesUpdateMessageHandler : public IPrimaryPackageHandler +{ +public: + GlobalVariablesUpdateMessageHandler() = default; + virtual ~GlobalVariablesUpdateMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(GlobalVariablesUpdateMessage& pkg) override + { + // URCL_LOG_INFO("---GlobalVariablesUpdateMessage---\n%s", pkg.toString().c_str()); + data_.reset(new GlobalVariablesUpdateMessage(pkg)); + } + + /*! + * \brief Get latest GlobalVariablesUpdateMessage + * + * \return latest GlobalVariablesUpdateMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A GlobalVariablesUpdateMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/joint_data_message_handler.h b/include/ur_client_library/primary/message_handlers/joint_data_message_handler.h new file mode 100644 index 00000000..5e756738 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/joint_data_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_JOINT_DATA_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_JOINT_DATA_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class JointDataHandler : public IPrimaryPackageHandler +{ +public: + JointDataHandler() = default; + virtual ~JointDataHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(JointData& pkg) override + { + // URCL_LOG_INFO("---JointData---\n%s", pkg.toString().c_str()); + data_.reset(new JointData(pkg)); + } + + /*! + * \brief Get latest JointData + * + * \return latest JointData + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A JointData package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_JOINT_DATA_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/key_message_handler.h b/include/ur_client_library/primary/message_handlers/key_message_handler.h new file mode 100644 index 00000000..5a4c8a96 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/key_message_handler.h @@ -0,0 +1,73 @@ +// 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_CLIENT_LIBRARY_KEY_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_KEY_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +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 + { + // URCL_LOG_INFO("---KeyMessage---\n%s", pkg.toString().c_str()); + data_.reset(new KeyMessage(pkg)); + } + + /*! + * \brief Get latest KeyMessage + * + * \return latest KeyMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A KeyMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_KEY_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/robot_mode_data_message_handler.h b/include/ur_client_library/primary/message_handlers/robot_mode_data_message_handler.h new file mode 100644 index 00000000..edfe175a --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/robot_mode_data_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class RobotModeDataHandler : public IPrimaryPackageHandler +{ +public: + RobotModeDataHandler() = default; + virtual ~RobotModeDataHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(RobotModeData& pkg) override + { + // URCL_LOG_INFO("---RobotModeData---\n%s", pkg.toString().c_str()); + data_.reset(new RobotModeData(pkg)); + } + + /*! + * \brief Get latest RobotModeData + * + * \return latest RobotModeData + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A RobotModeData package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/runtime_exception_message_handler.h b/include/ur_client_library/primary/message_handlers/runtime_exception_message_handler.h new file mode 100644 index 00000000..ea9e45c7 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/runtime_exception_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_RUNTIME_EXCEPTION_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_RUNTIME_EXCEPTION_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class RuntimeExceptionHandler : public IPrimaryPackageHandler +{ +public: + RuntimeExceptionHandler() = default; + virtual ~RuntimeExceptionHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(RuntimeExceptionMessage& pkg) override + { + // URCL_LOG_INFO("---RuntimeExceptionMessage---\n%s", pkg.toString().c_str()); + data_.reset(new RuntimeExceptionMessage(pkg)); + } + + /*! + * \brief Get latest RuntimeExceptionMessage + * + * \return latest RuntimeExceptionMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A RuntimeExceptionMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_RUNTIME_EXCEPTION_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/text_message_handler.h b/include/ur_client_library/primary/message_handlers/text_message_handler.h new file mode 100644 index 00000000..af6e42bd --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/text_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_TEXT_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_TEXT_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class TextMessageHandler : public IPrimaryPackageHandler +{ +public: + TextMessageHandler() = default; + virtual ~TextMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(TextMessage& pkg) override + { + // URCL_LOG_INFO("---TextMessage---\n%s", pkg.toString().c_str()); + data_.reset(new TextMessage(pkg)); + } + + /*! + * \brief Get latest TextMessage + * + * \return latest TextMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A TextMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_TEXT_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/version_message_handler.h b/include/ur_client_library/primary/message_handlers/version_message_handler.h new file mode 100644 index 00000000..0a6567ba --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/version_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_VERSION_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_VERSION_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class VersionMessageHandler : public IPrimaryPackageHandler +{ +public: + VersionMessageHandler() = default; + virtual ~VersionMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(VersionMessage& pkg) override + { + // URCL_LOG_INFO("---VersionMessage---\n%s", pkg.toString().c_str()); + data_.reset(new VersionMessage(pkg)); + } + + /*! + * \brief Get latest VersionMessage + * + * \return latest VersionMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A VersionMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_VERSION_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h new file mode 100644 index 00000000..34993bb8 --- /dev/null +++ b/include/ur_client_library/primary/primary_client.h @@ -0,0 +1,213 @@ +// 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_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class PrimaryClient +{ +public: + PrimaryClient() = delete; + PrimaryClient(const std::string& robot_ip, const std::string& calibration_checksum); + // virtual ~PrimaryClient() = default; + virtual ~PrimaryClient(); + + /*! + * \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 Configures the primary client + * + * Creates a connection to the stream and sets up producer, consumer and pipeline + */ + bool configure(); + + /*! + * \brief Checks if the robot is in local or remote control + * + * Checks for package with error code determining if robot is in remote or local control + */ + void checkRemoteLocalControl(); + + /*! + * \brief Stops the primary client + * + * Closes the thread checking for remote or local control + */ + void stop(); + + /*! + * \brief Returns whether the robot is in remote or local control + * + * Returns whether the robot is in remote or local control + */ + bool isInRemoteControl(); + + /*! + * \brief Starts the checkRemoteLocalControl thread. + * + * Starts the thread checking for remote or local control + */ + void startCheckRemoteControlThread(); + + /*! + * \brief Returns latest AdditionalInfo message + * + * Returns latest AdditionalInfo message + */ + std::shared_ptr getAdditionalInfo(); + + /*! + * \brief Returns latest CartesianInfo message + * + * Returns latest CartesianInfo message + */ + std::shared_ptr getCartesianInfo(); + + /*! + * \brief Returns latest ForceModeData message + * + * Returns latest ForceModeData message + */ + std::shared_ptr getForceModeData(); + + /*! + * \brief Returns latest JointData message + * + * Returns latest JointData message + */ + std::shared_ptr getJointData(); + + /*! + * \brief Returns latest RobotModeData message + * + * Returns latest RobotModeData message + */ + std::shared_ptr getRobotModeData(); + + /*! + * \brief Returns latest KeyMessage message + * + * Returns latest KeyMessage message + */ + std::shared_ptr getKeyMessage(); + + /*! + * \brief Returns latest ErrorCodeMessage message + * + * Returns latest ErrorCodeMessage message + */ + std::shared_ptr getErrorCodeMessage(); + + /*! + * \brief Returns latest RuntimeExceptionMessage message + * + * Returns latest RuntimeExceptionMessage message + */ + std::shared_ptr getRuntimeExceptionMessage(); + + /*! + * \brief Returns latest TextMessage message + * + * Returns latest TextMessage message + */ + std::shared_ptr getTextMessage(); + + /*! + * \brief Returns latest VersionMessage message + * + * Returns latest VersionMessage message + */ + std::shared_ptr getVersionMessage(); + + /*! + * \brief Returns latest GlobalVariablesSetupMessage message + * + * Returns latest GlobalVariablesSetupMessage message + */ + std::shared_ptr getGlobalVariablesSetupMessage(); + + /*! + * \brief Returns Calibration checker + * + * Returns Calibration checker + */ + std::shared_ptr getCalibrationChecker(); + + /*! + * \brief Set whether the robot is simulated or real + */ + void setSimulated(bool value); + + /*! + * \brief Check if the robot is simulated or real + * + * \returns true if robot is simulated, false otherwise. + */ + bool getSimulated(); + +private: + std::string robot_ip_; + int port_; + PrimaryParser parser_; + comm::INotifier notifier_; + bool connected_; + bool simulated_; + bool e_series_; + std::atomic running_, in_remote_control_; + std::unique_ptr consumer_; + std::unique_ptr> producer_; + std::unique_ptr> stream_; + std::unique_ptr> pipeline_; + std::unique_ptr dashboard_client_; + std::shared_ptr calibration_checker_; + std::thread rc_thread_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h new file mode 100644 index 00000000..58b21280 --- /dev/null +++ b/include/ur_client_library/primary/primary_consumer.h @@ -0,0 +1,319 @@ +// 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_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED + +#include "ur_client_library/log.h" +#include "ur_client_library/comm/pipeline.h" +#include "ur_client_library/primary/primary_package_handler.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" +#include "ur_client_library/primary/message_handlers/key_message_handler.h" +#include "ur_client_library/primary/message_handlers/version_message_handler.h" +#include "ur_client_library/primary/message_handlers/text_message_handler.h" +#include "ur_client_library/primary/message_handlers/runtime_exception_message_handler.h" +#include "ur_client_library/primary/message_handlers/error_code_message_handler.h" +#include "ur_client_library/primary/message_handlers/robot_mode_data_message_handler.h" +#include "ur_client_library/primary/message_handlers/joint_data_message_handler.h" +#include "ur_client_library/primary/message_handlers/cartesian_info_message_handler.h" +#include "ur_client_library/primary/message_handlers/force_mode_data_message_handler.h" +#include "ur_client_library/primary/message_handlers/addtional_info_message_handler.h" +#include "ur_client_library/primary/message_handlers/global_variables_update_message_handler.h" +#include "ur_client_library/primary/message_handlers/global_variables_setup_message_handler.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" +#include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/robot_state/kinematics_info.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Primary consumer implementation + * + * This class implements an 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() + { + URCL_LOG_INFO("Constructing primary consumer"); + key_message_worker_.reset(new KeyMessageHandler()); + text_message_worker_.reset(new TextMessageHandler()); + version_message_worker_.reset(new VersionMessageHandler()); + error_code_message_worker_.reset(new ErrorCodeMessageHandler()); + cartesian_info_message_worker_.reset(new CartesianInfoHandler()); + force_mode_data_message_worker_.reset(new ForceModeDataHandler()); + additional_info_message_worker_.reset(new AdditionalInfoHandler()); + robot_mode_data_message_worker_.reset(new RobotModeDataHandler()); + joint_data_message_worker_.reset(new JointDataHandler()); + runtime_exception_message_worker_.reset(new RuntimeExceptionHandler()); + global_variables_update_message_worker_.reset(new GlobalVariablesUpdateMessageHandler()); + global_variables_setup_message_worker_.reset(new GlobalVariablesSetupMessageHandler()); + URCL_LOG_INFO("Constructed primary consumer"); + } + virtual ~PrimaryConsumer() = default; + + // These need to be overriden + virtual bool consume(RobotMessage& msg) override + { + // URCL_LOG_INFO("---RobotMessage:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RobotState& msg) override + { + // URCL_LOG_INFO("---RobotState:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(ProgramStateMessage& msg) override + { + // URCL_LOG_INFO("---ProgramStateMessage---\n%s", msg.toString().c_str()); + return true; + } + + /*! + * \brief Handle a GlobalVariablesSetupMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(GlobalVariablesSetupMessage& pkg) override + { + if (global_variables_setup_message_worker_ != nullptr) + { + global_variables_setup_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a VersionMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(VersionMessage& pkg) override + { + if (version_message_worker_ != nullptr) + { + version_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a RuntimeExceptionMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(RuntimeExceptionMessage& pkg) override + { + if (runtime_exception_message_worker_ != nullptr) + { + runtime_exception_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a TextMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(TextMessage& pkg) override + { + if (text_message_worker_ != nullptr) + { + text_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a GlobalVariablesUpdateMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(GlobalVariablesUpdateMessage& pkg) override + { + if (global_variables_update_message_worker_ != nullptr) + { + global_variables_update_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a RobotModeData + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(RobotModeData& pkg) override + { + if (robot_mode_data_message_worker_ != nullptr) + { + robot_mode_data_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a JointData + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(JointData& pkg) override + { + if (joint_data_message_worker_ != nullptr) + { + joint_data_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a AdditionalInfo + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(AdditionalInfo& pkg) override + { + if (additional_info_message_worker_ != nullptr) + { + additional_info_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a ForceModeData + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(ForceModeData& pkg) override + { + if (force_mode_data_message_worker_ != nullptr) + { + force_mode_data_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a CartesianInfo + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(CartesianInfo& pkg) override + { + if (cartesian_info_message_worker_ != nullptr) + { + cartesian_info_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \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; + } + + std::shared_ptr> key_message_worker_; + std::shared_ptr> text_message_worker_; + std::shared_ptr> version_message_worker_; + std::shared_ptr> error_code_message_worker_; + std::shared_ptr> cartesian_info_message_worker_; + std::shared_ptr> kinematics_info_message_worker_; + std::shared_ptr> force_mode_data_message_worker_; + std::shared_ptr> additional_info_message_worker_; + std::shared_ptr> robot_mode_data_message_worker_; + std::shared_ptr> joint_data_message_worker_; + std::shared_ptr> runtime_exception_message_worker_; + std::shared_ptr> global_variables_update_message_worker_; + std::shared_ptr> global_variables_setup_message_worker_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_package_handler.h b/include/ur_client_library/primary/primary_package_handler.h new file mode 100644 index 00000000..cad4c8d7 --- /dev/null +++ b/include/ur_client_library/primary/primary_package_handler.h @@ -0,0 +1,67 @@ +// 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_CLIENT_LIBRARY_PRIMARY_PACKAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_PACKAGE_HANDLER_H_INCLUDED + +#include + +namespace urcl +{ +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; + + /*! + * \brief Get latest PackageT + * + * \return latest PackageT + */ + virtual std::shared_ptr getData() = 0; + +private: + /* data */ +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_PACKAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 3dfba100..f4654685 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -26,8 +26,20 @@ #include "ur_client_library/primary/package_header.h" #include "ur_client_library/primary/robot_state.h" #include "ur_client_library/primary/robot_message.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/robot_state/joint_data.h" +#include "ur_client_library/primary/robot_state/cartesian_info.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" +#include "ur_client_library/primary/robot_state/force_mode_data.h" +#include "ur_client_library/primary/robot_state/additional_info.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/program_state_message/global_variables_update_message.h" +#include "ur_client_library/primary/program_state_message/global_variables_setup_message.h" +#include namespace urcl { @@ -56,6 +68,7 @@ class PrimaryParser : public comm::Parser { int32_t packet_size; RobotPackageType type; + bp.parse(packet_size); bp.parse(type); @@ -115,7 +128,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); @@ -134,9 +147,31 @@ class PrimaryParser : public comm::Parser break; } + case RobotPackageType::PROGRAM_STATE_MESSAGE: + { + uint64_t timestamp; + ProgramStateMessageType state_type; + URCL_LOG_DEBUG("ProgramStateMessage received"); + + bp.parse(timestamp); + bp.parse(state_type); + + URCL_LOG_DEBUG("ProgramStateMessage of type %d received", static_cast(state_type)); + + std::unique_ptr packet(programStateFromType(state_type, timestamp)); + if (!packet->parseWith(bp)) + { + URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(state_type)); + return false; + } + + results.push_back(std::move(packet)); + return true; + } + default: { - URCL_LOG_DEBUG("Invalid robot package type recieved: %u", static_cast(type)); + URCL_LOG_DEBUG("Invalid robot package type received: %u", static_cast(type)); bp.consume(); return true; } @@ -149,14 +184,18 @@ class PrimaryParser : public comm::Parser { 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 RobotStateType::ROBOT_MODE_DATA: + return new RobotModeData(type); + case RobotStateType::JOINT_DATA: + return new JointData(type); + case RobotStateType::CARTESIAN_INFO: + return new CartesianInfo(type); case RobotStateType::KINEMATICS_INFO: return new KinematicsInfo(type); + case RobotStateType::FORCE_MODE_DATA: + return new ForceModeData(type); + case RobotStateType::ADDITIONAL_INFO: + return new AdditionalInfo(type); default: return new RobotState(type); } @@ -166,16 +205,31 @@ class PrimaryParser : public comm::Parser { 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 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, type); + } + } + + ProgramStateMessage* programStateFromType(ProgramStateMessageType type, uint64_t timestamp) + { + switch (type) + { + case ProgramStateMessageType::GLOBAL_VARIABLES_SETUP: + return new GlobalVariablesSetupMessage(timestamp); + case ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE: + return new GlobalVariablesUpdateMessage(timestamp); default: - return new RobotMessage(timestamp, source); + return new ProgramStateMessage(timestamp, type); } } }; diff --git a/include/ur_client_library/primary/program_state_message.h b/include/ur_client_library/primary/program_state_message.h new file mode 100644 index 00000000..6e5874a6 --- /dev/null +++ b/include/ur_client_library/primary/program_state_message.h @@ -0,0 +1,108 @@ +// 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 Exner + * \date 2022-02-21 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/primary_package.h" +#include "ur_client_library/primary/package_header.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Possible ProgramStateMessage types + */ +enum class ProgramStateMessageType : uint8_t +{ + GLOBAL_VARIABLES_SETUP = 0, + GLOBAL_VARIABLES_UPDATE = 1, + TYPE_VARIABLES_UPDATE = 2, +}; + +/*! + * \brief The ProgramStateMessage class is a parent class for the different received robot messages. + */ +class ProgramStateMessage : public PrimaryPackage +{ +public: + /*! + * \brief Creates a new ProgramStateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + */ + ProgramStateMessage(const uint64_t timestamp) : timestamp_(timestamp) + { + } + + /*! + * \brief Creates a new ProgramStateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param state_type The package's message type + */ + ProgramStateMessage(const uint64_t timestamp, const ProgramStateMessageType state_type) + : timestamp_(timestamp), state_type_(state_type) + { + } + virtual ~ProgramStateMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version 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; + + uint64_t timestamp_; + ProgramStateMessageType state_type_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif /* UR_CLIENT_LIBRARY_ROBOT_STATE_H_INCLUDED */ diff --git a/include/ur_client_library/primary/program_state_message/global_variables_setup_message.h b/include/ur_client_library/primary/program_state_message/global_variables_setup_message.h new file mode 100644 index 00000000..30915d22 --- /dev/null +++ b/include/ur_client_library/primary/program_state_message/global_variables_setup_message.h @@ -0,0 +1,102 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_SETUP_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_SETUP_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/program_state_message.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The GlobalVariablesSetupMessage class handles the key messages sent via the primary UR interface. + */ +class GlobalVariablesSetupMessage : public ProgramStateMessage +{ +public: + GlobalVariablesSetupMessage() = delete; + /*! + * \brief Creates a new GlobalVariablesSetupMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + */ + GlobalVariablesSetupMessage(const uint64_t timestamp) + : ProgramStateMessage(timestamp, ProgramStateMessageType::GLOBAL_VARIABLES_SETUP) + { + } + + /*! + * \brief Creates a copy of a GlobalVariablesSetupMessage object. + * + * \param pkg The GlobalVariablesSetupMessage object to be copied + */ + GlobalVariablesSetupMessage(const GlobalVariablesSetupMessage& pkg) + : ProgramStateMessage(pkg.timestamp_, ProgramStateMessageType::GLOBAL_VARIABLES_SETUP) + { + start_index_ = pkg.start_index_; + variable_names_ = pkg.variable_names_; + } + virtual ~GlobalVariablesSetupMessage() = 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; + + uint16_t start_index_; + std::string variable_names_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_SETUP_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/program_state_message/global_variables_update_message.h b/include/ur_client_library/primary/program_state_message/global_variables_update_message.h new file mode 100644 index 00000000..62548463 --- /dev/null +++ b/include/ur_client_library/primary/program_state_message/global_variables_update_message.h @@ -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 20222-02-21 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/program_state_message.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The GlobalVariablesUpdateMessage class handles the key messages sent via the primary UR interface. + */ +class GlobalVariablesUpdateMessage : public ProgramStateMessage +{ +public: + GlobalVariablesUpdateMessage() = delete; + /*! + * \brief Creates a new GlobalVariablesUpdateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + */ + GlobalVariablesUpdateMessage(const uint64_t timestamp) + : ProgramStateMessage(timestamp, ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE) + { + } + + /*! + * \brief Creates a copy of a GlobalVariablesUpdateMessage object. + * + * \param pkg The GlobalVariablesUpdateMessage object to be copied + */ + GlobalVariablesUpdateMessage(const GlobalVariablesUpdateMessage& pkg) + : ProgramStateMessage(pkg.timestamp_, ProgramStateMessageType::GLOBAL_VARIABLES_SETUP) + { + start_index_ = pkg.start_index_; + variables_ = pkg.variables_; + } + virtual ~GlobalVariablesUpdateMessage() = 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; + + uint16_t start_index_; + std::string variables_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message.h b/include/ur_client_library/primary/robot_message.h index 5ee0c3a2..4f7bb4f5 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -38,7 +38,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, @@ -63,7 +63,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; @@ -95,7 +107,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/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h new file mode 100644 index 00000000..f13b016a --- /dev/null +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -0,0 +1,122 @@ +// 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_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +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) + { + } + + /*! + * \brief Creates a copy of a ErrorCodeMessage object. + * + * \param pkg The ErrorCodeMessage object to be copied + */ + ErrorCodeMessage(const ErrorCodeMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE) + { + message_code_ = pkg.message_code_; + message_argument_ = pkg.message_argument_; + report_level_ = pkg.report_level_; + data_type_ = pkg.data_type_; + data_ = pkg.data_; + text_ = pkg.text_; + } + 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 urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/key_message.h b/include/ur_client_library/primary/robot_message/key_message.h new file mode 100644 index 00000000..8cbaf328 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/key_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_CLIENT_LIBRARY_PRIMARY_KEY_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_KEY_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +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) + { + } + + /*! + * \brief Creates a copy of a KeyMessage object. + * + * \param pkg The KeyMessage object to be copied + */ + KeyMessage(const KeyMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_KEY) + { + message_code_ = pkg.message_code_; + message_argument_ = pkg.message_argument_; + title_size_ = pkg.title_size_; + title_ = pkg.title_; + text_ = pkg.text_; + } + 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 urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/runtime_exception_message.h b/include/ur_client_library/primary/robot_message/runtime_exception_message.h new file mode 100644 index 00000000..be2ec287 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/runtime_exception_message.h @@ -0,0 +1,102 @@ +// 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_CLIENT_LIBRARY_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +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) + { + } + + /*! + * \brief Creates a copy of a RuntimeExceptionMessage object. + * + * \param pkg The RuntimeExceptionMessage object to be copied + */ + RuntimeExceptionMessage(const RuntimeExceptionMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION) + { + line_number_ = pkg.line_number_; + column_number_ = pkg.column_number_; + text_ = pkg.text_; + } + 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 urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/text_message.h b/include/ur_client_library/primary/robot_message/text_message.h new file mode 100644 index 00000000..1d8413e8 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/text_message.h @@ -0,0 +1,98 @@ +// 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_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +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) + { + } + + /*! + * \brief Creates a copy of a TextMessage object. + * + * \param pkg The TextMessage object to be copied + */ + TextMessage(const TextMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_TEXT) + { + text_ = pkg.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 urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/version_message.h b/include/ur_client_library/primary/robot_message/version_message.h index 9663691d..84f5a136 100644 --- a/include/ur_client_library/primary/robot_message/version_message.h +++ b/include/ur_client_library/primary/robot_message/version_message.h @@ -48,9 +48,27 @@ 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) { } + + /*! + * \brief Creates a copy of a VersionMessage object. + * + * \param pkg The VersionMessage object to be copied + */ + VersionMessage(const VersionMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_VERSION) + { + project_name_length_ = pkg.project_name_length_; + project_name_ = pkg.project_name_; + major_version_ = pkg.major_version_; + minor_version_ = pkg.minor_version_; + svn_version_ = pkg.svn_version_; + build_number_ = pkg.build_number_; + build_date_ = pkg.build_date_; + } virtual ~VersionMessage() = default; /*! diff --git a/include/ur_client_library/primary/robot_state.h b/include/ur_client_library/primary/robot_state.h index 2e468a60..8e4865df 100644 --- a/include/ur_client_library/primary/robot_state.h +++ b/include/ur_client_library/primary/robot_state.h @@ -50,7 +50,9 @@ enum class RobotStateType : uint8_t CONFIGURATION_DATA = 6, FORCE_MODE_DATA = 7, ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 + TOOL_COMMUNICATION_INFO = 11, + TOOL_MODE_INFO = 12, + SINGULARITY_INFO = 13 }; /*! diff --git a/include/ur_client_library/primary/robot_state/additional_info.h b/include/ur_client_library/primary/robot_state/additional_info.h new file mode 100644 index 00000000..819c680d --- /dev/null +++ b/include/ur_client_library/primary/robot_state/additional_info.h @@ -0,0 +1,102 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_ADDITIONAL_INFO_H_INCLUDED +#define UR_CLIENT_LIBRARY_ADDITIONAL_INFO_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains information about the button states and freedrive of the robot. + */ +class AdditionalInfo : public RobotState +{ +public: + AdditionalInfo() = delete; + /*! + * \brief Creates a new AdditionalInfo object. + * + * \param type The type of RobotState message received + */ + AdditionalInfo(const RobotStateType type) : RobotState(type) + { + } + + AdditionalInfo(const AdditionalInfo& pkg) : RobotState(RobotStateType::ADDITIONAL_INFO) + { + tp_button_state_ = pkg.tp_button_state_; + freedrive_button_enabled_ = pkg.freedrive_button_enabled_; + io_enabled_freedrive_ = pkg.io_enabled_freedrive_; + reserved_length_ = pkg.reserved_length_; + reserved_ = pkg.reserved_; + } + virtual ~AdditionalInfo() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this specific 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; + + uint8_t tp_button_state_; + bool freedrive_button_enabled_; + bool io_enabled_freedrive_; + uint8_t reserved_length_; + std::string reserved_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_ADDITIONAL_INFO_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_state/cartesian_info.h b/include/ur_client_library/primary/robot_state/cartesian_info.h new file mode 100644 index 00000000..12eebbe7 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/cartesian_info.h @@ -0,0 +1,111 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_CARTESIAN_INFO_H_INCLUDED +#define UR_CLIENT_LIBRARY_CARTESIAN_INFO_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains cartesian information on the robot. The flange_coordinates vector is the flange + * coordinates of the robot and the tcp_offset_coordinates is the coordinates of the tcp offset. Both vectors are x, y, + * z, rx, ry, rz. + */ +class CartesianInfo : public RobotState +{ +public: + CartesianInfo() = delete; + /*! + * \brief Creates a new CartesianInfo object. + * + * \param type The type of RobotState message received + */ + CartesianInfo(const RobotStateType type) : RobotState(type) + { + } + + /*! + * \brief Creates a copy of a CartesianInfo object. + * + * \param pkg The CartesianInfo object to be copied + */ + CartesianInfo(const CartesianInfo& pkg) : RobotState(RobotStateType::CARTESIAN_INFO) + { + flange_coordinates_ = pkg.flange_coordinates_; + tcp_offset_coordinates_ = pkg.tcp_offset_coordinates_; + } + + virtual ~CartesianInfo() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this specific 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; + + /*! + * \brief Calculates a hash value of the parameters to allow for identification of a calibration. + * + * \returns A hash value of the parameters + */ + // std::string toHash() const; + + vector6d_t flange_coordinates_; + vector6d_t tcp_offset_coordinates_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_CARTESIAN_INFO_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_state/force_mode_data.h b/include/ur_client_library/primary/robot_state/force_mode_data.h new file mode 100644 index 00000000..edced1b7 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/force_mode_data.h @@ -0,0 +1,96 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_FORCE_MODE_DATA_H_INCLUDED +#define UR_CLIENT_LIBRARY_FORCE_MODE_DATA_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains the wrench applied to the flange?tcp? in base frame?tcp frame? + */ +class ForceModeData : public RobotState +{ +public: + ForceModeData() = delete; + /*! + * \brief Creates a new ForceModeData object. + * + * \param type The type of RobotState message received + */ + ForceModeData(const RobotStateType type) : RobotState(type) + { + } + + ForceModeData(const ForceModeData& pkg) : RobotState(RobotStateType::FORCE_MODE_DATA) + { + wrench_ = pkg.wrench_; + robot_dexterity_ = pkg.robot_dexterity_; + } + virtual ~ForceModeData() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this specific 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; + + vector6d_t wrench_; + double robot_dexterity_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_FORCE_MODE_DATA_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_state/joint_data.h b/include/ur_client_library/primary/robot_state/joint_data.h new file mode 100644 index 00000000..68412870 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/joint_data.h @@ -0,0 +1,113 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_JOINT_DATA_H_INCLUDED +#define UR_CLIENT_LIBRARY_JOINT_DATA_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains data about the joints of the robot and the motors of the joints. + */ +class JointData : public RobotState +{ +public: + JointData() = delete; + /*! + * \brief Creates a new JointData object. + * + * \param type The type of RobotState message received + */ + JointData(const RobotStateType type) : RobotState(type) + { + } + + /*! + * \brief Creates a copy of a JointData object. + * + * \param pkg The JointData object to be copied + */ + JointData(const JointData& pkg) : RobotState(RobotStateType::JOINT_DATA) + { + q_actual_ = pkg.q_actual_; + q_target_ = pkg.q_target_; + qd_actual_ = pkg.qd_actual_; + i_actual_ = pkg.i_actual_; + v_actual_ = pkg.v_actual_; + t_motor_ = pkg.t_motor_; + t_micro_ = pkg.t_micro_; + joint_mode_ = pkg.joint_mode_; + } + virtual ~JointData() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this specific 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; + + vector6d_t q_actual_; + vector6d_t q_target_; + vector6d_t qd_actual_; + vector6f_t i_actual_; + vector6f_t v_actual_; + vector6f_t t_motor_; + vector6f_t t_micro_; // Deprecated + vector6uint8_t joint_mode_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_JOINT_DATA_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_state/kinematics_info.h b/include/ur_client_library/primary/robot_state/kinematics_info.h index 4549d4a0..e4a3a93c 100644 --- a/include/ur_client_library/primary/robot_state/kinematics_info.h +++ b/include/ur_client_library/primary/robot_state/kinematics_info.h @@ -52,6 +52,21 @@ class KinematicsInfo : public RobotState KinematicsInfo(const RobotStateType type) : RobotState(type) { } + + /*! + * \brief Creates a copy of a KinematicsInfo object. + * + * \param pkg The KinematicsInfo object to be copied + */ + KinematicsInfo(const KinematicsInfo& pkg) : RobotState(RobotStateType::JOINT_DATA) + { + checksum_ = pkg.checksum_; + dh_theta_ = pkg.dh_theta_; + dh_a_ = pkg.dh_a_; + dh_d_ = pkg.dh_d_; + dh_alpha_ = pkg.dh_alpha_; + calibration_status_ = pkg.calibration_status_; + } virtual ~KinematicsInfo() = default; /*! diff --git a/include/ur_client_library/primary/robot_state/robot_mode_data.h b/include/ur_client_library/primary/robot_state/robot_mode_data.h new file mode 100644 index 00000000..46e52ab7 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/robot_mode_data.h @@ -0,0 +1,125 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED +#define UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains data about the mode of the robot. + */ +class RobotModeData : public RobotState +{ +public: + RobotModeData() = delete; + /*! + * \brief Creates a new RobotModeData object. + * + * \param type The type of RobotState message received + */ + RobotModeData(const RobotStateType type) : RobotState(type) + { + } + + /*! + * \brief Creates a copy of a RobotModeData object. + * + * \param pkg The RobotModeData object to be copied + */ + RobotModeData(const RobotModeData& pkg) : RobotState(RobotStateType::ROBOT_MODE_DATA) + { + timestamp_ = pkg.timestamp_; + is_real_robot_connected_ = pkg.is_real_robot_connected_; + is_real_robot_enabled_ = pkg.is_real_robot_enabled_; + is_robot_power_on_ = pkg.is_robot_power_on_; + is_emergency_stopped_ = pkg.is_emergency_stopped_; + is_protective_stopped_ = pkg.is_protective_stopped_; + is_program_running_ = pkg.is_program_running_; + is_program_paused_ = pkg.is_program_paused_; + robot_mode_ = pkg.robot_mode_; + control_mode_ = pkg.control_mode_; + target_speed_fraction_ = pkg.target_speed_fraction_; + speed_scaling_ = pkg.speed_scaling_; + target_speed_fraction_limit_ = pkg.target_speed_fraction_limit_; + reserved_ = pkg.reserved_; + } + virtual ~RobotModeData() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this specific 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; + + uint64_t timestamp_; + bool is_real_robot_connected_; + bool is_real_robot_enabled_; + bool is_robot_power_on_; + bool is_emergency_stopped_; + bool is_protective_stopped_; + bool is_program_running_; + bool is_program_paused_; + uint8_t robot_mode_; + uint8_t control_mode_; + double target_speed_fraction_; + double speed_scaling_; + double target_speed_fraction_limit_; + std::string reserved_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index 7a9cbbca..02fa8a9f 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -28,6 +28,8 @@ namespace urcl { using vector3d_t = std::array; using vector6d_t = std::array; +using vector6f_t = std::array; +using vector6uint8_t = std::array; using vector6int32_t = std::array; using vector6uint32_t = std::array; diff --git a/include/ur_client_library/ur/calibration_checker.h b/include/ur_client_library/ur/calibration_checker.h index d68efed5..69dc4ec7 100644 --- a/include/ur_client_library/ur/calibration_checker.h +++ b/include/ur_client_library/ur/calibration_checker.h @@ -28,18 +28,17 @@ #ifndef UR_CLIENT_LIBRARY_UR_CALIBRATION_CHECKER_H_INCLUDED #define UR_CLIENT_LIBRARY_UR_CALIBRATION_CHECKER_H_INCLUDED -#include - +#include #include namespace urcl { /*! - * \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: /*! @@ -51,40 +50,15 @@ 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. @@ -107,10 +81,33 @@ class CalibrationChecker : public comm::IConsumer getData() + { + if (data_ == nullptr) + throw UrException("A KinematicsInfo package has not been received yet"); + return data_; + } + + /*! + * \brief Returns whether the calibration check was successful. + * + * \param expected_hash The hash expected to match the hash on the robot + * + * \returns True if the robot's expected_hash matches the one given to the checker. False + * if it doesn't match or the check was not yet performed. + */ + bool checkCalibration(const std::string& expected_hash); + private: std::string expected_hash_; bool checked_; bool matches_; + std::shared_ptr data_; }; } // namespace urcl diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 16ce45ea..f6091396 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -38,6 +38,7 @@ #include "ur_client_library/ur/tool_communication.h" #include "ur_client_library/ur/version_information.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/primary_client.h" #include "ur_client_library/rtde/rtde_writer.h" namespace urcl @@ -112,6 +113,7 @@ class UrDriver * \param tool_comm_setup Configuration for using the tool communication. * \param calibration_checksum Expected checksum of calibration. Will be matched against the * calibration reported by the robot. + * \param simulated Set the robot as simulated or real. * \param reverse_port Port that will be opened by the driver to allow direct communication between the driver * and the robot controller. * \param script_sender_port The driver will offer an interface to receive the program's URScript on this port. If @@ -129,9 +131,10 @@ class UrDriver UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum = "", - const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, - double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "", - const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004); + const bool& simulated = false, const uint32_t reverse_port = 50001, + const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, + bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, + const uint32_t script_command_port = 50004); /*! * \brief Constructs a new UrDriver object. * @@ -145,6 +148,7 @@ class UrDriver * \param headless_mode Parameter to control if the driver should be started in headless mode. * \param calibration_checksum Expected checksum of calibration. Will be matched against the * calibration reported by the robot. + * \param simulated Set the robot as simulated or real. * \param reverse_port Port that will be opened by the driver to allow direct communication between the driver * and the robot controller * \param script_sender_port The driver will offer an interface to receive the program's URScript on this port. @@ -161,13 +165,13 @@ class UrDriver */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, - const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, - const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, - bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, - const uint32_t script_command_port = 50004) + const std::string& calibration_checksum = "", const bool& simulated = false, + const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, + double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "", + const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004) : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain, - servoj_lookahead_time, non_blocking_read, reverse_ip) + std::unique_ptr{}, calibration_checksum, simulated, reverse_port, script_sender_port, + servoj_gain, servoj_lookahead_time, non_blocking_read, reverse_ip) { } @@ -351,6 +355,16 @@ class UrDriver trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb); } + /*! + * \brief Getter for the primary client + * + * \returns The used primary client + */ + std::shared_ptr getPrimaryClient() + { + return primary_client_; + } + private: std::string readScriptFile(const std::string& filename); @@ -361,8 +375,7 @@ class UrDriver std::unique_ptr trajectory_interface_; std::unique_ptr script_command_interface_; std::unique_ptr script_sender_; - std::unique_ptr> primary_stream_; - std::unique_ptr> secondary_stream_; + std::shared_ptr primary_client_; double servoj_time_; uint32_t servoj_gain_; diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index 4421be3c..3fbcea72 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -58,7 +58,10 @@ void TCPSocket::setOptions(int socket_fd) bool TCPSocket::setup(std::string& host, int port) { if (state_ == SocketState::Connected) + { + URCL_LOG_DEBUG("Socket already connected on: %s:%d", host.c_str(), port); return false; + } URCL_LOG_DEBUG("Setting up connection: %s:%d", host.c_str(), port); @@ -208,6 +211,5 @@ void TCPSocket::setReceiveTimeout(const timeval& timeout) setOptions(socket_fd_); } } - } // namespace comm } // namespace urcl diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp new file mode 100644 index 00000000..c40426a2 --- /dev/null +++ b/src/primary/primary_client.cpp @@ -0,0 +1,264 @@ +// 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 urcl +{ +namespace primary_interface +{ +PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& calibration_checksum) + : robot_ip_(robot_ip), port_(UR_PRIMARY_PORT), simulated_(false) +{ + stream_.reset(new comm::URStream(robot_ip_, port_)); + producer_.reset(new comm::URProducer(*stream_, parser_)); + producer_->setupProducer(); + + consumer_.reset(new PrimaryConsumer()); + calibration_checker_.reset(new CalibrationChecker(calibration_checksum)); + consumer_->setKinematicsInfoHandler(calibration_checker_); + + pipeline_.reset(new comm::Pipeline(*producer_, consumer_.get(), "primary pipeline", notifier_)); + pipeline_->run(); + in_remote_control_ = true; + running_ = true; + + std::chrono::duration time_done(0); + std::chrono::duration timeout = std::chrono::seconds(1); + std::chrono::duration retry_period = std::chrono::milliseconds(10); + while (!calibration_checker_->isChecked() || time_done < timeout) + { + std::this_thread::sleep_for(retry_period); + time_done += retry_period; + } + if (calibration_checker_->checkSuccessful()) + { + URCL_LOG_INFO("Calibration checked successfully."); + } + else + { + URCL_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."); + } + + try + { + // Check if robot is e-series or not + e_series_ = getVersionMessage()->major_version_ > 3; + if (e_series_) + startCheckRemoteControlThread(); + } + catch (const UrException& e) + { + URCL_LOG_WARN(e.what()); + URCL_LOG_WARN("This message is only relevant if the robot is an e-series:\nThe thread checking if the robot is in " + "remote control or not has not been started. It needs to be started manually by calling the " + "startCheckRemoteControlThread() function"); + } +} + +PrimaryClient::~PrimaryClient() +{ + URCL_LOG_DEBUG("Destructing primary client"); + stop(); +} + +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 (!in_remote_control_ && !simulated_) + { + URCL_LOG_ERROR("Not in remote control. Cannot send script whilst in local control"); + return false; + } + + if (stream_->write(data, len, written)) + { + URCL_LOG_INFO("Sent program to robot:\n%s", program_with_newline.c_str()); + return true; + } + URCL_LOG_ERROR("Could not send program to robot"); + return false; +} + +bool PrimaryClient::configure() +{ + URCL_LOG_DEBUG("Reconnecting to stream on %s:%d", robot_ip_.c_str(), port_); + pipeline_->stop(); + + if (stream_->getState() == comm::SocketState::Connected) + stream_->disconnect(); + + bool res = stream_->connect(); + pipeline_->run(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + return res; +} + +void PrimaryClient::checkRemoteLocalControl() +{ + dashboard_client_.reset(new urcl::DashboardClient(robot_ip_)); + dashboard_client_->connect(); + bool old_val = dashboard_client_->sendAndReceive("is in remote control\n") == "true"; + while (running_) + { + if (dashboard_client_->getState() == comm::SocketState::Connected) + { + std::string answer = dashboard_client_->sendAndReceive("is in remote control\n"); + in_remote_control_ = answer == "true"; + + if (old_val != in_remote_control_) + configure(); + } + else + { + URCL_LOG_INFO("Reconnecting to Dashboard Server"); + dashboard_client_->connect(); + } + old_val = in_remote_control_; + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } +} + +void PrimaryClient::stop() +{ + running_ = false; + if (rc_thread_.joinable()) + { + rc_thread_.join(); + } + pipeline_->stop(); + stream_->disconnect(); +} + +void PrimaryClient::startCheckRemoteControlThread() +{ + rc_thread_ = std::thread(&PrimaryClient::checkRemoteLocalControl, this); +} + +bool PrimaryClient::isInRemoteControl() +{ + return in_remote_control_; +} + +std::shared_ptr PrimaryClient::getAdditionalInfo() +{ + return consumer_->additional_info_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getCartesianInfo() +{ + return consumer_->cartesian_info_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getForceModeData() +{ + return consumer_->force_mode_data_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getJointData() +{ + return consumer_->joint_data_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getRobotModeData() +{ + return consumer_->robot_mode_data_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getKeyMessage() +{ + return consumer_->key_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getErrorCodeMessage() +{ + return consumer_->error_code_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getRuntimeExceptionMessage() +{ + return consumer_->runtime_exception_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getTextMessage() +{ + return consumer_->text_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getVersionMessage() +{ + return consumer_->version_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getGlobalVariablesSetupMessage() +{ + return consumer_->global_variables_setup_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getCalibrationChecker() +{ + return calibration_checker_; +} + +void PrimaryClient::setSimulated(bool value) +{ + simulated_ = value; +} + +bool PrimaryClient::getSimulated() +{ + return simulated_; +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/program_state_message.cpp b/src/primary/program_state_message.cpp new file mode 100644 index 00000000..bbaba194 --- /dev/null +++ b/src/primary/program_state_message.cpp @@ -0,0 +1,73 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 FZI Forschungszentrum Informatik (ur_robot_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 Exner exner@fzi.de + * \date 2022-02-21 + * + */ +//---------------------------------------------------------------------- +#include + +#include "ur_client_library/primary/program_state_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool ProgramStateMessage::parseWith(comm::BinParser& bp) +{ + return true; +} + +bool ProgramStateMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string ProgramStateMessage::toString() const +{ + std::stringstream ss; + ss << "timestamp: " << timestamp_ << std::endl; + ss << "Type: " << static_cast(state_type_) << std::endl; + switch (state_type_) + { + case ProgramStateMessageType::GLOBAL_VARIABLES_SETUP: + { + ss << GlobalVariablesSetupMessage(timestamp_).toString(); + break; + } + case ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE: + { + ss << GlobalVariablesUpdateMessage(timestamp_).toString(); + break; + } + default: + { + ss << "Unknown RobotStateType: " << static_cast(state_type_) << std::endl << ss.str(); + URCL_LOG_ERROR("%s", ss.str().c_str()); + } + } + return ss.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/program_state_message/global_variables_setup_message.cpp b/src/primary/program_state_message/global_variables_setup_message.cpp new file mode 100644 index 00000000..ac78c3bb --- /dev/null +++ b/src/primary/program_state_message/global_variables_setup_message.cpp @@ -0,0 +1,60 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/program_state_message/global_variables_setup_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool GlobalVariablesSetupMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(start_index_); + bp.parseRemainder(variable_names_); + + return true; // not really possible to check dynamic size packets +} + +bool GlobalVariablesSetupMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string GlobalVariablesSetupMessage::toString() const +{ + std::stringstream ss; + ss << "start index: " << start_index_ << std::endl; + ss << "variable names: " << variable_names_ << std::endl; + return ss.str(); +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/program_state_message/global_variables_update_message.cpp b/src/primary/program_state_message/global_variables_update_message.cpp new file mode 100644 index 00000000..0b23b069 --- /dev/null +++ b/src/primary/program_state_message/global_variables_update_message.cpp @@ -0,0 +1,101 @@ +// 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 2022-02-21 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/program_state_message/global_variables_update_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool GlobalVariablesUpdateMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(start_index_); + bp.parseRemainder(variables_); + + return true; // not really possible to check dynamic size packets +} + +bool GlobalVariablesUpdateMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string GlobalVariablesUpdateMessage::toString() const +{ + // int value_type; + std::stringstream ss; + ss << "start index: " << start_index_ << std::endl; + ss << "variables: (" << variables_.length() << ")"; + for (const char& c : variables_) + { + // value_type = static_cast(c); + // Parsing might not be as straight forward. Where should the global variables be stores? Maybe make functions for + // parsing different global variable types and leave it at that? + // switch (value_type) + // { + // case 0x00: + // break; + // case 0x03: + // std::cout << "const string" << std::endl; + // break; + // case 0x04: + // std::cout << "var string" << std::endl; + // break; + // case 0x0c: + // std::cout << "pose" << std::endl; + // break; + // case 0x0d: + // std::cout << "bool" << std::endl; + // break; + // case 0x0e: + // std::cout << "num" << std::endl; + // break; + // case 0x0f: + // std::cout << "int" << std::endl; + // break; + // case 0x10: + // std::cout << "float" << std::endl; + // break; + // case 0x11: + // std::cout << "list" << std::endl; + // break; + // case 0x12: + // std::cout << "matrix" << std::endl; + // break; + // } + ss << std::hex << static_cast(c) << " "; + } + ss << std::endl; + return ss.str(); +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp new file mode 100644 index 00000000..8370f448 --- /dev/null +++ b/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_client_library/log.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +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 urcl diff --git a/src/primary/robot_message/key_message.cpp b/src/primary/robot_message/key_message.cpp new file mode 100644 index 00000000..4c98bc23 --- /dev/null +++ b/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_client_library/log.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +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 urcl diff --git a/src/primary/robot_message/runtime_exception_message.cpp b/src/primary/robot_message/runtime_exception_message.cpp new file mode 100644 index 00000000..86bfa99a --- /dev/null +++ b/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_client_library/log.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +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 urcl diff --git a/src/primary/robot_message/text_message.cpp b/src/primary/robot_message/text_message.cpp new file mode 100644 index 00000000..1e544f10 --- /dev/null +++ b/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_client_library/log.h" +#include "ur_client_library/primary/robot_message/text_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +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 urcl diff --git a/src/primary/robot_message/version_message.cpp b/src/primary/robot_message/version_message.cpp index f18e07ab..30b6fd80 100644 --- a/src/primary/robot_message/version_message.cpp +++ b/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/src/primary/robot_state.cpp b/src/primary/robot_state.cpp index 15d8a276..57b848ec 100644 --- a/src/primary/robot_state.cpp +++ b/src/primary/robot_state.cpp @@ -46,11 +46,64 @@ bool RobotState::consumeWith(AbstractPrimaryConsumer& consumer) std::string RobotState::toString() const { - std::stringstream ss; - ss << "Type: " << static_cast(state_type_) << std::endl; - ss << PrimaryPackage::toString(); - return ss.str(); + std::stringstream out_ss; + out_ss << "Type: " << static_cast(state_type_) << std::endl; + // ss << PrimaryPackage::toString(); + switch (state_type_) + { + case RobotStateType::ROBOT_MODE_DATA: + { + out_ss << RobotModeData(state_type_).toString(); + break; + } + case RobotStateType::JOINT_DATA: + { + out_ss << JointData(state_type_).toString(); + break; + } + case RobotStateType::TOOL_DATA: + { + URCL_LOG_INFO("TOOL_DATA"); + break; + } + case RobotStateType::MASTERBOARD_DATA: + { + URCL_LOG_INFO("MASTERBOARD_DATA"); + break; + } + case RobotStateType::CARTESIAN_INFO: + { + out_ss << CartesianInfo(state_type_).toString(); + break; + } + case RobotStateType::KINEMATICS_INFO: + { + out_ss << KinematicsInfo(state_type_).toString(); + break; + } + case RobotStateType::CONFIGURATION_DATA: + { + URCL_LOG_INFO("CONFIGURATION_DATA"); + break; + } + case RobotStateType::FORCE_MODE_DATA: + { + out_ss << ForceModeData(state_type_).toString(); + break; + } + case RobotStateType::ADDITIONAL_INFO: + { + out_ss << AdditionalInfo(state_type_).toString(); + break; + } + default: + { + std::stringstream ss; + ss << "Unknown RobotStateType: " << static_cast(state_type_) << std::endl << out_ss.str(); + URCL_LOG_ERROR("%s", ss.str().c_str()); + } + } + return out_ss.str().c_str(); } - } // namespace primary_interface } // namespace urcl diff --git a/src/primary/robot_state/additional_info.cpp b/src/primary/robot_state/additional_info.cpp new file mode 100644 index 00000000..f62330a9 --- /dev/null +++ b/src/primary/robot_state/additional_info.cpp @@ -0,0 +1,75 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/additional_info.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool AdditionalInfo::parseWith(comm::BinParser& bp) +{ + bp.parse(tp_button_state_); + bp.parse(freedrive_button_enabled_); + bp.parse(io_enabled_freedrive_); + bp.parseRemainder(reserved_); + + return true; +} + +bool AdditionalInfo::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string AdditionalInfo::toString() const +{ + std::stringstream os; + os << "tpButtonState: " << unsigned(tp_button_state_) << std::endl; + os << "freedrive_button_enabled: "; + os << freedrive_button_enabled_ << std::endl; + os << "io_enabled_freedrive: "; + os << io_enabled_freedrive_ << std::endl; + os << "reserved: (" << reserved_.length() << ") "; + for (const char& c : reserved_) + { + os << std::hex << static_cast(c) << " "; + } + os << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/src/primary/robot_state/cartesian_info.cpp b/src/primary/robot_state/cartesian_info.cpp new file mode 100644 index 00000000..9bc46c43 --- /dev/null +++ b/src/primary/robot_state/cartesian_info.cpp @@ -0,0 +1,85 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/cartesian_info.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool CartesianInfo::parseWith(comm::BinParser& bp) +{ + bp.parse(flange_coordinates_); + bp.parse(tcp_offset_coordinates_); + + return true; +} + +bool CartesianInfo::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string CartesianInfo::toString() const +{ + std::stringstream os; + os << "flange_coordinates: ["; + for (size_t i = 0; i < flange_coordinates_.size(); ++i) + { + os << std::setprecision(15) << flange_coordinates_[i] << " "; + } + os << "]" << std::endl; + os << "tcp_offset_coordinates: ["; + for (size_t i = 0; i < tcp_offset_coordinates_.size(); ++i) + { + os << std::setprecision(15) << tcp_offset_coordinates_[i] << " "; + } + os << "]" << std::endl; + + return os.str(); +} + +// std::string CartesianInfo::toHash() const +// { +// std::stringstream ss; +// for (size_t i = 0; i < 6; ++i) +// { +// ss << flange_coordinates_[i]; +// ss << tcp_offset_coordinates_[i]; +// } +// std::hash hash_fn; +// return "calib_" + std::to_string(hash_fn(ss.str())); +// } +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/src/primary/robot_state/force_mode_data.cpp b/src/primary/robot_state/force_mode_data.cpp new file mode 100644 index 00000000..9aab121d --- /dev/null +++ b/src/primary/robot_state/force_mode_data.cpp @@ -0,0 +1,70 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/force_mode_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool ForceModeData::parseWith(comm::BinParser& bp) +{ + bp.parse(wrench_); + bp.parse(robot_dexterity_); + + return true; +} + +bool ForceModeData::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string ForceModeData::toString() const +{ + std::stringstream os; + os << "wrench: ["; + for (size_t i = 0; i < wrench_.size(); ++i) + { + os << std::setprecision(15) << wrench_[i] << " "; + } + os << "]" << std::endl; + os << "robot_dexterity: "; + os << std::setprecision(15) << robot_dexterity_ << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/src/primary/robot_state/joint_data.cpp b/src/primary/robot_state/joint_data.cpp new file mode 100644 index 00000000..3d496d4d --- /dev/null +++ b/src/primary/robot_state/joint_data.cpp @@ -0,0 +1,89 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/joint_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool JointData::parseWith(comm::BinParser& bp) +{ + for (size_t i = 0; i < q_actual_.size(); i++) + { + bp.parse(q_actual_.at(i)); + bp.parse(q_target_.at(i)); + bp.parse(qd_actual_.at(i)); + bp.parse(i_actual_.at(i)); + bp.parse(v_actual_.at(i)); + bp.parse(t_motor_.at(i)); + bp.parse(t_micro_.at(i)); // Deprecated + bp.parse(joint_mode_.at(i)); + } + + return true; +} + +bool JointData::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string JointData::toString() const +{ + std::stringstream os; + os << "q_actual: " << q_actual_ << std::endl; + + os << "q_target: " << q_target_ << std::endl; + + os << "qd_actual: " << qd_actual_ << std::endl; + + os << "I_actual: " << i_actual_ << std::endl; + + os << "V_actual: " << v_actual_ << std::endl; + + os << "T_motor: " << t_motor_ << std::endl; + + os << "joint_mode: ["; + for (size_t i = 0; i < joint_mode_.size(); ++i) + { + os << unsigned(joint_mode_[i]) << " "; + } + os << "]" << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_state/robot_mode_data.cpp b/src/primary/robot_state/robot_mode_data.cpp new file mode 100644 index 00000000..17f6ec9d --- /dev/null +++ b/src/primary/robot_state/robot_mode_data.cpp @@ -0,0 +1,93 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool RobotModeData::parseWith(comm::BinParser& bp) +{ + bp.parse(timestamp_); + bp.parse(is_real_robot_connected_); + bp.parse(is_real_robot_enabled_); + bp.parse(is_robot_power_on_); + bp.parse(is_emergency_stopped_); + bp.parse(is_protective_stopped_); + bp.parse(is_program_running_); + bp.parse(is_program_paused_); + bp.parse(robot_mode_); + bp.parse(control_mode_); + bp.parse(target_speed_fraction_); + bp.parse(speed_scaling_); + bp.parse(target_speed_fraction_limit_); + bp.parseRemainder(reserved_); + + return true; +} + +bool RobotModeData::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string RobotModeData::toString() const +{ + std::stringstream os; + os << "Timestamp: " << timestamp_ << std::endl; + os << "Is real robot connected: " << is_real_robot_connected_ << std::endl; + os << "Is real robot enabled: " << is_real_robot_enabled_ << std::endl; + os << "Is robot power on: " << is_robot_power_on_ << std::endl; + os << "Is emergency stopped: " << is_emergency_stopped_ << std::endl; + os << "Is protective stopped: " << is_protective_stopped_ << std::endl; + os << "Is program running: " << is_program_running_ << std::endl; + os << "Is program paused: " << is_program_paused_ << std::endl; + os << "Robot mode: " << unsigned(robot_mode_) << std::endl; + os << "Control mode: " << unsigned(control_mode_) << std::endl; + os << "Target speed fraction: " << target_speed_fraction_ << std::endl; + os << "Speed scaling: " << speed_scaling_ << std::endl; + os << "Target speed fraction limit: " << target_speed_fraction_limit_ << std::endl; + os << "Reserved: ( " << reserved_.length() << ")"; + for (const char& c : reserved_) + { + os << std::hex << static_cast(c) << ", "; + } + os << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/src/ur/calibration_checker.cpp b/src/ur/calibration_checker.cpp index b7da09ed..870924c7 100644 --- a/src/ur/calibration_checker.cpp +++ b/src/ur/calibration_checker.cpp @@ -34,18 +34,24 @@ CalibrationChecker::CalibrationChecker(const std::string& expected_hash) : expected_hash_(expected_hash), checked_(false), matches_(false) { } -bool CalibrationChecker::consume(std::shared_ptr product) + +void CalibrationChecker::handle(primary_interface::KinematicsInfo& kin_info) +{ + // URCL_LOG_INFO("%s", kin_info.toString().c_str()); + // + matches_ = kin_info.toHash() == expected_hash_; + + checked_ = true; + + data_.reset(new primary_interface::KinematicsInfo(kin_info)); +} + +bool CalibrationChecker::checkCalibration(const std::string& expected_hash) { - auto kin_info = std::dynamic_pointer_cast(product); - if (kin_info != nullptr) - { - // URCL_LOG_INFO("%s", product->toString().c_str()); - // - matches_ = kin_info->toHash() == expected_hash_; + matches_ = expected_hash == expected_hash_; - checked_ = true; - } + checked_ = true; - return true; + return matches_; } } // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index d1036b2d..e4eef0e2 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -53,9 +53,10 @@ static const std::string SCRIPT_COMMAND_PORT_REPLACE("{{SCRIPT_COMMAND_SERVER_PO urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, - std::unique_ptr tool_comm_setup, const uint32_t reverse_port, - const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, - bool non_blocking_read, const std::string& reverse_ip, const uint32_t trajectory_port, + std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, + const bool& simulated, const uint32_t reverse_port, const uint32_t script_sender_port, + int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, + const std::string& reverse_ip, const uint32_t trajectory_port, const uint32_t script_command_port) : servoj_time_(0.008) , servoj_gain_(servoj_gain) @@ -66,12 +67,9 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ URCL_LOG_DEBUG("Initializing urdriver"); URCL_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_, urcl::primary_interface::UR_PRIMARY_PORT)); - secondary_stream_.reset( - new comm::URStream(robot_ip_, urcl::primary_interface::UR_SECONDARY_PORT)); - secondary_stream_->connect(); + URCL_LOG_DEBUG("Initializing Primary client"); + primary_client_.reset(new primary_interface::PrimaryClient(robot_ip, calibration_checksum)); + primary_client_->setSimulated(simulated); non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -179,31 +177,14 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, - 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, const std::string& reverse_ip, - const uint32_t trajectory_port, const uint32_t script_command_port) + std::unique_ptr tool_comm_setup, const uint32_t reverse_port, + const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, + bool non_blocking_read, const std::string& reverse_ip, const uint32_t trajectory_port, + const uint32_t script_command_port) : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, - non_blocking_read, reverse_ip, trajectory_port) + std::move(tool_comm_setup), "", false, reverse_port, script_sender_port, servoj_gain, + servoj_lookahead_time, non_blocking_read, reverse_ip, trajectory_port, script_command_port) { - URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " - "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This " - "notice is for application developers using this library. If you are only using an application using " - "this library, you can ignore this message."); - if (checkCalibration(calibration_checksum)) - { - URCL_LOG_INFO("Calibration checked successfully."); - } - else - { - URCL_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."); - } } std::unique_ptr urcl::UrDriver::getDataPackage() @@ -339,27 +320,7 @@ std::string UrDriver::readScriptFile(const std::string& filename) bool 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)); - } - URCL_LOG_DEBUG("Got calibration information from robot."); - return consumer.checkSuccessful(); + return primary_client_->getCalibrationChecker()->checkCalibration(checksum); } rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() @@ -369,29 +330,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)) - { - URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); - return true; - } - URCL_LOG_ERROR("Could not send program to robot"); - return false; + return primary_client_->sendScript(program); } bool UrDriver::sendRobotProgram() diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ea8bb011..c36cf460 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -44,11 +44,19 @@ if (INTEGRATION_TESTS) target_link_libraries(dashboard_client_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) gtest_add_tests(TARGET dashboard_client_tests ) + + add_executable(primary_client_tests test_primary_client.cpp) + target_compile_options(primary_client_tests PRIVATE ${CXX17_FLAG}) + target_include_directories(primary_client_tests PRIVATE ${GTEST_INCLUDE_DIRS}) + target_link_libraries(primary_client_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) + gtest_add_tests(TARGET primary_client_tests + ) else() message(STATUS "Skipping integration tests.") endif() + add_executable(primary_parser_tests test_primary_parser.cpp) target_compile_options(primary_parser_tests PRIVATE ${CXX17_FLAG}) target_include_directories(primary_parser_tests PRIVATE ${GTEST_INCLUDE_DIRS}) diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp new file mode 100644 index 00000000..2fc19296 --- /dev/null +++ b/tests/test_primary_client.cpp @@ -0,0 +1,303 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include + +#include +#define private public +#include + +using namespace urcl; + +std::string ROBOT_IP = "127.0.0.1"; +int PRIMARY_PORT = 30001; +int DASHBOARD_PORT = 29999; +int FAKE_PRIMARY_PORT = 60061; +int FAKE_DASHBOARD_PORT = 60059; + +class PrimaryClientTest : public ::testing::Test +{ +protected: + void SetUp() + { + in_remote_control_ = true; + } + + void TearDown() + { + dashboard_server_.reset(); + primary_server_.reset(); + client_.reset(); + } + + void run() + { + unsigned char message[] = { 0x00, 0x00, 0x00, 0x50, 0x19, 0x00, 0x00, 0x00, + 0x00, 0x56, 0x76, 0xd3, 0xa0, 0x00, 0x00, 0x00 }; // Empty GlobalVariablesSetupMessage + size_t len = sizeof(message); + size_t written; + while (running_) + { + primary_server_->write(client_fd_, message, len, written); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + + void stopThread() + { + URCL_LOG_DEBUG("Shutting down thread"); + running_ = false; + if (server_thread_.joinable()) + { + server_thread_.join(); + } + } + + void connectionCallback(const int filedescriptor) + { + std::lock_guard lk(connect_mutex_); + client_fd_ = filedescriptor; + connect_cv_.notify_one(); + connection_callback_ = true; + } + + void connectionCallbackDB(const int filedescriptor) + { + std::lock_guard lk(connect_mutex_db_); + client_fd_db_ = filedescriptor; + + unsigned char message[] = { + 0x43, 0x6f, 0x6e, 0x6e, 0x65, 0x63, 0x74, 0x65, 0x64, 0x3a, 0x20, 0x55, 0x6e, 0x69, 0x76, 0x65, 0x72, 0x73, 0x61, + 0x6c, 0x20, 0x52, 0x6f, 0x62, 0x6f, 0x74, 0x73, 0x20, 0x46, 0x61, 0x6b, 0x65, 0x20, 0x54, 0x65, 0x73, 0x74, 0x20, + 0x44, 0x61, 0x73, 0x68, 0x62, 0x6f, 0x61, 0x72, 0x64, 0x20, 0x53, 0x65, 0x72, 0x76, 0x65, 0x72, 0x0a + }; // "Connected: Universal Robots Fake Test Dashboard Server\n" + size_t len = sizeof(message); + size_t written; + dashboard_server_->write(client_fd_db_, message, len, written); + connect_cv_db_.notify_one(); + connection_callback_db_ = true; + } + + void messageCallback(const int filedescriptor, char* buffer) + { + std::lock_guard lk(message_mutex_); + message_ = std::string(buffer); + message_cv_.notify_one(); + message_callback_ = true; + } + + void messageCallbackDB(const int filedescriptor, char* buffer) + { + std::lock_guard lk(message_mutex_db_); + message_db_ = std::string(buffer); + if (message_db_ == "is in remote control\n") + { + unsigned char message_true[] = { 0x74, 0x72, 0x75, 0x65, 0x0a }; // "true\n" + unsigned char message_false[] = { 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a }; // "false\n" + + size_t len = in_remote_control_ ? sizeof(message_true) : sizeof(message_false); + size_t written; + dashboard_server_->write(client_fd_db_, in_remote_control_ ? message_true : message_false, len, written); + } + if (message_db_ == "PolyscopeVersion\n") + { + unsigned char message_pv[] = { + 0x55, 0x52, 0x53, 0x6f, 0x66, 0x74, 0x77, 0x61, 0x72, 0x65, 0x20, 0x35, 0x2e, 0x31, + 0x32, 0x2e, 0x32, 0x2e, 0x31, 0x31, 0x30, 0x31, 0x35, 0x33, 0x34, 0x20, 0x28, 0x4a, + 0x75, 0x6c, 0x20, 0x30, 0x36, 0x20, 0x32, 0x30, 0x32, 0x32, 0x29, 0x0a + }; // URSoftware 5.12.2.1101534 (Jul 06 2022) + size_t len = sizeof(message_pv); + size_t written; + dashboard_server_->write(client_fd_db_, message_pv, len, written); + } + + message_cv_db_.notify_one(); + message_callback_db_ = true; + } + + bool waitForConnectionCallback(int milliseconds = 100) + { + std::unique_lock lk(connect_mutex_); + if (connect_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + connection_callback_ == true) + { + connection_callback_ = false; + return true; + } + else + { + return false; + } + } + + bool waitForMessageCallback(int milliseconds = 100) + { + std::unique_lock lk(message_mutex_); + if (message_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + message_callback_ == true) + { + message_callback_ = false; + return true; + } + else + { + return false; + } + } + + bool waitForMessageCallbackDB(int milliseconds = 100) + { + std::unique_lock lk(message_mutex_db_); + if (message_cv_db_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + message_callback_db_ == true) + { + message_callback_db_ = false; + return true; + } + else + { + return false; + } + } + + std::string message_ = ""; + std::string message_db_ = ""; + int client_fd_ = -1; + int client_fd_db_ = -1; + + std::atomic running_; + std::thread server_thread_; + std::unique_ptr client_; + std::unique_ptr primary_server_; + std::unique_ptr dashboard_server_; + + std::condition_variable connect_cv_, connect_cv_db_; + std::mutex connect_mutex_, connect_mutex_db_; + + std::condition_variable message_cv_, message_cv_db_; + std::mutex message_mutex_, message_mutex_db_; + + bool connection_callback_ = false; + bool connection_callback_db_ = false; + bool message_callback_ = false; + bool message_callback_db_ = false; + + bool in_remote_control_; +}; + +TEST_F(PrimaryClientTest, check_remote_control) +{ + dashboard_server_.reset(new comm::TCPServer(FAKE_DASHBOARD_PORT)); + primary_server_.reset(new comm::TCPServer(FAKE_PRIMARY_PORT)); + primary_server_->setMessageCallback(std::bind(&PrimaryClientTest_check_remote_control_Test::messageCallback, this, + std::placeholders::_1, std::placeholders::_2)); + dashboard_server_->setMessageCallback(std::bind(&PrimaryClientTest_check_remote_control_Test::messageCallbackDB, this, + std::placeholders::_1, std::placeholders::_2)); + primary_server_->setConnectCallback( + std::bind(&PrimaryClientTest_check_remote_control_Test::connectionCallback, this, std::placeholders::_1)); + dashboard_server_->setConnectCallback( + std::bind(&PrimaryClientTest_check_remote_control_Test::connectionCallbackDB, this, std::placeholders::_1)); + dashboard_server_->start(); + primary_server_->start(); + server_thread_ = std::thread(&PrimaryClientTest_check_remote_control_Test::run, this); + + std::unique_ptr temp_client; + client_.reset(new primary_interface::PrimaryClient(ROBOT_IP, "")); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Let connections set up + + if (!client_->e_series_) + { + client_->startCheckRemoteControlThread(); + } + + // Disconnect from URSim servers and connect to fake servers + client_->pipeline_->stop(); + client_->stream_->disconnect(); + client_->dashboard_client_->disconnect(); + client_->dashboard_client_->port_ = FAKE_DASHBOARD_PORT; + client_->stream_->port_ = FAKE_PRIMARY_PORT; + client_->stream_->connect(); + client_->dashboard_client_->connect(); + client_->pipeline_->run(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // let connections set up + + // When in_remote_control_ is true the primary client should be able to send script + in_remote_control_ = true; + client_->sendScript("true\n"); + EXPECT_TRUE(waitForMessageCallback(1000)); + + // Make sure thread sets in_remote_control_ to false and primary client has time to reconnect + in_remote_control_ = false; + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // When in_remote_control_ is false the primary client NOT should be able to send script + client_->sendScript("false\n"); + EXPECT_FALSE(waitForMessageCallback(1000)); + + stopThread(); +} + +TEST_F(PrimaryClientTest, send_script) +{ + client_.reset(new primary_interface::PrimaryClient(ROBOT_IP, "")); + client_->setSimulated(true); + std::stringstream cmd; + cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' + cmd << "sec setup():" << std::endl + << " textmsg(\"Command through primary interface complete \")" << std::endl + << "end"; + + std::string script_code = cmd.str(); + auto program_with_newline = script_code + '\n'; + // Should always return true in pipeline as robot is simulated + EXPECT_TRUE(client_->sendScript(program_with_newline)); +} + +TEST_F(PrimaryClientTest, get_data) +{ + client_.reset(new primary_interface::PrimaryClient(ROBOT_IP, "")); + client_->setSimulated(true); + EXPECT_EQ(client_->getVersionMessage()->build_number_, 0); + vector6d_t zero_array = { 0 }; + EXPECT_EQ(client_->getCartesianInfo()->tcp_offset_coordinates_, zero_array); + EXPECT_EQ(client_->getForceModeData()->wrench_, zero_array); + // getGlobalVariablesSetupMessage() will throw an exception since a program on the robot has not been started while + // this client has been connected + EXPECT_THROW(client_->getGlobalVariablesSetupMessage()->variable_names_, UrException); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/tests/test_primary_parser.cpp b/tests/test_primary_parser.cpp index cba6d99a..4666b9db 100644 --- a/tests/test_primary_parser.cpp +++ b/tests/test_primary_parser.cpp @@ -33,116 +33,338 @@ using namespace urcl; +unsigned char raw_data[] = { + 0x00, 0x00, 0x05, 0xbf, 0x10, 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x04, 0x9c, 0x87, 0x7f, 0xf8, 0x01, + 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb, 0x01, + 0x40, 0x15, 0x89, 0xb9, 0xa0, 0x00, 0x00, 0x00, 0x40, 0x15, 0x89, 0xb7, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0xbd, 0x57, 0x18, 0x34, 0x42, 0x3e, 0x74, 0x53, 0x41, 0xed, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xfd, 0xbf, 0xf7, 0x97, 0xb5, 0x8c, 0x52, 0x45, 0x50, 0xbf, 0xf7, 0x97, 0xbd, 0x58, 0x52, 0x45, 0x50, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x27, 0xa5, 0x09, 0x42, 0x3e, 0x65, 0xa5, 0x41, 0xf4, 0x80, + 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xc0, 0x05, 0xc9, 0x52, 0x60, 0x00, 0x00, 0x00, 0xc0, 0x05, 0xc9, 0x59, 0x60, + 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x85, 0xc5, 0x81, 0x42, 0x3e, 0x83, 0x00, + 0x41, 0xf5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0xf3, 0x14, 0x8c, 0xcc, 0x52, 0x45, 0x50, 0xbf, 0xf3, + 0x14, 0xa4, 0x6c, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x77, 0x02, 0x3b, 0x42, + 0x3e, 0x65, 0xa5, 0x42, 0x0f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0x96, 0xed, 0xd4, 0x44, 0x2d, 0x18, + 0x00, 0xbf, 0x96, 0xe5, 0x34, 0x44, 0x2d, 0x18, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbe, 0x37, + 0x2a, 0x27, 0x42, 0x3e, 0x83, 0x00, 0x42, 0x1d, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x40, 0x08, 0x13, 0x72, + 0x40, 0x00, 0x00, 0x00, 0x40, 0x08, 0x13, 0x72, 0xc0, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3c, 0xbd, 0x2b, 0x48, 0x42, 0x3e, 0x39, 0x9e, 0x42, 0x22, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, + 0x00, 0x00, 0x65, 0x04, 0xbf, 0xa3, 0x56, 0x0a, 0xd8, 0xab, 0xe1, 0xc1, 0xbf, 0xd4, 0xdf, 0x01, 0x9c, 0x33, 0x4b, + 0xe9, 0x3f, 0xc7, 0x82, 0xfd, 0x1e, 0xfc, 0x1f, 0xe5, 0xbf, 0xf9, 0x19, 0xa6, 0xcb, 0xa7, 0xcf, 0x17, 0xbf, 0xf7, + 0x4b, 0x73, 0x19, 0x28, 0x8c, 0xfc, 0x40, 0x01, 0x6a, 0xf6, 0xae, 0x78, 0xab, 0xf9, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe1, 0x05, 0xad, 0x33, 0xdc, 0x43, 0xad, 0x52, 0x81, 0xf1, 0x2e, + 0x6a, 0x72, 0x7e, 0xad, 0xe9, 0x54, 0x2d, 0xad, 0xe8, 0xf5, 0x9e, 0xad, 0xeb, 0x88, 0x89, 0x3e, 0x58, 0xb7, 0x52, + 0x37, 0x00, 0x00, 0x00, 0x3f, 0xda, 0x3d, 0xdc, 0x79, 0x9e, 0xb9, 0xec, 0x40, 0x14, 0xed, 0x48, 0x93, 0xe2, 0x13, + 0x60, 0x3f, 0xe4, 0x86, 0xb0, 0xe7, 0xf1, 0xca, 0x9e, 0x3e, 0x84, 0x41, 0x1e, 0x96, 0x7e, 0x00, 0x00, 0x3e, 0xa0, + 0x82, 0x46, 0x96, 0x5a, 0x00, 0x00, 0x3f, 0x26, 0x18, 0x3b, 0x6b, 0xe3, 0xed, 0x33, 0xbf, 0xd8, 0xf7, 0x98, 0xba, + 0x58, 0x2a, 0x97, 0xbf, 0xd4, 0x19, 0xd6, 0x44, 0x6b, 0x34, 0x85, 0x3f, 0x1f, 0xaa, 0x89, 0x47, 0x6c, 0x84, 0x7a, + 0xbe, 0xef, 0x01, 0x0f, 0xfb, 0xb4, 0x20, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xc1, + 0x5f, 0x95, 0x34, 0x38, 0x6a, 0x40, 0x64, 0x04, 0xe3, 0x36, 0x9b, 0xf9, 0x53, 0xc0, 0x6b, 0xab, 0x59, 0xb7, 0xa6, + 0xa2, 0xec, 0x40, 0x4e, 0xaa, 0xec, 0x6f, 0x63, 0x01, 0xfd, 0x3f, 0xb9, 0x83, 0xdc, 0xec, 0xb9, 0x10, 0x9a, 0x3f, + 0xb9, 0x87, 0x6a, 0x0b, 0xb9, 0xb4, 0xfc, 0x3f, 0xf9, 0x21, 0x17, 0x86, 0x80, 0x32, 0xe0, 0x3f, 0x51, 0x58, 0xdb, + 0xb7, 0x00, 0x50, 0xb6, 0x3f, 0x6f, 0x65, 0x0e, 0x77, 0x42, 0xe7, 0x3e, 0x3f, 0xf9, 0x1a, 0xa7, 0x3b, 0xc0, 0xb4, + 0xeb, 0xbf, 0xf9, 0x2a, 0x45, 0x93, 0x44, 0x14, 0xb9, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x35, 0x09, 0x3f, 0xe2, 0x52, 0x62, 0x73, 0x85, 0xeb, 0x0c, 0x3f, 0xfa, 0x55, 0xb0, + 0x75, 0xda, 0xac, 0x08, 0x3f, 0xfc, 0x18, 0x09, 0x2b, 0xcd, 0x55, 0x80, 0x3f, 0xd4, 0x53, 0x0f, 0x7a, 0xa0, 0x7f, + 0xcc, 0x3f, 0xcd, 0xf4, 0x17, 0x0c, 0xcd, 0xec, 0xc8, 0xbf, 0xc7, 0x8e, 0x59, 0x16, 0xa2, 0xec, 0x24, 0x00, 0x00, + 0x00, 0x4b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x3f, 0xbc, 0x8d, 0x9c, 0x00, 0x00, + 0x00, 0x00, 0x3f, 0xc3, 0x77, 0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, + 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, 0x00, 0x42, 0x08, 0xc0, 0x00, 0x42, 0x3e, 0xd9, 0x0d, 0x3f, 0x0c, + 0x14, 0xc3, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x65, 0xf5, 0xbb, 0x2b, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, + 0x25, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x3c, 0x58, 0xc4, 0x40, 0x42, 0x1f, 0x80, 0x00, 0xfd, 0x00, 0x00, 0x01, 0xbd, + 0x06, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, + 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, + 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, + 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, + 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, + 0x2f, 0x63, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, + 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, + 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, + 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x3f, 0xf0, 0xc1, 0x52, 0x38, 0x2d, 0x73, 0x65, 0x3f, 0xf6, 0x57, 0x18, 0x4a, 0xe7, 0x44, 0x87, + 0x3f, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf3, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3f, 0xd0, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, 0x33, + 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, 0xcc, + 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, 0xb9, + 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, + 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, + 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x3d, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x3e, 0x80, 0x4a, 0xad, 0x81, 0xf7, 0x98, 0x78, 0x00, 0x00, 0x00, 0x09, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, + 0x00, 0x2b, 0x0a, 0x65, 0xf5, 0xbb, 0x2b, 0x00, 0x01, 0xbf, 0xb9, 0x24, 0x07, 0xe2, 0x9d, 0x71, 0xb2, 0xbf, 0x9a, + 0x90, 0xa7, 0x16, 0xd3, 0x2a, 0x62, 0x3f, 0xe2, 0xbc, 0xae, 0xd2, 0xd2, 0xce, 0x76, 0x3f, 0xc0, 0xa3, 0xd7, 0x0a, + 0x3d, 0x70, 0xa4, 0x00, 0x00, 0x00, 0x1a, 0x0b, 0x01, 0x00, 0x0f, 0x42, 0x40, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, + 0x00, 0x01, 0x3f, 0xc0, 0x00, 0x00, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x0c, 0x00, 0x01, 0x01, 0x00, + 0x00, 0x00, 0x55, 0x0e, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x59, + 0x37, 0x08, 0xaa, 0xc9, 0x6c, 0xc6, 0x3f, 0x59, 0x37, 0x08, 0xaa, 0xc9, 0x6c, 0xc6, 0x3f, 0x59, 0x37, 0x08, 0xaa, + 0xc9, 0x6c, 0xc6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 +}; + +/* First RobotState of UR10e from real robot 5.11 + * + * This package contains: + * - ROBOT_MODE_DATA, + * - JOINT_DATA, + * - CARTESIAN_INFO, + * - KINEMATICS_INFO, + * - CALIBRATION_DATA + * - MASTERBOARD_DATA, + * - TOOL_DATA, + * - CONFIGURATION_DATA, + * - FORCE_MODE_DATA, + * - ADDITIONAL_INFO, + * - SAFETY_DATA + * - TOOL_COMMUNICATION_INFO, + * - TOOL_MODE_INFO, + * - SINGULARITY_INFO + * - TYPE_14 + */ + +TEST(primary_parser, parse_robot_mode_data) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::RobotModeData* data = dynamic_cast(products[0].get())) + { + EXPECT_EQ(data->timestamp_, 19805995000); + EXPECT_EQ(data->is_real_robot_connected_, true); + EXPECT_EQ(data->is_real_robot_enabled_, true); + EXPECT_EQ(data->is_robot_power_on_, true); + EXPECT_EQ(data->is_emergency_stopped_, false); + EXPECT_EQ(data->is_protective_stopped_, false); + EXPECT_EQ(data->is_program_running_, false); + EXPECT_EQ(data->is_program_paused_, false); + EXPECT_EQ(unsigned(data->robot_mode_), 7); // Robot mode running + EXPECT_EQ(unsigned(data->control_mode_), 0); // Control mode position + EXPECT_EQ(data->target_speed_fraction_, 1); + EXPECT_EQ(data->speed_scaling_, 0); + EXPECT_EQ(data->target_speed_fraction_limit_, 1); + EXPECT_EQ(static_cast(data->reserved_[0]), 0); + } + else + { + std::cout << "Failed to get robot mode data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_joint_data) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::JointData* data = dynamic_cast(products[1].get())) + { + vector6d_t q_actual = { 5.384497166, -1.474538372, -2.723301649, -1.192517088, -0.02239162127, 3.009495258 }; + for (size_t i = 0; i < data->q_actual_.size(); ++i) + { + EXPECT_NEAR(data->q_actual_[i], q_actual[i], std::abs(q_actual[i] * 1.0e-5)); + } + vector6d_t q_target = { 5.384488583, -1.474545808, -2.723315001, -1.192539619, -0.0223587195, 3.009496212 }; + for (size_t i = 0; i < data->q_target_.size(); ++i) + { + EXPECT_NEAR(data->q_target_[i], q_target[i], std::abs(q_target[i] * 1.0e-5)); + } + EXPECT_EQ(data->qd_actual_, vector6d_t({ 0, 0, 0, 0, 0, 0 })); + vector6d_t i_actual = { -0.05251331627, 0.6548619866, 1.045089841, 0.2412194461, -0.1788717359, 0.02309192717 }; + for (size_t i = 0; i < data->i_actual_.size(); ++i) + { + EXPECT_NEAR(data->i_actual_[i], i_actual[i], std::abs(i_actual[i] * 1.0e-5)); + } + vector6d_t v_actual = { 47.61359787, 47.59926224, 47.62792969, 47.59926224, 47.62792969, 47.55626678 }; + for (size_t i = 0; i < data->v_actual_.size(); ++i) + { + EXPECT_NEAR(data->v_actual_[i], v_actual[i], std::abs(v_actual[i] * 1.0e-5)); + } + EXPECT_EQ(data->t_motor_, vector6f_t({ 29.625, 30.5625, 30.625, 35.875, 39.3125, 40.625 })); + for (size_t i = 0; i < data->joint_mode_.size(); ++i) + { + EXPECT_EQ(unsigned(data->joint_mode_[i]), 253); + } + } + else + { + std::cout << "Failed to get joint data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_cartesian_info) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::CartesianInfo* data = dynamic_cast(products[2].get())) + { + vector6d_t flange_coordinates = { -0.03776582618, -0.3261112237, 0.1836849595, + -1.568762584, -1.455920313, 2.17722832 }; + for (size_t i = 0; i < data->flange_coordinates_.size(); ++i) + { + EXPECT_NEAR(data->flange_coordinates_[i], flange_coordinates[i], std::abs(flange_coordinates[i] * 1.0e-5)); + } + vector6d_t tcp_offset_coordinates = { 0, 0, 0, 0, 0, 0 }; + for (size_t i = 0; i < data->tcp_offset_coordinates_.size(); ++i) + { + EXPECT_NEAR(data->tcp_offset_coordinates_[i], tcp_offset_coordinates[i], + std::abs(tcp_offset_coordinates[i] * 1.0e-5)); + } + } + else + { + std::cout << "Failed to get cartesian info" << std::endl; + GTEST_FAIL(); + } +} + TEST(primary_parser, parse_calibration_data) { - /* First RobotState of UR5e from URSim v5.8 - * - * This package contains: - * - ROBOT_MODE_DATA - * - JOINT_DATA - * - CARTESIAN_INFO - * - KINEMATICS_INFO - * - NEEDED_FOR_CALIB_DATA - * - MASTERBOARD_DATA - * - TOOL_DATA - * - CONFIGURATION_DATA - * - FORCE_MODE_DATA - * - ADDITIONAL_INFO - * - SAFETY_DATA - * - TOOL_COMM_INFO - * - TOOL_MODE_INFO - */ - unsigned char raw_data[] = { - 0x00, 0x00, 0x05, 0x6a, 0x10, 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x05, 0xf8, 0x7d, 0x17, 0x40, 0x01, - 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x3f, 0xef, 0x0a, 0x3d, 0x70, 0xa3, 0xd7, 0x0a, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb, 0x01, - 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0xa3, 0xa8, 0x79, 0x38, 0x00, 0x00, 0x00, 0x00, 0x41, 0xcc, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xfd, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x09, 0x06, 0x5a, 0x00, 0x00, 0x00, 0x00, 0x41, 0xc8, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xc0, 0x01, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x38, 0xc0, 0x01, 0x9f, 0xbe, 0x76, - 0xc8, 0xb4, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xbf, 0x0f, 0xf7, 0x00, 0x00, 0x00, 0x00, - 0x41, 0xc4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0xe9, 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0xbf, 0xe9, - 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x6e, 0xbb, 0xe2, 0x00, - 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, - 0xf6, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, 0xf6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3a, 0xe6, - 0x05, 0x69, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0x9f, 0xbe, 0x76, - 0xc8, 0xb4, 0x39, 0x00, 0xbf, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x39, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, - 0x00, 0x00, 0x65, 0x04, 0xbf, 0xc2, 0x6d, 0x90, 0xa0, 0x1d, 0xe7, 0x77, 0xbf, 0xdb, 0xe1, 0x32, 0xf6, 0xa8, 0x66, - 0x44, 0x3f, 0xc9, 0xdc, 0x1e, 0xb0, 0x03, 0x31, 0xe6, 0xbf, 0x54, 0x02, 0xc0, 0xf8, 0xe6, 0xe6, 0x79, 0x40, 0x08, - 0xee, 0x22, 0x63, 0x78, 0xfa, 0xe0, 0x3f, 0xa3, 0xe9, 0xa4, 0x23, 0x7a, 0x7b, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe1, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, - 0x33, 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, - 0xcc, 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, - 0xb9, 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, - 0x50, 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x4b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, - 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x42, 0x40, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, - 0x25, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, 0x00, 0x01, 0xbd, - 0x06, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, - 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, - 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, - 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, - 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, - 0x2f, 0x63, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, - 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, - 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, - 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x3f, 0xf0, 0xc1, 0x52, 0x38, 0x2d, 0x73, 0x65, 0x3f, 0xf6, 0x57, 0x18, 0x4a, 0xe7, 0x44, 0x87, - 0x3f, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf3, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3f, 0xd0, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, 0x33, - 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, 0xcc, - 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, 0xb9, - 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, - 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, - 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x3d, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x3f, 0x6c, 0xf5, 0xac, 0x1d, 0xb9, 0xa1, 0x08, 0x00, 0x00, 0x00, 0x09, 0x08, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, - 0x00, 0x2b, 0x0a, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0xbf, 0xb8, 0x4d, 0xc2, 0x84, 0x9f, 0xed, 0xcf, 0xbf, 0xb0, - 0x37, 0x9e, 0xd0, 0x87, 0xba, 0x97, 0x3f, 0xe2, 0xa2, 0x5b, 0x78, 0xc3, 0x9f, 0x6c, 0x3f, 0xc0, 0xa3, 0xd7, 0x0a, - 0x3d, 0x70, 0xa4, 0x00, 0x00, 0x00, 0x1a, 0x0b, 0x00, 0x00, 0x01, 0xc2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x01, 0x3f, 0xc0, 0x00, 0x00, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x0c, 0x00, 0x01, 0x01 - }; comm::BinParser bp(raw_data, sizeof(raw_data)); std::vector> products; primary_interface::PrimaryParser parser; parser.parse(bp, products); - EXPECT_EQ(products.size(), 13); + EXPECT_EQ(products.size(), 14); if (primary_interface::KinematicsInfo* data = dynamic_cast(products[3].get())) { EXPECT_EQ(data->checksum_, - vector6uint32_t({ 4294967295, 4294967295, 4294967295, 4294967295, 4294967295, 4294967295 })); - EXPECT_EQ(data->dh_theta_, vector6d_t({ 0, 0, 0, 0, 0, 0 })); - EXPECT_EQ(data->dh_a_, vector6d_t({ 0, -0.425, -0.3922, 0, 0, 0 })); - EXPECT_EQ(data->dh_d_, vector6d_t({ 0.1625, 0, 0, 0.1333, 0.0997, 0.0996 })); - EXPECT_EQ(data->dh_alpha_, vector6d_t({ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 })); + vector6uint32_t({ 2905857091, 2907865585, 778728062, 2917749805, 2917725598, 2917894281 })); + vector6d_t dh_theta = { 2.30187e-08, 0.410026, 5.23172, 0.641442, 1.50907e-07, 4.92003e-07 }; + for (size_t i = 0; i < data->dh_theta_.size(); ++i) + { + EXPECT_NEAR(data->dh_theta_[i], dh_theta[i], std::abs(dh_theta[i] * 1.0e-5)); + } + vector6d_t dh_a = { 0.000168569, -0.390112, -0.314077, 0.000120797, -1.47839e-05, 0 }; + for (size_t i = 0; i < data->dh_a_.size(); ++i) + { + EXPECT_NEAR(data->dh_a_[i], dh_a[i], std::abs(dh_a[i] * -1.0e-5)); + } + vector6d_t dh_d = { 0.162151, 160.153, -221.355, 61.3353, 0.0996683, 0.0997225 }; + for (size_t i = 0; i < data->dh_d_.size(); ++i) + { + EXPECT_NEAR(data->dh_d_[i], dh_d[i], std::abs(dh_d[i] * 1.0e-5)); + } + vector6d_t dh_alpha = { 1.57058, 0.00105878, 0.00383237, 1.56901, -1.57282, 0 }; + for (size_t i = 0; i < data->dh_alpha_.size(); ++i) + { + EXPECT_NEAR(data->dh_alpha_[i], dh_alpha[i], std::abs(dh_alpha[i] * 1.0e-5)); + } + } + else + { + std::cout << "Failed to get kinematics info" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_force_mode_data) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::ForceModeData* data = dynamic_cast(products[8].get())) + { + vector6d_t wrench = { 0, 0, 0, 0, 0, 0 }; + for (size_t i = 0; i < data->wrench_.size(); ++i) + { + EXPECT_NEAR(data->wrench_[i], wrench[i], std::abs(wrench[i] * 1.0e-5)); + } + double robot_dexterity = 1.213826985e-07; + EXPECT_NEAR(data->robot_dexterity_, robot_dexterity, std::abs(robot_dexterity * 1.0e-5)); + } + else + { + std::cout << "Failed to get force mode data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_additional_info) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::AdditionalInfo* data = dynamic_cast(products[9].get())) + { + EXPECT_EQ(unsigned(data->tp_button_state_), false); + EXPECT_EQ(data->freedrive_button_enabled_, false); + EXPECT_EQ(data->io_enabled_freedrive_, false); + } + else + { + std::cout << "Failed to get additional info" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_global_variables_setup_message) +{ + unsigned char raw_program_state_msg_data[] = { 0x00, 0x00, 0x00, 0x50, 0x19, 0x00, 0x00, 0x00, 0x00, 0x56, 0x76, 0xd3, + 0xa0, 0x00, 0x00, 0x00, 0x6c, 0x6f, 0x77, 0x0a, 0x66, 0x61, 0x72, 0x0a, + 0x6e, 0x5f, 0x72, 0x65, 0x70, 0x65, 0x61, 0x74, 0x73, 0x0a, 0x6d, 0x65, + 0x64, 0x69, 0x75, 0x6d, 0x0a, 0x72, 0x65, 0x73, 0x65, 0x74, 0x5f, 0x76, + 0x61, 0x0a, 0x62, 0x6c, 0x65, 0x6e, 0x64, 0x69, 0x6e, 0x67, 0x0a, 0x68, + 0x69, 0x67, 0x68, 0x0a, 0x63, 0x6c, 0x6f, 0x73, 0x65, 0x0a, 0x6f, 0x6e, + 0x5f, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x0a }; + comm::BinParser bp(raw_program_state_msg_data, sizeof(raw_program_state_msg_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 1); + + if (primary_interface::GlobalVariablesSetupMessage* data = + dynamic_cast(products[0].get())) + { + EXPECT_EQ(data->start_index_, 0); + EXPECT_EQ(data->variable_names_, "low\nfar\nn_repeats\nmedium\nreset_va\nblending\nhigh\nclose\non_return\n"); + } + else + { + std::cout << "Failed to get Global Variables Update Message info" << std::endl; + GTEST_FAIL(); } }