diff --git a/CMakeLists.txt b/CMakeLists.txt index 9a499a1..e63f68f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,6 +91,7 @@ add_library(ur_rtde_driver src/rtde/request_protocol_version.cpp src/rtde/rtde_package.cpp src/rtde/text_message.cpp + src/rtde/rtde_client.cpp ) target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES}) add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/include/ur_rtde_driver/comm/pipeline.h b/include/ur_rtde_driver/comm/pipeline.h index cf35d38..c790022 100644 --- a/include/ur_rtde_driver/comm/pipeline.h +++ b/include/ur_rtde_driver/comm/pipeline.h @@ -181,9 +181,9 @@ public: notifier_.stopped(name_); } - bool getLatestProduct(std::unique_ptr> product, std::chrono::milliseconds timeout) + bool getLatestProduct(std::unique_ptr>& product, std::chrono::milliseconds timeout) { - return !queue_.wait_dequeue_timed(product, timeout); + return queue_.wait_dequeue_timed(product, timeout); } private: diff --git a/include/ur_rtde_driver/rtde/rtde_client.h b/include/ur_rtde_driver/rtde/rtde_client.h new file mode 100644 index 0000000..cd0ca9b --- /dev/null +++ b/include/ur_rtde_driver/rtde/rtde_client.h @@ -0,0 +1,65 @@ +// 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-04-10 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED +#define UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED + +#include "ur_rtde_driver/comm/pipeline.h" +#include "ur_rtde_driver/rtde/package_header.h" +#include "ur_rtde_driver/rtde/rtde_package.h" +#include "ur_rtde_driver/comm/stream.h" +#include "ur_rtde_driver/rtde/rtde_parser.h" +#include "ur_rtde_driver/comm/producer.h" +#include "ur_rtde_driver/rtde/data_package.h" +#include "ur_rtde_driver/rtde/request_protocol_version.h" + +static const int UR_RTDE_PORT = 30004; +static const std::string PIPELINE_NAME = "RTDE Data Pipeline"; + +namespace ur_driver +{ +namespace rtde_interface +{ +class RTDEClient +{ +public: + RTDEClient() = delete; + RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier); + ~RTDEClient() = default; + bool getDataPackage(std::unique_ptr>& data_package, std::chrono::milliseconds timeout); + +private: + comm::URStream stream_; + RTDEParser parser_; + comm::URProducer prod_; + comm::Pipeline pipeline_; +}; + +} // namespace rtde_interface +} // namespace ur_driver + +#endif // UR_RTDE_DRIVER_RTDE_CLIENT_H_INCLUDED diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp new file mode 100644 index 0000000..521979a --- /dev/null +++ b/src/rtde/rtde_client.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 +// +// 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-04-10 + * + */ +//---------------------------------------------------------------------- + +#include "ur_rtde_driver/rtde/rtde_client.h" + +namespace ur_driver +{ +namespace rtde_interface +{ +RTDEClient::RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier) + : stream_(ROBOT_IP, UR_RTDE_PORT), parser_(), prod_(stream_, parser_), + pipeline_(prod_, PIPELINE_NAME, notifier) +{ + pipeline_.run(); + + sleep(1); + + uint8_t buffer[4096]; + size_t size; + size_t written; + size = RequestProtocolVersionRequest::generateSerializedRequest(buffer); + std::cout << "size: " << size << std::endl; + std::cout << "buffer: " << static_cast(buffer[0]) << " - " + << static_cast(buffer[1]) << " - " + << static_cast(buffer[2]) << " - " + << static_cast(buffer[3]) << " - " + << static_cast(buffer[4]) << std::endl; + stream_.write(buffer, size, written); + + std::cout << "wrote: " << written << std::endl; +} + +bool RTDEClient::getDataPackage( + std::unique_ptr>& data_package, + std::chrono::milliseconds timeout) +{ + return pipeline_.getLatestProduct(data_package, timeout); +} +} // namespace rtde_interface +} // namespace ur_driver