diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt index 206d995..51c9ac0 100644 --- a/ur_rtde_driver/CMakeLists.txt +++ b/ur_rtde_driver/CMakeLists.txt @@ -102,6 +102,7 @@ add_library(ur_rtde_driver src/ur/ur_driver.cpp src/ur/calibration_checker.cpp src/ur/tool_communication.cpp + src/rtde/rtde_writer.cpp ) target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES}) add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_inputs.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_inputs.h index d5b4375..ddd4672 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_inputs.h +++ b/ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_inputs.h @@ -57,7 +57,12 @@ public: } virtual ~ControlPackageSetupInputsRequest() = default; + static size_t generateSerializedRequest(uint8_t* buffer, std::vector variable_names); + std::string variable_names_; + +private: + static const PackageType PACKAGE_TYPE = PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS; }; } // namespace rtde_interface diff --git a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h index 34336d3..afcf62d 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h +++ b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h @@ -39,6 +39,7 @@ #include "ur_rtde_driver/rtde/control_package_setup_outputs.h" #include "ur_rtde_driver/rtde/control_package_start.h" #include "ur_rtde_driver/log.h" +#include "ur_rtde_driver/rtde/rtde_writer.h" static const int UR_RTDE_PORT = 30004; static const std::string PIPELINE_NAME = "RTDE Data Pipeline"; @@ -51,7 +52,8 @@ class RTDEClient { public: RTDEClient() = delete; - RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& recipe_file); + RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, + const std::string& input_recipe_file); ~RTDEClient() = default; bool init(); bool start(); @@ -74,12 +76,15 @@ public: */ std::string getIP() const; + RTDEWriter& getWriter(); + private: comm::URStream stream_; std::vector recipe_; RTDEParser parser_; comm::URProducer prod_; comm::Pipeline pipeline_; + RTDEWriter writer_; VersionInformation urcontrol_version_; diff --git a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h new file mode 100644 index 0000000..ab7f4d4 --- /dev/null +++ b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h @@ -0,0 +1,63 @@ +// 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-07-25 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_RTDE_WRITER_H_INCLUDED +#define UR_RTDE_DRIVER_RTDE_WRITER_H_INCLUDED + +#include "ur_rtde_driver/rtde/package_header.h" +#include "ur_rtde_driver/rtde/rtde_package.h" +#include "ur_rtde_driver/comm/stream.h" + +namespace ur_driver +{ +namespace rtde_interface +{ +class RTDEWriter +{ +public: + RTDEWriter() = delete; + RTDEWriter(comm::URStream* stream, const std::string& recipe_file); + ~RTDEWriter() = default; + bool init(uint8_t recipe_id); + bool start(); + + bool sendSpeedSlider(double speed_slider_fraction); + bool sendStandardDigitalOutput(uint8_t output_pin, bool value); + bool sendConfigurableDigitalOutput(uint8_t output_pin, bool value); + bool sendToolDigitalOutput(bool value); + bool sendStandardAnalogOuput(uint8_t output_pin, bool value); + +private: + comm::URStream* stream_; + std::vector recipe_; + uint8_t recipe_id_; +}; + +} // namespace rtde_interface +} // namespace ur_driver + +#endif // UR_RTDE_DRIVER_RTDE_WRITER_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 98e64a2..22615b1 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 @@ -32,6 +32,7 @@ #include "ur_rtde_driver/comm/script_sender.h" #include "ur_rtde_driver/ur/tool_communication.h" #include "ur_rtde_driver/primary/robot_message/version_message.h" +#include "ur_rtde_driver/rtde/rtde_writer.h" namespace ur_driver { @@ -51,19 +52,20 @@ public: * \param script_file URScript file that should be sent to the robot * \param tool_comm_setup Configuration for using the tool communication */ - UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file, - std::function handle_program_state, std::unique_ptr tool_comm_setup, - const std::string& calibration_checksum = ""); + 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, + std::unique_ptr tool_comm_setup, const std::string& calibration_checksum = ""); /*! * \brief Constructs a new UrDriver object. * * \param robot_ip IP-address under which the robot is reachable. * \param script_file URScript file that should be sent to the robot */ - UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file, - std::function handle_program_state, const std::string& calibration_checksum = "") - : UrDriver(robot_ip, script_file, recipe_file, handle_program_state, std::unique_ptr{}, - calibration_checksum) + 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, + const std::string& calibration_checksum = "") + : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, + std::unique_ptr{}, calibration_checksum) { } @@ -115,6 +117,8 @@ public: void checkCalibration(const std::string& checksum); + rtde_interface::RTDEWriter& getRTDEWriter(); + private: std::string readScriptFile(const std::string& filename); std::string readKeepalive(); diff --git a/ur_rtde_driver/launch/ur_control.launch b/ur_rtde_driver/launch/ur_control.launch index aad3ff6..e8b8646 100644 --- a/ur_rtde_driver/launch/ur_control.launch +++ b/ur_rtde_driver/launch/ur_control.launch @@ -14,7 +14,8 @@ - + + @@ -29,7 +30,8 @@ - + + diff --git a/ur_rtde_driver/resources/rtde_input_recipe.txt b/ur_rtde_driver/resources/rtde_input_recipe.txt new file mode 100644 index 0000000..8f30e31 --- /dev/null +++ b/ur_rtde_driver/resources/rtde_input_recipe.txt @@ -0,0 +1,2 @@ +speed_slider_mask +speed_slider_fraction diff --git a/ur_rtde_driver/resources/rtde_recipe.txt b/ur_rtde_driver/resources/rtde_output_recipe.txt similarity index 100% rename from ur_rtde_driver/resources/rtde_recipe.txt rename to ur_rtde_driver/resources/rtde_output_recipe.txt diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index ce05885..0dc4a0c 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -55,19 +55,25 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } }; robot_ip_ = robot_hw_nh.param("robot_ip", "192.168.56.101"); std::string script_filename; - std::string recipe_filename; + std::string output_recipe_filename; + std::string input_recipe_filename; if (!robot_hw_nh.getParam("script_file", script_filename)) { ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("script_file") << " not given."); return false; } - if (!robot_hw_nh.getParam("recipe_file", recipe_filename)) + if (!robot_hw_nh.getParam("output_recipe_file", output_recipe_filename)) { - ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("recipe_file") << " not given."); + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("output_recipe_file") << " not given."); return false; } + if (!robot_hw_nh.getParam("input_recipe_file", input_recipe_filename)) + { + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("input_recipe_file") << " not given."); + return false; + } std::string tf_prefix = robot_hw_nh.param("tf_prefix", ""); program_state_pub_ = robot_hw_nh.advertise("robot_program_running", 10, true); @@ -135,7 +141,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h ROS_INFO_STREAM("Initializing urdriver"); try { - ur_driver_.reset(new UrDriver(robot_ip_, script_filename, recipe_filename, + ur_driver_.reset(new UrDriver(robot_ip_, script_filename, output_recipe_filename, input_recipe_filename, std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1), std::move(tool_comm_setup), calibration_checksum)); } diff --git a/ur_rtde_driver/src/rtde/control_package_setup_inputs.cpp b/ur_rtde_driver/src/rtde/control_package_setup_inputs.cpp index 74966b4..7227a39 100644 --- a/ur_rtde_driver/src/rtde/control_package_setup_inputs.cpp +++ b/ur_rtde_driver/src/rtde/control_package_setup_inputs.cpp @@ -46,5 +46,25 @@ std::string ControlPackageSetupInputs::toString() const return ss.str(); } + +size_t ControlPackageSetupInputsRequest::generateSerializedRequest(uint8_t* buffer, + std::vector variable_names) +{ + if (variable_names.size() == 0) + { + return 0; + } + std::string variables; + for (const auto& piece : variable_names) + variables += (piece + ","); + variables.pop_back(); + uint16_t payload_size = sizeof(double) + variables.size(); + + size_t size = 0; + size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, payload_size); + size += comm::PackageSerializer::serialize(buffer + size, variables); + + return size; +} } // namespace rtde_interface } // namespace ur_driver diff --git a/ur_rtde_driver/src/rtde/rtde_client.cpp b/ur_rtde_driver/src/rtde/rtde_client.cpp index 16a61c6..5c39e4b 100644 --- a/ur_rtde_driver/src/rtde/rtde_client.cpp +++ b/ur_rtde_driver/src/rtde/rtde_client.cpp @@ -32,12 +32,14 @@ namespace ur_driver { namespace rtde_interface { -RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& recipe_file) +RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, + const std::string& input_recipe_file) : stream_(robot_ip, UR_RTDE_PORT) - , recipe_(readRecipe(recipe_file)) + , recipe_(readRecipe(output_recipe_file)) , parser_(recipe_) , prod_(stream_, parser_) , pipeline_(prod_, PIPELINE_NAME, notifier) + , writer_(&stream_, input_recipe_file) , max_frequency_(URE_MAX_FREQUENCY) { } @@ -137,5 +139,10 @@ std::string RTDEClient::getIP() const { return stream_.getIP(); } + +RTDEWriter& RTDEClient::getWriter() +{ + return writer_; +} } // namespace rtde_interface } // namespace ur_driver diff --git a/ur_rtde_driver/src/rtde/rtde_writer.cpp b/ur_rtde_driver/src/rtde/rtde_writer.cpp new file mode 100644 index 0000000..4d28b0e --- /dev/null +++ b/ur_rtde_driver/src/rtde/rtde_writer.cpp @@ -0,0 +1,69 @@ +// 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-07-25 + * + */ +//---------------------------------------------------------------------- + +#include "ur_rtde_driver/rtde/rtde_writer.h" + +namespace ur_driver +{ +namespace rtde_interface +{ +RTDEWriter::RTDEWriter(comm::URStream* stream, const std::string& recipe_file) : stream_(stream) +{ +} + +bool RTDEWriter::init(uint8_t recipe_id) +{ + return false; +} +bool RTDEWriter::start() +{ + return false; +} + +bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) +{ + return false; +} +bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value) +{ + return false; +} +bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) +{ + return false; +} +bool RTDEWriter::sendToolDigitalOutput(bool value) +{ + return false; +} +bool RTDEWriter::sendStandardAnalogOuput(uint8_t output_pin, bool value) +{ + return false; +} + +} // namespace rtde_interface +} // namespace ur_driver diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index 56379e9..f30eabd 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -47,7 +47,8 @@ static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}"); static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, - const std::string& recipe_file, std::function handle_program_state, + const std::string& output_recipe_file, const std::string& input_recipe_file, + std::function handle_program_state, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum) : servoj_time_(0.008) , servoj_gain_(2000) @@ -58,7 +59,7 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc { LOG_DEBUG("Initializing urdriver"); LOG_DEBUG("Initializing RTDE client"); - rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, recipe_file)); + rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_file, input_recipe_file)); LOG_INFO("Checking if calibration data matches connected robot."); checkCalibration(calibration_checksum); @@ -234,4 +235,9 @@ void UrDriver::checkCalibration(const std::string& checksum) ROS_DEBUG_STREAM("Got calibration information from robot."); } +rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() +{ + return rtde_client_->getWriter(); +} + } // namespace ur_driver