From a0ea50a031b2573bd94bff51d62d67e8dcdd4258 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Wed, 22 May 2019 15:44:43 +0200 Subject: [PATCH] implemented script_sender and moved sending of robot program --- .../ur_rtde_driver/comm/script_sender.h | 125 ++++++++++++++++++ .../include/ur_rtde_driver/ur/ur_driver.h | 2 + ur_rtde_driver/src/ur/ur_driver.cpp | 18 +-- 3 files changed, 132 insertions(+), 13 deletions(-) create mode 100644 ur_rtde_driver/include/ur_rtde_driver/comm/script_sender.h diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/script_sender.h b/ur_rtde_driver/include/ur_rtde_driver/comm/script_sender.h new file mode 100644 index 0000000..297c9b1 --- /dev/null +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/script_sender.h @@ -0,0 +1,125 @@ + +// 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 Tristan Schnell schnell@fzi.de + * \date 2019-05-22 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_SCRIPT_SENDER_H_INCLUDED +#define UR_RTDE_DRIVER_SCRIPT_SENDER_H_INCLUDED + +#include "ur_rtde_driver/comm/server.h" +#include "ur_rtde_driver/log.h" + +namespace ur_driver +{ +namespace comm +{ +class ScriptSender +{ +public: + ScriptSender() = delete; + ScriptSender(uint32_t port, const std::string& program) : server_(port), script_thread_(), program_(program) + { + if (!server_.bind()) + { + throw std::runtime_error("Could not bind to server"); + } + } + + void start() + { + script_thread_ = std::thread(&ScriptSender::runScriptSender, this); + } + +private: + URServer server_; + std::thread script_thread_; + std::string program_; + + const std::string PROGRAM_REQUEST = std::string("request_program\n"); + + void runScriptSender() + { + while (true) + { + if (!server_.accept()) + { + throw std::runtime_error("Failed to accept robot connection"); + } + if (requestRead()) + { + LOG_INFO("Robot requested program"); + sendProgram(); + } + server_.disconnectClient(); + } + } + + bool requestRead() + { + size_t buf_len = 1024; + char buffer[buf_len]; + + bool read_successful = server_.readLine(buffer, buf_len); + + if (read_successful) + { + if (std::string(buffer) == PROGRAM_REQUEST) + { + return true; + } + else + { + LOG_WARN("Received unexpected message on script request port "); + return false; + } + } + else + { + LOG_WARN("Could not read on script request port"); + } + } + + void sendProgram() + { + size_t len = program_.size(); + const uint8_t* data = reinterpret_cast(program_.c_str()); + size_t written; + + if (server_.write(data, len, written)) + { + LOG_INFO("Sent program to robot"); + } + else + { + LOG_ERROR("Could not send program to robot"); + } + } +}; + +} // namespace comm +} // namespace ur_driver + +#endif // UR_RTDE_DRIVER_SCRIPT_SENDER_H_INCLUDED diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h index 5d061c8..e5617e7 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h @@ -27,6 +27,7 @@ #include "ur_rtde_driver/rtde/rtde_client.h" #include "ur_rtde_driver/comm/reverse_interface.h" +#include "ur_rtde_driver/comm/script_sender.h" namespace ur_driver { @@ -71,6 +72,7 @@ private: comm::INotifier notifier_; std::unique_ptr rtde_client_; std::unique_ptr reverse_interface_; + std::unique_ptr script_sender_; double servoj_time_; uint32_t servoj_gain_; diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index f7a9eb5..32aab99 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -62,7 +62,8 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc stream.connect(); std::string local_ip = stream.getIP(); - uint32_t reverse_port = 50001; // TODO: Make this a parameter + uint32_t reverse_port = 50001; // TODO: Make this a parameter + uint32_t script_sender_port = 50002; // TODO: Make this a parameter std::string prog = readScriptFile(script_file); prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); @@ -72,19 +73,10 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip); prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port)); - size_t len = prog.size(); - const uint8_t* data = reinterpret_cast(prog.c_str()); - size_t written; - - if (stream.write(data, len, written)) - { - LOG_INFO("Sent program to robot"); - } - else - { - LOG_ERROR("Could not send program to robot"); - } + script_sender_.reset(new comm::ScriptSender(script_sender_port, prog)); + script_sender_->start(); + LOG_INFO("Created script sender"); reverse_interface_.reset(new comm::ReverseInterface(reverse_port)); LOG_INFO("Created reverse interface");