From 8b7bccd6b81a8732a03416d716bb1b729e1db09a Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Wed, 6 Nov 2019 16:50:42 +0100 Subject: [PATCH 01/22] Added a Getter for the RTDE output recipe to be used in the hardware interface --- .../include/ur_robot_driver/rtde/rtde_client.h | 10 ++++++++++ ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h | 7 +++++++ ur_robot_driver/src/ur/ur_driver.cpp | 5 +++++ 3 files changed, 22 insertions(+) diff --git a/ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h b/ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h index 27547f473..96c63a7c8 100644 --- a/ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h +++ b/ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h @@ -125,6 +125,16 @@ class RTDEClient */ RTDEWriter& getWriter(); + /*! + * \brief Getter for the RTDE output recipe. + * + * \returns The output recipe + */ + std::vector getOutputRecipe() + { + return output_recipe_; + } + private: comm::URStream stream_; std::vector output_recipe_; diff --git a/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h b/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h index 3b4f10960..d6b64eec5 100644 --- a/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h +++ b/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h @@ -192,6 +192,13 @@ class UrDriver return robot_version_; } + /*! + * \brief Getter for the RTDE output recipe used in the RTDE client. + * + * \returns The used RTDE output recipe + */ + std::vector getRTDEOutputRecipe(); + private: std::string readScriptFile(const std::string& filename); std::string readKeepalive(); diff --git a/ur_robot_driver/src/ur/ur_driver.cpp b/ur_robot_driver/src/ur/ur_driver.cpp index 68bbd41c2..fd42cdba6 100644 --- a/ur_robot_driver/src/ur/ur_driver.cpp +++ b/ur_robot_driver/src/ur/ur_driver.cpp @@ -295,4 +295,9 @@ bool UrDriver::sendRobotProgram() return false; } } + +std::vector UrDriver::getRTDEOutputRecipe() +{ + return rtde_client_->getOutputRecipe(); +} } // namespace ur_driver From 1bcac0ed2dfefacac2fd6074abdd066ccefb1554 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Wed, 6 Nov 2019 16:51:39 +0100 Subject: [PATCH 02/22] Added functionality to check if a RTDE data package value is of a given type --- .../include/ur_robot_driver/rtde/data_package.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ur_robot_driver/include/ur_robot_driver/rtde/data_package.h b/ur_robot_driver/include/ur_robot_driver/rtde/data_package.h index 7b0f8a1a1..2ef8f3984 100644 --- a/ur_robot_driver/include/ur_robot_driver/rtde/data_package.h +++ b/ur_robot_driver/include/ur_robot_driver/rtde/data_package.h @@ -102,6 +102,19 @@ class DataPackage : public RTDEPackage */ size_t serializePackage(uint8_t* buffer); + /*! + * \brief Check if a specific data field is of a given type. + * + * \param name The string identifier for the data field as used in the documentation. + * + * \returns True, if the data field is of the given type, false otherwise. + */ + template + static bool isType(const std::string& name) + { + return g_type_list[name].which() == _rtde_type_variant(T()).which(); + } + /*! * \brief Get a data field from the DataPackage. * From 7ca839c0883a07f2350a2bd193f322a74ae64765 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Wed, 6 Nov 2019 16:52:32 +0100 Subject: [PATCH 03/22] Added generalized publisher to automatically publish RTDE data package values to ROS topics --- ur_robot_driver/CMakeLists.txt | 1 + .../ros/data_field_publisher.h | 176 ++++++++++++++++++ .../ros/data_package_publisher.h | 67 +++++++ .../ur_robot_driver/ros/hardware_interface.h | 2 + .../src/ros/data_field_publisher.cpp | 110 +++++++++++ .../src/ros/hardware_interface.cpp | 7 + 6 files changed, 363 insertions(+) create mode 100644 ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h create mode 100644 ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h create mode 100644 ur_robot_driver/src/ros/data_field_publisher.cpp diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 57f550358..5c859c41f 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -105,6 +105,7 @@ add_library(ur_robot_driver src/ur/dashboard_client.cpp src/ur/tool_communication.cpp src/rtde/rtde_writer.cpp + src/ros/data_field_publisher.cpp ) target_link_libraries(ur_robot_driver ${catkin_LIBRARIES}) add_dependencies(ur_robot_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h new file mode 100644 index 000000000..4ef1b49f7 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h @@ -0,0 +1,176 @@ +// 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-10-30 + * + */ +//---------------------------------------------------------------------- +#ifndef UR_DRIVER_DATA_FIELD_PUBLISHER_H_INCLUDED +#define UR_DRIVER_DATA_FIELD_PUBLISHER_H_INCLUDED + +#include +#include + +namespace ur_driver +{ +namespace rtde_interface +{ +/*! + * \brief The DataFieldPublisher class implements an abstract wrapper for various ROS publishers + * that publish fields of the RTDE data package. In addition, it contains a static factory to + * create correct publishers for a data field. + */ +class DataFieldPublisher +{ +public: + DataFieldPublisher() = default; + + /*! + * \brief Publishes partial information from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const std::unique_ptr& data_package) = 0; + + /*! + * \brief Creates a DataFieldPublisher object based on a given data field. + * + * \param data_field_identifier The name of the data field to publish + * \param nh The used ROS node handle + * + * \returns A unique pointer to the created Publisher object + */ + static std::unique_ptr createFromString(const std::string& data_field_identifier, + ros::NodeHandle& nh); +}; + +/*! + * \brief Implements a publisher that directly publishes a datafield of a given type to a ROS topic + * of a given message type. + * + */ +template +class DirectDataPublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a DirectDataPublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + DirectDataPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier) + { + pub_.reset(new realtime_tools::RealtimePublisher(nh, "rtde_data/" + data_field_identifier_, 1)); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const std::unique_ptr& data_package) + { + if (data_package->getData(data_field_identifier_, data_)) + { + if (pub_) + { + if (pub_->trylock()) + { + pub_->msg_.data = data_; + pub_->unlockAndPublish(); + return true; + } + } + } + return false; + } + +private: + DataT data_; + std::string data_field_identifier_; + std::unique_ptr> pub_; +}; + +/*! + * \brief Implements a publisher that publishes a datafield containing an array of a given type and size to a ROS topic + * of a given message type. + * + */ +template +class ArrayDataPublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a DirectDataPublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + ArrayDataPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier) + { + pub_.reset(new realtime_tools::RealtimePublisher(nh, "rtde_data/" + data_field_identifier_, 1)); + pub_->msg_.data.resize(N); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const std::unique_ptr& data_package) + { + if (data_package->getData(data_field_identifier_, data_)) + { + if (pub_) + { + if (pub_->trylock()) + { + for (size_t i = 0; i < N; i++) + { + pub_->msg_.data[i] = data_[i]; + } + pub_->unlockAndPublish(); + return true; + } + } + } + return false; + } + +private: + std::array data_; + std::string data_field_identifier_; + std::unique_ptr> pub_; +}; +} // namespace rtde_interface +} // namespace ur_driver + +#endif // ifndef UR_DRIVER_DATA_FIELD_PUBLISHER_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h b/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h new file mode 100644 index 000000000..ecedf9d79 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h @@ -0,0 +1,67 @@ +// 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-10-30 + * + */ +//---------------------------------------------------------------------- +#ifndef UR_DRIVER_DATA_PACKAGE_PUBLISHER_H_INCLUDED +#define UR_DRIVER_DATA_PACKAGE_PUBLISHER_H_INCLUDED + +#include +#include +#include + +namespace ur_driver +{ +namespace rtde_interface +{ +class DataPackagePublisher +{ +public: + DataPackagePublisher() = delete; + + DataPackagePublisher(const std::vector& recipe, ros::NodeHandle& nh) : recipe_(recipe) + { + for (auto str : recipe) + { + publishers_.push_back(DataFieldPublisher::createFromString(str, nh)); + } + } + + void publishData(const std::unique_ptr& data_package) + { + for (auto const& i : publishers_) + { + i->publish(data_package); + } + } + +private: + std::vector recipe_; + std::list> publishers_; +}; + +} // namespace rtde_interface +} // namespace ur_driver + +#endif // ifndef UR_DRIVER_DATA_PACKAGE_PUBLISHER_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h index ff380f0f4..b72e0dc01 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h @@ -53,6 +53,7 @@ #include #include +#include "ur_robot_driver/ros/data_package_publisher.h" namespace ur_driver { @@ -239,6 +240,7 @@ class HardwareInterface : public hardware_interface::RobotHW std::unique_ptr> tool_data_pub_; std::unique_ptr> robot_mode_pub_; std::unique_ptr> safety_mode_pub_; + std::unique_ptr rtde_data_pub_; ros::ServiceServer set_speed_slider_srv_; ros::ServiceServer set_io_srv_; diff --git a/ur_robot_driver/src/ros/data_field_publisher.cpp b/ur_robot_driver/src/ros/data_field_publisher.cpp new file mode 100644 index 000000000..b3806814c --- /dev/null +++ b/ur_robot_driver/src/ros/data_field_publisher.cpp @@ -0,0 +1,110 @@ +#include "ur_robot_driver/ros/data_field_publisher.h" +// 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-10-30 + * + */ +//---------------------------------------------------------------------- + +#include "ur_robot_driver/exceptions.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ur_driver +{ +namespace rtde_interface +{ +using _bool_publisher = DirectDataPublisher; +using _uint8_publisher = DirectDataPublisher; +using _uint32_publisher = DirectDataPublisher; +using _uint64_publisher = DirectDataPublisher; +using _int32_publisher = DirectDataPublisher; +using _double_publisher = DirectDataPublisher; +using _vector3d_publisher = ArrayDataPublisher; +using _vector6d_publisher = ArrayDataPublisher; +using _vector6int32_publisher = ArrayDataPublisher; +using _vector6uint32_publisher = ArrayDataPublisher; +using _string_publisher = DirectDataPublisher; + +std::unique_ptr DataFieldPublisher::createFromString(const std::string& data_field_identifier, + ros::NodeHandle& nh) +{ + if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _bool_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _uint8_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _uint32_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _uint64_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _int32_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _double_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _vector3d_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _vector6d_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _vector6int32_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _vector6uint32_publisher(data_field_identifier, nh)); + } + else if (DataPackage::isType(data_field_identifier)) + { + return std::unique_ptr(new _string_publisher(data_field_identifier, nh)); + } + else + { + throw UrException("No supported publisher for RTDE data field " + data_field_identifier); + } +} +} // namespace rtde_interface +} // namespace ur_driver diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 692130b74..db77e463d 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -52,6 +52,8 @@ HardwareInterface::HardwareInterface() bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { + ROS_ERROR_STREAM(rtde_interface::DataPackage::isType("actual_q")); + ROS_ERROR_STREAM(rtde_interface::DataPackage::isType("actual_q")); joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } }; joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } }; std::string script_filename; @@ -303,6 +305,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw safety_mode_pub_.reset( new realtime_tools::RealtimePublisher(robot_hw_nh, "safety_mode", 1, true)); + std::vector recipe = ur_driver_->getRTDEOutputRecipe(); + rtde_data_pub_.reset(new rtde_interface::DataPackagePublisher(recipe, robot_hw_nh)); + // Set the speed slider fraction used by the robot's execution. Values should be between 0 and 1. // Only set this smaller than 1 if you are using the scaled controllers (as by default) or you know what you're // doing. Using this with other controllers might lead to unexpected behaviors. @@ -394,6 +399,8 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) publishPose(); publishRobotAndSafetyMode(); + rtde_data_pub_->publishData(data_pkg); + // pausing state follows runtime state when pausing if (runtime_state_ == static_cast(rtde_interface::RUNTIME_STATE::PAUSED)) { From db56a8d12d015bf2309e47484a93837cd53a36e4 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 7 Nov 2019 10:18:56 +0100 Subject: [PATCH 04/22] No longer automatically publishes a list of data fields that are already manually published --- .../src/ros/hardware_interface.cpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index db77e463d..4bb104b73 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -305,7 +305,32 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw safety_mode_pub_.reset( new realtime_tools::RealtimePublisher(robot_hw_nh, "safety_mode", 1, true)); + std::string already_published[] = { "actual_q", + "actual_qd", + "target_speed_fraction", + "speed_scaling", + "runtime_state", + "actual_TCP_force", + "actual_TCP_pose", + "standard_analog_input0", + "standard_analog_input1", + "standard_analog_output0", + "standard_analog_output1", + "tool_mode", + "tool_analog_input0", + "tool_analog_input1", + "tool_output_voltage", + "tool_output_current", + "tool_temperature", + "actual_digital_input_bits", + "actual_digital_output_bits", + "analog_io_types", + "tool_analog_input_types" }; std::vector recipe = ur_driver_->getRTDEOutputRecipe(); + for (auto s : already_published) + { + recipe.erase(std::remove(recipe.begin(), recipe.end(), s), recipe.end()); + } rtde_data_pub_.reset(new rtde_interface::DataPackagePublisher(recipe, robot_hw_nh)); // Set the speed slider fraction used by the robot's execution. Values should be between 0 and 1. From 21ea8f44928522db4c1a33cc578a4b176a24b1eb Mon Sep 17 00:00:00 2001 From: t-schnell Date: Tue, 17 Dec 2019 10:02:02 +0100 Subject: [PATCH 05/22] Update ur_robot_driver/src/ros/hardware_interface.cpp Co-Authored-By: Felix Exner --- ur_robot_driver/src/ros/hardware_interface.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 4bb104b73..77f084b8a 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -305,6 +305,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw safety_mode_pub_.reset( new realtime_tools::RealtimePublisher(robot_hw_nh, "safety_mode", 1, true)); + // As we automatically create publishers for raw data of additional fields, we prevent republishing data + // that has already been published on ROS-interfaces. This list has to be kept updated by hand. std::string already_published[] = { "actual_q", "actual_qd", "target_speed_fraction", From 9349a299acadd2434fcdb4de69996dcc5b4dabc5 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 19 Dec 2019 13:17:57 +0100 Subject: [PATCH 06/22] added Doxygen comments for data package publisher --- .../ur_robot_driver/ros/data_package_publisher.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h b/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h index ecedf9d79..c485d792c 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h @@ -35,11 +35,21 @@ namespace ur_driver { namespace rtde_interface { + /*! + * \brief The DataPackagePublisher class handles publishing all data fields of an RTDE data + * package to various ROS topics. + */ class DataPackagePublisher { public: DataPackagePublisher() = delete; + /*! + * \brief Creates a new DataPackagePublisher object. + * + * \param recipe The different data fields contained in the package that should be published + * \param nh The node handle to advertise publishers on + */ DataPackagePublisher(const std::vector& recipe, ros::NodeHandle& nh) : recipe_(recipe) { for (auto str : recipe) @@ -49,6 +59,11 @@ class DataPackagePublisher } void publishData(const std::unique_ptr& data_package) + /*! + * \brief Publishes all relevant data fields of a given data package. + * + * \param data_package The data package to publish + */ { for (auto const& i : publishers_) { From 0d58c64da99aeb0c207084accd2a098a68540578 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 19 Dec 2019 13:19:27 +0100 Subject: [PATCH 07/22] switched from handling the data package as a unique pointer to a plain reference --- .../include/ur_robot_driver/ros/data_field_publisher.h | 10 +++++----- .../ur_robot_driver/ros/data_package_publisher.h | 2 +- .../include/ur_robot_driver/rtde/data_package.h | 8 ++++---- ur_robot_driver/src/ros/hardware_interface.cpp | 2 +- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h index 4ef1b49f7..d6d6214ee 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h @@ -51,7 +51,7 @@ class DataFieldPublisher * * \returns True if the realtime publisher could publish the data. */ - virtual bool publish(const std::unique_ptr& data_package) = 0; + virtual bool publish(const DataPackage& data_package) = 0; /*! * \brief Creates a DataFieldPublisher object based on a given data field. @@ -93,9 +93,9 @@ class DirectDataPublisher : public DataFieldPublisher * * \returns True if the realtime publisher could publish the data. */ - virtual bool publish(const std::unique_ptr& data_package) + virtual bool publish(const DataPackage& data_package) { - if (data_package->getData(data_field_identifier_, data_)) + if (data_package.getData(data_field_identifier_, data_)) { if (pub_) { @@ -145,9 +145,9 @@ class ArrayDataPublisher : public DataFieldPublisher * * \returns True if the realtime publisher could publish the data. */ - virtual bool publish(const std::unique_ptr& data_package) + virtual bool publish(const DataPackage& data_package) { - if (data_package->getData(data_field_identifier_, data_)) + if (data_package.getData(data_field_identifier_, data_)) { if (pub_) { diff --git a/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h b/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h index c485d792c..2e3e2c973 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h @@ -58,12 +58,12 @@ class DataPackagePublisher } } - void publishData(const std::unique_ptr& data_package) /*! * \brief Publishes all relevant data fields of a given data package. * * \param data_package The data package to publish */ + void publishData(const DataPackage& data_package) { for (auto const& i : publishers_) { diff --git a/ur_robot_driver/include/ur_robot_driver/rtde/data_package.h b/ur_robot_driver/include/ur_robot_driver/rtde/data_package.h index 2ef8f3984..7565a09bc 100644 --- a/ur_robot_driver/include/ur_robot_driver/rtde/data_package.h +++ b/ur_robot_driver/include/ur_robot_driver/rtde/data_package.h @@ -127,11 +127,11 @@ class DataPackage : public RTDEPackage * \returns True on success, false if the field cannot be found inside the package. */ template - bool getData(const std::string& name, T& val) + bool getData(const std::string& name, T& val) const { if (data_.find(name) != data_.end()) { - val = boost::strict_get(data_[name]); + val = boost::strict_get(data_.at(name)); } else { @@ -152,13 +152,13 @@ class DataPackage : public RTDEPackage * \returns True on success, false if the field cannot be found inside the package. */ template - bool getData(const std::string& name, std::bitset& val) + bool getData(const std::string& name, std::bitset& val) const { static_assert(sizeof(T) * 8 >= N, "Bitset is too large for underlying variable"); if (data_.find(name) != data_.end()) { - val = std::bitset(boost::strict_get(data_[name])); + val = std::bitset(boost::strict_get(data_.at(name))); } else { diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 77f084b8a..9308d7f67 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -426,7 +426,7 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) publishPose(); publishRobotAndSafetyMode(); - rtde_data_pub_->publishData(data_pkg); + rtde_data_pub_->publishData(*data_pkg); // pausing state follows runtime state when pausing if (runtime_state_ == static_cast(rtde_interface::RUNTIME_STATE::PAUSED)) From 95f60cf4322041571498992e5f5655aa87a617e3 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 19 Dec 2019 16:58:59 +0100 Subject: [PATCH 08/22] added custom messages for RTDE data fields --- ur_rtde_msgs/CMakeLists.txt | 203 +++++++++++++++++++++++++ ur_rtde_msgs/msg/BitRegisterArray.msg | 3 + ur_rtde_msgs/msg/JointAcceleration.msg | 3 + ur_rtde_msgs/msg/JointCurrents.msg | 3 + ur_rtde_msgs/msg/JointPosition.msg | 3 + ur_rtde_msgs/msg/JointTorques.msg | 3 + ur_rtde_msgs/msg/JointVelocity.msg | 3 + ur_rtde_msgs/msg/JointVoltages.msg | 3 + ur_rtde_msgs/package.xml | 65 ++++++++ 9 files changed, 289 insertions(+) create mode 100644 ur_rtde_msgs/CMakeLists.txt create mode 100644 ur_rtde_msgs/msg/BitRegisterArray.msg create mode 100644 ur_rtde_msgs/msg/JointAcceleration.msg create mode 100644 ur_rtde_msgs/msg/JointCurrents.msg create mode 100644 ur_rtde_msgs/msg/JointPosition.msg create mode 100644 ur_rtde_msgs/msg/JointTorques.msg create mode 100644 ur_rtde_msgs/msg/JointVelocity.msg create mode 100644 ur_rtde_msgs/msg/JointVoltages.msg create mode 100644 ur_rtde_msgs/package.xml diff --git a/ur_rtde_msgs/CMakeLists.txt b/ur_rtde_msgs/CMakeLists.txt new file mode 100644 index 000000000..fbfaa585d --- /dev/null +++ b/ur_rtde_msgs/CMakeLists.txt @@ -0,0 +1,203 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ur_rtde_msgs) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + ur_msgs + message_generation +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder + add_message_files( + DIRECTORY msg + ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here + generate_messages( + DEPENDENCIES + ur_msgs + ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ur_rtde_msgs + CATKIN_DEPENDS message_runtime ur_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/ur_rtde_msgs.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/ur_rtde_msgs_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_rtde_msgs.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/ur_rtde_msgs/msg/BitRegisterArray.msg b/ur_rtde_msgs/msg/BitRegisterArray.msg new file mode 100644 index 000000000..66a6c8614 --- /dev/null +++ b/ur_rtde_msgs/msg/BitRegisterArray.msg @@ -0,0 +1,3 @@ +# This message holds an array of values for bit registers. + +ur_msgs/Digital[] registers diff --git a/ur_rtde_msgs/msg/JointAcceleration.msg b/ur_rtde_msgs/msg/JointAcceleration.msg new file mode 100644 index 000000000..9378a704a --- /dev/null +++ b/ur_rtde_msgs/msg/JointAcceleration.msg @@ -0,0 +1,3 @@ +# This message holds data to describe the accelerations of all robot joints. + +float64[] data diff --git a/ur_rtde_msgs/msg/JointCurrents.msg b/ur_rtde_msgs/msg/JointCurrents.msg new file mode 100644 index 000000000..05c14269a --- /dev/null +++ b/ur_rtde_msgs/msg/JointCurrents.msg @@ -0,0 +1,3 @@ +# This message holds data to describe the currents of all robot joints. + +float64[] data diff --git a/ur_rtde_msgs/msg/JointPosition.msg b/ur_rtde_msgs/msg/JointPosition.msg new file mode 100644 index 000000000..133083fcb --- /dev/null +++ b/ur_rtde_msgs/msg/JointPosition.msg @@ -0,0 +1,3 @@ +# This message holds data to describe the positions of all robot joints. + +float64[] data diff --git a/ur_rtde_msgs/msg/JointTorques.msg b/ur_rtde_msgs/msg/JointTorques.msg new file mode 100644 index 000000000..060b5b666 --- /dev/null +++ b/ur_rtde_msgs/msg/JointTorques.msg @@ -0,0 +1,3 @@ +# This message holds data to describe the torques of all robot joints. + +float64[] data diff --git a/ur_rtde_msgs/msg/JointVelocity.msg b/ur_rtde_msgs/msg/JointVelocity.msg new file mode 100644 index 000000000..f8cb5fd16 --- /dev/null +++ b/ur_rtde_msgs/msg/JointVelocity.msg @@ -0,0 +1,3 @@ +# This message holds data to describe the velocities of all robot joints. + +float64[] data diff --git a/ur_rtde_msgs/msg/JointVoltages.msg b/ur_rtde_msgs/msg/JointVoltages.msg new file mode 100644 index 000000000..8abbf1105 --- /dev/null +++ b/ur_rtde_msgs/msg/JointVoltages.msg @@ -0,0 +1,3 @@ +# This message holds data to describe the voltages of all robot joints. + +float64[] data diff --git a/ur_rtde_msgs/package.xml b/ur_rtde_msgs/package.xml new file mode 100644 index 000000000..5a6a651c3 --- /dev/null +++ b/ur_rtde_msgs/package.xml @@ -0,0 +1,65 @@ + + + ur_rtde_msgs + 0.0.0 + The ur_rtde_msgs package + + + + + Tristan Schnell + + + + + + Apache 2.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + message_generation + message_runtime + message_runtime + ur_msgs + ur_msgs + ur_msgs + + + + + + + + From 05989e0877b2e88c9803f001d091e9e524c3323a Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 19 Dec 2019 17:20:39 +0100 Subject: [PATCH 09/22] Changed realtime publisher from unique_ptr to direct member --- .../ros/data_field_publisher.h | 35 +++++++------------ 1 file changed, 13 insertions(+), 22 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h index d6d6214ee..cabe20cea 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h @@ -81,9 +81,8 @@ class DirectDataPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ DirectDataPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) { - pub_.reset(new realtime_tools::RealtimePublisher(nh, "rtde_data/" + data_field_identifier_, 1)); } /*! @@ -97,14 +96,11 @@ class DirectDataPublisher : public DataFieldPublisher { if (data_package.getData(data_field_identifier_, data_)) { - if (pub_) + if (pub_.trylock()) { - if (pub_->trylock()) - { - pub_->msg_.data = data_; - pub_->unlockAndPublish(); - return true; - } + pub_.msg_.data = data_; + pub_.unlockAndPublish(); + return true; } } return false; @@ -113,7 +109,7 @@ class DirectDataPublisher : public DataFieldPublisher private: DataT data_; std::string data_field_identifier_; - std::unique_ptr> pub_; + realtime_tools::RealtimePublisher pub_; }; /*! @@ -132,10 +128,8 @@ class ArrayDataPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ ArrayDataPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) { - pub_.reset(new realtime_tools::RealtimePublisher(nh, "rtde_data/" + data_field_identifier_, 1)); - pub_->msg_.data.resize(N); } /*! @@ -149,17 +143,14 @@ class ArrayDataPublisher : public DataFieldPublisher { if (data_package.getData(data_field_identifier_, data_)) { - if (pub_) + if (pub_.trylock()) { - if (pub_->trylock()) + for (size_t i = 0; i < N; i++) { - for (size_t i = 0; i < N; i++) - { - pub_->msg_.data[i] = data_[i]; - } - pub_->unlockAndPublish(); - return true; + pub_.msg_.data[i] = data_[i]; } + pub_.unlockAndPublish(); + return true; } } return false; @@ -168,7 +159,7 @@ class ArrayDataPublisher : public DataFieldPublisher private: std::array data_; std::string data_field_identifier_; - std::unique_ptr> pub_; + realtime_tools::RealtimePublisher pub_; }; } // namespace rtde_interface } // namespace ur_driver From dba9ced31e0630df2c2139d341fe861f61ad0fec Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 19 Dec 2019 17:23:52 +0100 Subject: [PATCH 10/22] moved data_field_publisher to ros specific library --- ur_robot_driver/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 5c859c41f..31aea4f8b 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -105,7 +105,6 @@ add_library(ur_robot_driver src/ur/dashboard_client.cpp src/ur/tool_communication.cpp src/rtde/rtde_writer.cpp - src/ros/data_field_publisher.cpp ) target_link_libraries(ur_robot_driver ${catkin_LIBRARIES}) add_dependencies(ur_robot_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -113,6 +112,7 @@ add_dependencies(ur_robot_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX add_library(ur_robot_driver_plugin src/ros/dashboard_client_ros.cpp src/ros/hardware_interface.cpp + src/ros/data_field_publisher.cpp ) target_link_libraries(ur_robot_driver_plugin ur_robot_driver ${catkin_LIBRARIES}) add_dependencies(ur_robot_driver_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -121,6 +121,7 @@ add_executable(ur_robot_driver_node src/ros/dashboard_client_ros.cpp src/ros/hardware_interface.cpp src/ros/hardware_interface_node.cpp + src/ros/data_field_publisher.cpp ) target_link_libraries(ur_robot_driver_node ${catkin_LIBRARIES} ur_robot_driver) add_dependencies(ur_robot_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) From f47151a656cdb8f7832f86885a3f4fe3c4e2b3a2 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 19 Dec 2019 17:31:12 +0100 Subject: [PATCH 11/22] removed debug output from hardware_interface --- ur_robot_driver/src/ros/hardware_interface.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 9308d7f67..71a031b4f 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -52,8 +52,6 @@ HardwareInterface::HardwareInterface() bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { - ROS_ERROR_STREAM(rtde_interface::DataPackage::isType("actual_q")); - ROS_ERROR_STREAM(rtde_interface::DataPackage::isType("actual_q")); joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } }; joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } }; std::string script_filename; From f6fe4b0357609f11855d157023bebc2ef4de5b39 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Fri, 20 Dec 2019 15:11:21 +0100 Subject: [PATCH 12/22] added additional custom rtde data messages --- ur_robot_driver/CMakeLists.txt | 2 ++ ur_robot_driver/package.xml | 1 + ur_rtde_msgs/msg/JointMode.msg | 21 +++++++++++++++++++++ ur_rtde_msgs/msg/JointTemperature.msg | 3 +++ ur_rtde_msgs/msg/RobotStatusBits.msg | 6 ++++++ ur_rtde_msgs/msg/SafetyStatus.msg | 17 +++++++++++++++++ ur_rtde_msgs/msg/SafetyStatusBits.msg | 13 +++++++++++++ ur_rtde_msgs/msg/ToolMode.msg | 13 +++++++++++++ 8 files changed, 76 insertions(+) create mode 100644 ur_rtde_msgs/msg/JointMode.msg create mode 100644 ur_rtde_msgs/msg/JointTemperature.msg create mode 100644 ur_rtde_msgs/msg/RobotStatusBits.msg create mode 100644 ur_rtde_msgs/msg/SafetyStatus.msg create mode 100644 ur_rtde_msgs/msg/SafetyStatusBits.msg create mode 100644 ur_rtde_msgs/msg/ToolMode.msg diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 31aea4f8b..ff7daae64 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(catkin REQUIRED trajectory_msgs ur_controllers ur_dashboard_msgs + ur_rtde_msgs ur_msgs ) find_package(Boost REQUIRED) @@ -49,6 +50,7 @@ catkin_package( trajectory_msgs ur_controllers ur_dashboard_msgs + ur_rtde_msgs ur_msgs std_srvs DEPENDS diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 7929d7a3d..cb48b1367 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -36,6 +36,7 @@ trajectory_msgs ur_controllers ur_dashboard_msgs + ur_rtde_msgs ur_msgs std_srvs diff --git a/ur_rtde_msgs/msg/JointMode.msg b/ur_rtde_msgs/msg/JointMode.msg new file mode 100644 index 000000000..fd0116ad9 --- /dev/null +++ b/ur_rtde_msgs/msg/JointMode.msg @@ -0,0 +1,21 @@ +#This message holds data describing the modes of the robot's joints + +uint8 JOINT_MODE_RESET=235 +uint8 JOINT_MODE_SHUTTING_DOWN=236 +uint8 JOINT_PART_D_CALIBRATION_MODE=237 +uint8 JOINT_MODE_BACKDRIVE=238 +uint8 JOINT_MODE_POWER_OFF=239 +uint8 JOINT_MODE_READY_FOR_POWER_OFF=240 +uint8 JOINT_MODE_NOT_RESPONDING=245 +uint8 JOINT_MODE_MOTOR_INITIALISATION=246 +uint8 JOINT_MODE_BOOTING=247 +uint8 JOINT_PART_D_CALIBRATION_ERROR_MODE=248 +uint8 JOINT_MODE_BOOTLOADER=249 +uint8 JOINT_CALIBRATION_MODE=250 +uint8 JOINT_MODE_VIOLATION=251 +uint8 JOINT_MODE_FAULT=252 +uint8 JOINT_MODE_RUNNING=253 +uint8 JOINT_MODE_IDLE=255 + +uint8[] mode + diff --git a/ur_rtde_msgs/msg/JointTemperature.msg b/ur_rtde_msgs/msg/JointTemperature.msg new file mode 100644 index 000000000..8b942244a --- /dev/null +++ b/ur_rtde_msgs/msg/JointTemperature.msg @@ -0,0 +1,3 @@ +# This message holds data to describe the temperatures of all robot joints. + +float64[] data diff --git a/ur_rtde_msgs/msg/RobotStatusBits.msg b/ur_rtde_msgs/msg/RobotStatusBits.msg new file mode 100644 index 000000000..f9e93a3ac --- /dev/null +++ b/ur_rtde_msgs/msg/RobotStatusBits.msg @@ -0,0 +1,6 @@ +# This message holds information about the robot status as explicit booleans + +bool is_power_on +bool is_program_running +bool is_teach_button_pressed +bool is_power_button_pressed diff --git a/ur_rtde_msgs/msg/SafetyStatus.msg b/ur_rtde_msgs/msg/SafetyStatus.msg new file mode 100644 index 000000000..55a3768ff --- /dev/null +++ b/ur_rtde_msgs/msg/SafetyStatus.msg @@ -0,0 +1,17 @@ +#This message holds data describing the safety status value + +uint8 SAFETY_STATUS_SYSTEM_THREE_POSITION_ENABLING_STOP=13 +uint8 SAFETY_STATUS_AUTOMATIC_MODE_SAFEGUARD_STOP=12 +uint8 SAFETY_STATUS_UNDEFINED_SAFETY_MODE=11 +uint8 SAFETY_STATUS_VALIDATE_JOINT_ID=10 +uint8 SAFETY_STATUS_FAULT=9 +uint8 SAFETY_STATUS_VIOLATION=8 +uint8 SAFETY_STATUS_ROBOT_EMERGENCY_STOP=7 +uint8 SAFETY_STATUS_SYSTEM_EMERGENCY_STOP=6 +uint8 SAFETY_STATUS_SAFEGUARD_STOP=5 +uint8 SAFETY_STATUS_RECOVERY=4 +uint8 SAFETY_STATUS_PROTECTIVE_STOP=3 +uint8 SAFETY_STATUS_REDUCED=2 +uint8 SAFETY_STATUS_NORMAL=1 + +uint8 status diff --git a/ur_rtde_msgs/msg/SafetyStatusBits.msg b/ur_rtde_msgs/msg/SafetyStatusBits.msg new file mode 100644 index 000000000..3f0dcd375 --- /dev/null +++ b/ur_rtde_msgs/msg/SafetyStatusBits.msg @@ -0,0 +1,13 @@ +# This message holds information about the safety status of the robot as explicit booleans + +bool is_normal_mode +bool is_reduced_mode +bool is_protective_stopped +bool is_recovery_mode +bool is_safeguard_stopped +bool is_system_emergency_stopped +bool is_robot_emergency_stopped +bool is_emergency_stopped +bool is_violation +bool is_fault +bool is_stopped_due_to_safety diff --git a/ur_rtde_msgs/msg/ToolMode.msg b/ur_rtde_msgs/msg/ToolMode.msg new file mode 100644 index 000000000..10aa2748a --- /dev/null +++ b/ur_rtde_msgs/msg/ToolMode.msg @@ -0,0 +1,13 @@ +#This message holds data describing the mode of a tool + +uint8 JOINT_MODE_RESET=235 +uint8 JOINT_MODE_SHUTTING_DOWN=236 +uint8 JOINT_MODE_POWER_OFF=239 +uint8 JOINT_MODE_NOT_RESPONDING=245 +uint8 JOINT_MODE_BOOTING=247 +uint8 JOINT_MODE_BOOTLOADER=249 +uint8 JOINT_MODE_FAULT=252 +uint8 JOINT_MODE_RUNNING=253 +uint8 JOINT_MODE_IDLE=255 + +uint8 mode From 682727dd582f8652c759e7a25023379b65f7e292 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Fri, 20 Dec 2019 15:13:51 +0100 Subject: [PATCH 13/22] implemented additional more specific publishers for various RTDE data fields --- .../ros/data_field_publisher.h | 61 +++- .../ros/data_package_publisher.h | 8 +- .../ros/geometry_data_publishers.h | 205 ++++++++++++ .../ros/robot_state_publishers.h | 293 ++++++++++++++++++ .../ros/sensor_data_publishers.h | 133 ++++++++ 5 files changed, 695 insertions(+), 5 deletions(-) create mode 100644 ur_robot_driver/include/ur_robot_driver/ros/geometry_data_publishers.h create mode 100644 ur_robot_driver/include/ur_robot_driver/ros/robot_state_publishers.h create mode 100644 ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h diff --git a/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h index cabe20cea..62993cfa6 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h @@ -29,6 +29,8 @@ #include #include +#include +#include namespace ur_driver { @@ -122,7 +124,7 @@ class ArrayDataPublisher : public DataFieldPublisher { public: /*! - * \brief Creates a DirectDataPublisher object. + * \brief Creates a ArrayDataPublisher object. * * \param data_field_identifier The string identifier of the data field to publish * \param nh The used ROS node handle @@ -130,6 +132,7 @@ class ArrayDataPublisher : public DataFieldPublisher ArrayDataPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) { + pub_.msg_.data.resize(N); } /*! @@ -161,6 +164,62 @@ class ArrayDataPublisher : public DataFieldPublisher std::string data_field_identifier_; realtime_tools::RealtimePublisher pub_; }; + +/*! + * \brief Implements a publisher that publishes a datafield containing an array of bit registers + * of a given message type. + * + */ +class BitRegisterArrayPublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a DirectDataPublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + BitRegisterArrayPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh, uint8_t start_pin) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + pub_.msg_.registers.resize(ARRAY_SIZE); + for (size_t i = 0; i < ARRAY_SIZE; i++) + { + pub_.msg_.registers[i] = ur_msgs::Digital(); + pub_.msg_.registers[i].pin = start_pin + i; + } + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + for (size_t i = 0; i < ARRAY_SIZE; i++) + { + pub_.msg_.registers[i].state = data_[i]; + } + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + static const size_t ARRAY_SIZE = 32; + std::bitset data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; } // namespace rtde_interface } // namespace ur_driver diff --git a/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h b/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h index 2e3e2c973..d672bb059 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h @@ -35,10 +35,10 @@ namespace ur_driver { namespace rtde_interface { - /*! - * \brief The DataPackagePublisher class handles publishing all data fields of an RTDE data - * package to various ROS topics. - */ +/*! + * \brief The DataPackagePublisher class handles publishing all data fields of an RTDE data + * package to various ROS topics. + */ class DataPackagePublisher { public: diff --git a/ur_robot_driver/include/ur_robot_driver/ros/geometry_data_publishers.h b/ur_robot_driver/include/ur_robot_driver/ros/geometry_data_publishers.h new file mode 100644 index 000000000..0542f2eb6 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/ros/geometry_data_publishers.h @@ -0,0 +1,205 @@ +// 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-12-20 + * + */ +//---------------------------------------------------------------------- +#ifndef UR_DRIVER_GEOMETRY_DATA_PUBLISHERS_H_INCLUDED +#define UR_DRIVER_GEOMETRY_DATA_PUBLISHERS_H_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace ur_driver +{ +namespace rtde_interface +{ +/*! + * \brief Implements a publisher that publishes a datafield containing a pose. + * + */ +class PosePublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a PosePublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + PosePublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + pub_.msg_ = geometry_msgs::Pose(); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + tcp_angle_ = std::sqrt(std::pow(data_[3], 2) + std::pow(data_[4], 2) + std::pow(data_[5], 2)); + + rotation_vec_ = tf2::Vector3(data_[3], data_[4], data_[5]); + if (tcp_angle_ > 1e-16) + { + rotation_.setRotation(rotation_vec_.normalized(), tcp_angle_); + } + pub_.msg_.position.x = data_[0]; + pub_.msg_.position.y = data_[1]; + pub_.msg_.position.z = data_[2]; + + pub_.msg_.orientation = tf2::toMsg(rotation_); + + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + std::array data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; + + double tcp_angle_; + tf2::Vector3 rotation_vec_; + tf2::Quaternion rotation_; +}; + +/*! + * \brief Implements a publisher that publishes a datafield containing a velocity. + * + */ +class TwistPublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a TwistPublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + TwistPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + pub_.msg_ = geometry_msgs::Twist(); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + pub_.msg_.linear.x = data_[0]; + pub_.msg_.linear.y = data_[1]; + pub_.msg_.linear.z = data_[2]; + pub_.msg_.angular.x = data_[3]; + pub_.msg_.angular.y = data_[4]; + pub_.msg_.angular.z = data_[5]; + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + std::array data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; + +/*! + * \brief Implements a publisher that publishes a datafield containing a 3D vector. + * + */ +class Vector3Publisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a Vector3Publisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + Vector3Publisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + pub_.msg_ = geometry_msgs::Vector3(); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + pub_.msg_.x = data_[0]; + pub_.msg_.y = data_[1]; + pub_.msg_.z = data_[2]; + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + std::array data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; +} // namespace rtde_interface +} // namespace ur_driver + +#endif // ifndef UR_DRIVER_GEOMETRY_DATA_PUBLISHERS_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/ros/robot_state_publishers.h b/ur_robot_driver/include/ur_robot_driver/ros/robot_state_publishers.h new file mode 100644 index 000000000..218026d61 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/ros/robot_state_publishers.h @@ -0,0 +1,293 @@ +// 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-12-20 + * + */ +//---------------------------------------------------------------------- +#ifndef UR_DRIVER_ROBOT_STATE_PUBLISHERS_H_INCLUDED +#define UR_DRIVER_ROBOT_STATE_PUBLISHERS_H_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace ur_driver +{ +namespace rtde_interface +{ +/*! + * \brief Implements a publisher that directly publishes a datafield containing an arbitrary mode. + * + */ +template +class ModePublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a ModePublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + ModePublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + pub_.msg_.mode = data_; + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + DataT data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; + +/*! + * \brief Implements a publisher that directly publishes a datafield containing an arbitrary status. + * + */ +template +class StatusPublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a StatusPublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + StatusPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + pub_.msg_.status = data_; + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + DataT data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; + +/*! + * \brief Implements a publisher that publishes a datafield containing a the joint modes. + * + */ +class JointModePublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a JointModePublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + JointModePublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + pub_.msg_ = ur_rtde_msgs::JointMode(); + pub_.msg_.mode.resize(NUM_JOINTS); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + for (size_t i = 0; i < NUM_JOINTS; i++) + { + pub_.msg_.mode[i] = data_[i]; + } + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + static const size_t NUM_JOINTS = 6; + std::array data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; + +/*! + * \brief Implements a publisher that publishes a datafield containing a the robot status bits. + * + */ +class RobotStatusBitsPublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a RobotStatusBitsPublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + RobotStatusBitsPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + pub_.msg_ = ur_rtde_msgs::RobotStatusBits(); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + pub_.msg_.is_power_on = data_[0]; + pub_.msg_.is_program_running = data_[1]; + pub_.msg_.is_teach_button_pressed = data_[2]; + pub_.msg_.is_power_button_pressed = data_[3]; + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + static const size_t ARRAY_SIZE = 32; + std::bitset data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; + +/*! + * \brief Implements a publisher that publishes a datafield containing a the safety status bits. + * + */ +class SafetyStatusBitsPublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a SafetyStatusBitsPublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + SafetyStatusBitsPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + pub_.msg_ = ur_rtde_msgs::SafetyStatusBits(); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + pub_.msg_.is_normal_mode = data_[0]; + pub_.msg_.is_reduced_mode = data_[1]; + pub_.msg_.is_protective_stopped = data_[2]; + pub_.msg_.is_recovery_mode = data_[3]; + pub_.msg_.is_safeguard_stopped = data_[4]; + pub_.msg_.is_system_emergency_stopped = data_[5]; + pub_.msg_.is_robot_emergency_stopped = data_[6]; + pub_.msg_.is_emergency_stopped = data_[7]; + pub_.msg_.is_violation = data_[8]; + pub_.msg_.is_fault = data_[9]; + pub_.msg_.is_stopped_due_to_safety = data_[10]; + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + static const size_t ARRAY_SIZE = 32; + std::bitset data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; +} // namespace rtde_interface +} // namespace ur_driver + +#endif // ifndef UR_DRIVER_ROBOT_STATE_PUBLISHERS_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h b/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h new file mode 100644 index 000000000..a74e7dfec --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h @@ -0,0 +1,133 @@ +// 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-12-20 + * + */ +//---------------------------------------------------------------------- +#ifndef UR_DRIVER_SENSOR_DATA_PUBLISHERS_H_INCLUDED +#define UR_DRIVER_SENSOR_DATA_PUBLISHERS_H_INCLUDED + +#include +#include +#include +#include + +namespace ur_driver +{ +namespace rtde_interface +{ +/*! + * \brief Implements a publisher that publishes a datafield containing a duration. + * + */ +class DurationPublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a DurationPublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + DurationPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + pub_.msg_ = std_msgs::Duration(); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + pub_.msg_.data.fromSec(data_); + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + double data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; + +/*! + * \brief Implements a publisher that publishes a datafield containing the joint temperatures. + * + */ +class JointTemperaturePublisher : public DataFieldPublisher +{ +public: + /*! + * \brief Creates a JointTemperaturePublisher object. + * + * \param data_field_identifier The string identifier of the data field to publish + * \param nh The used ROS node handle + */ + JointTemperaturePublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) + : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + { + pub_.msg_ = std_msgs::Duration(); + } + + /*! + * \brief Publishes the relevant data field from a data package. + * + * \param data_package The given data package to publish from + * + * \returns True if the realtime publisher could publish the data. + */ + virtual bool publish(const DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + if (pub_.trylock()) + { + pub_.msg_.data.fromSec(data_); + pub_.unlockAndPublish(); + return true; + } + } + return false; + } + +private: + double data_; + std::string data_field_identifier_; + realtime_tools::RealtimePublisher pub_; +}; +} // namespace rtde_interface +} // namespace ur_driver + +#endif // ifndef UR_DRIVER_SENSOR_DATA_PUBLISHERS_H_INCLUDED From a7c14e15e387698bf127ed69c1bd4014c479b1c6 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Fri, 20 Dec 2019 15:14:35 +0100 Subject: [PATCH 14/22] updated factory method to utilize new publishers --- .../src/ros/data_field_publisher.cpp | 214 +++++++++++++++--- 1 file changed, 186 insertions(+), 28 deletions(-) diff --git a/ur_robot_driver/src/ros/data_field_publisher.cpp b/ur_robot_driver/src/ros/data_field_publisher.cpp index b3806814c..58a24191b 100644 --- a/ur_robot_driver/src/ros/data_field_publisher.cpp +++ b/ur_robot_driver/src/ros/data_field_publisher.cpp @@ -27,16 +27,27 @@ //---------------------------------------------------------------------- #include "ur_robot_driver/exceptions.h" +#include "ur_robot_driver/ros/geometry_data_publishers.h" +#include "ur_robot_driver/ros/sensor_data_publishers.h" +#include "ur_robot_driver/ros/robot_state_publishers.h" #include #include #include #include #include #include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace ur_driver { @@ -48,16 +59,183 @@ using _uint32_publisher = DirectDataPublisher; using _uint64_publisher = DirectDataPublisher; using _int32_publisher = DirectDataPublisher; using _double_publisher = DirectDataPublisher; -using _vector3d_publisher = ArrayDataPublisher; -using _vector6d_publisher = ArrayDataPublisher; -using _vector6int32_publisher = ArrayDataPublisher; -using _vector6uint32_publisher = ArrayDataPublisher; using _string_publisher = DirectDataPublisher; std::unique_ptr DataFieldPublisher::createFromString(const std::string& data_field_identifier, ros::NodeHandle& nh) { - if (DataPackage::isType(data_field_identifier)) + if (data_field_identifier == "timestamp") + { + return std::unique_ptr(new DurationPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "target_q") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "target_qd") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "target_qdd") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "target_current") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "target_movement") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "actual_q") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "actual_qd") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "actual_current") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "joint_control_output") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "actual_TCP_pose") + { + return std::unique_ptr(new PosePublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "actual_TCP_speed") + { + return std::unique_ptr(new TwistPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "target_TCP_pose") + { + return std::unique_ptr(new PosePublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "target_TCP_speed") + { + return std::unique_ptr(new TwistPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "joint_temperatures") + { + // could use sensor_msgs/Temperature, but would then be the only data field that is actively + // connected to the joint_names of the robot, as they are needed as reference frames + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "actual_execution_time") + { + // High possibility that actual execution time isn't given in seconds and scaling has to be + // added, but nothing found in RTDE documentation + return std::unique_ptr(new DurationPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "robot_mode") + { + return std::unique_ptr( + new ModePublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "joint_mode") + { + return std::unique_ptr(new JointModePublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "safety_mode") + { + return std::unique_ptr( + new ModePublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "safety_status") + { + return std::unique_ptr( + new StatusPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "actual_tool_accelerometer") + { + return std::unique_ptr(new Vector3Publisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "actual_joint_voltage") + { + return std::unique_ptr( + new ArrayDataPublisher(data_field_identifier, nh)); + //} else if (data_field_identifier == "runtime_state") { + // to be added once documentation for runtime_state output is found + } + else if (data_field_identifier == "elbow_position") + { + return std::unique_ptr(new Vector3Publisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "elbow_velocity") + { + return std::unique_ptr(new Vector3Publisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "robot_status_bits") + { + return std::unique_ptr(new RobotStatusBitsPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "safety_status_bits") + { + return std::unique_ptr(new SafetyStatusBitsPublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "euromap67_input_bits") + { + return std::unique_ptr(new BitRegisterArrayPublisher(data_field_identifier, nh, 0)); + } + else if (data_field_identifier == "euromap67_output_bits") + { + return std::unique_ptr(new BitRegisterArrayPublisher(data_field_identifier, nh, 0)); + } + else if (data_field_identifier == "output_bit_registers0_to_31") + { + return std::unique_ptr(new BitRegisterArrayPublisher(data_field_identifier, nh, 0)); + } + else if (data_field_identifier == "output_bit_registers32_to_63") + { + return std::unique_ptr(new BitRegisterArrayPublisher(data_field_identifier, nh, 32)); + } + else if (data_field_identifier == "input_bit_registers0_to_31") + { + return std::unique_ptr(new BitRegisterArrayPublisher(data_field_identifier, nh, 0)); + } + else if (data_field_identifier == "input_bit_registers32_to_63") + { + return std::unique_ptr(new BitRegisterArrayPublisher(data_field_identifier, nh, 32)); + } + else if (data_field_identifier == "tool_output_mode") + { + return std::unique_ptr( + new ModePublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "tool_digital_output0_mode") + { + return std::unique_ptr( + new ModePublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "tool_digital_output1_mode") + { + return std::unique_ptr( + new ModePublisher(data_field_identifier, nh)); + } + else if (data_field_identifier == "input_bit_registers0_to_31") + { + return std::unique_ptr(new BitRegisterArrayPublisher(data_field_identifier, nh, 0)); + } + else if (data_field_identifier == "input_bit_registers32_to_63") + { + return std::unique_ptr(new BitRegisterArrayPublisher(data_field_identifier, nh, 32)); + } + else if (DataPackage::isType(data_field_identifier)) { return std::unique_ptr(new _bool_publisher(data_field_identifier, nh)); } @@ -81,26 +259,6 @@ std::unique_ptr DataFieldPublisher::createFromString(const s { return std::unique_ptr(new _double_publisher(data_field_identifier, nh)); } - else if (DataPackage::isType(data_field_identifier)) - { - return std::unique_ptr(new _vector3d_publisher(data_field_identifier, nh)); - } - else if (DataPackage::isType(data_field_identifier)) - { - return std::unique_ptr(new _vector6d_publisher(data_field_identifier, nh)); - } - else if (DataPackage::isType(data_field_identifier)) - { - return std::unique_ptr(new _vector6int32_publisher(data_field_identifier, nh)); - } - else if (DataPackage::isType(data_field_identifier)) - { - return std::unique_ptr(new _vector6uint32_publisher(data_field_identifier, nh)); - } - else if (DataPackage::isType(data_field_identifier)) - { - return std::unique_ptr(new _string_publisher(data_field_identifier, nh)); - } else { throw UrException("No supported publisher for RTDE data field " + data_field_identifier); From 1a3865b7b518e207b302ed20aac2bb9146042382 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 23 Apr 2020 10:17:19 +0200 Subject: [PATCH 15/22] removed automatic "rtde_data/" prefix for publisher topics --- .../include/ur_robot_driver/ros/data_field_publisher.h | 6 +++--- .../ur_robot_driver/ros/geometry_data_publishers.h | 6 +++--- .../ur_robot_driver/ros/robot_state_publishers.h | 10 +++++----- .../ur_robot_driver/ros/sensor_data_publishers.h | 4 ++-- 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h index 62993cfa6..ed4ad1db3 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h @@ -83,7 +83,7 @@ class DirectDataPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ DirectDataPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { } @@ -130,7 +130,7 @@ class ArrayDataPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ ArrayDataPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_.data.resize(N); } @@ -180,7 +180,7 @@ class BitRegisterArrayPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ BitRegisterArrayPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh, uint8_t start_pin) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_.registers.resize(ARRAY_SIZE); for (size_t i = 0; i < ARRAY_SIZE; i++) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/geometry_data_publishers.h b/ur_robot_driver/include/ur_robot_driver/ros/geometry_data_publishers.h index 0542f2eb6..679e008b8 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/geometry_data_publishers.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/geometry_data_publishers.h @@ -53,7 +53,7 @@ class PosePublisher : public DataFieldPublisher * \param nh The used ROS node handle */ PosePublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_ = geometry_msgs::Pose(); } @@ -115,7 +115,7 @@ class TwistPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ TwistPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_ = geometry_msgs::Twist(); } @@ -166,7 +166,7 @@ class Vector3Publisher : public DataFieldPublisher * \param nh The used ROS node handle */ Vector3Publisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_ = geometry_msgs::Vector3(); } diff --git a/ur_robot_driver/include/ur_robot_driver/ros/robot_state_publishers.h b/ur_robot_driver/include/ur_robot_driver/ros/robot_state_publishers.h index 218026d61..0ac66a8d4 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/robot_state_publishers.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/robot_state_publishers.h @@ -53,7 +53,7 @@ class ModePublisher : public DataFieldPublisher * \param nh The used ROS node handle */ ModePublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { } @@ -99,7 +99,7 @@ class StatusPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ StatusPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { } @@ -144,7 +144,7 @@ class JointModePublisher : public DataFieldPublisher * \param nh The used ROS node handle */ JointModePublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_ = ur_rtde_msgs::JointMode(); pub_.msg_.mode.resize(NUM_JOINTS); @@ -195,7 +195,7 @@ class RobotStatusBitsPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ RobotStatusBitsPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_ = ur_rtde_msgs::RobotStatusBits(); } @@ -245,7 +245,7 @@ class SafetyStatusBitsPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ SafetyStatusBitsPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_ = ur_rtde_msgs::SafetyStatusBits(); } diff --git a/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h b/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h index a74e7dfec..5118a9690 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h @@ -50,7 +50,7 @@ class DurationPublisher : public DataFieldPublisher * \param nh The used ROS node handle */ DurationPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_ = std_msgs::Duration(); } @@ -96,7 +96,7 @@ class JointTemperaturePublisher : public DataFieldPublisher * \param nh The used ROS node handle */ JointTemperaturePublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, "rtde_data/" + data_field_identifier_, 1) + : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) { pub_.msg_ = std_msgs::Duration(); } From ef048be8a82828d00d2e142d52c74c61614dfa0f Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 23 Apr 2020 10:18:13 +0200 Subject: [PATCH 16/22] added new node handle in rtde_data namespace to be used by rtde_data publishers --- ur_robot_driver/src/ros/hardware_interface.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 71a031b4f..4ee374b7f 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -331,7 +331,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw { recipe.erase(std::remove(recipe.begin(), recipe.end(), s), recipe.end()); } - rtde_data_pub_.reset(new rtde_interface::DataPackagePublisher(recipe, robot_hw_nh)); + //create node_handle in rtde_data namespace + ros::NodeHandle rtde_nh(robot_hw_nh, "rtde_data"); + rtde_data_pub_.reset(new rtde_interface::DataPackagePublisher(recipe, rtde_nh)); // Set the speed slider fraction used by the robot's execution. Values should be between 0 and 1. // Only set this smaller than 1 if you are using the scaled controllers (as by default) or you know what you're From 6cea2fa338cde1ba44e082ce06125b5b04c7131d Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 23 Apr 2020 10:20:15 +0200 Subject: [PATCH 17/22] removed unused JointTemperaturePublisher class --- .../ros/sensor_data_publishers.h | 45 ------------------- 1 file changed, 45 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h b/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h index 5118a9690..3ead753b2 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h @@ -82,51 +82,6 @@ class DurationPublisher : public DataFieldPublisher realtime_tools::RealtimePublisher pub_; }; -/*! - * \brief Implements a publisher that publishes a datafield containing the joint temperatures. - * - */ -class JointTemperaturePublisher : public DataFieldPublisher -{ -public: - /*! - * \brief Creates a JointTemperaturePublisher object. - * - * \param data_field_identifier The string identifier of the data field to publish - * \param nh The used ROS node handle - */ - JointTemperaturePublisher(const std::string& data_field_identifier, ros::NodeHandle& nh) - : data_field_identifier_(data_field_identifier), pub_(nh, data_field_identifier_, 1) - { - pub_.msg_ = std_msgs::Duration(); - } - - /*! - * \brief Publishes the relevant data field from a data package. - * - * \param data_package The given data package to publish from - * - * \returns True if the realtime publisher could publish the data. - */ - virtual bool publish(const DataPackage& data_package) - { - if (data_package.getData(data_field_identifier_, data_)) - { - if (pub_.trylock()) - { - pub_.msg_.data.fromSec(data_); - pub_.unlockAndPublish(); - return true; - } - } - return false; - } - -private: - double data_; - std::string data_field_identifier_; - realtime_tools::RealtimePublisher pub_; -}; } // namespace rtde_interface } // namespace ur_driver From 587b76d07e751aa8fa7dca5198497ed1f7f10591 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 23 Apr 2020 10:23:30 +0200 Subject: [PATCH 18/22] removed _-prefix for typenames --- .../src/ros/data_field_publisher.cpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/ur_robot_driver/src/ros/data_field_publisher.cpp b/ur_robot_driver/src/ros/data_field_publisher.cpp index 58a24191b..ae69f53b7 100644 --- a/ur_robot_driver/src/ros/data_field_publisher.cpp +++ b/ur_robot_driver/src/ros/data_field_publisher.cpp @@ -53,13 +53,13 @@ namespace ur_driver { namespace rtde_interface { -using _bool_publisher = DirectDataPublisher; -using _uint8_publisher = DirectDataPublisher; -using _uint32_publisher = DirectDataPublisher; -using _uint64_publisher = DirectDataPublisher; -using _int32_publisher = DirectDataPublisher; -using _double_publisher = DirectDataPublisher; -using _string_publisher = DirectDataPublisher; +using bool_publisher = DirectDataPublisher; +using uint8_publisher = DirectDataPublisher; +using uint32_publisher = DirectDataPublisher; +using uint64_publisher = DirectDataPublisher; +using int32_publisher = DirectDataPublisher; +using double_publisher = DirectDataPublisher; +using string_publisher = DirectDataPublisher; std::unique_ptr DataFieldPublisher::createFromString(const std::string& data_field_identifier, ros::NodeHandle& nh) @@ -237,27 +237,27 @@ std::unique_ptr DataFieldPublisher::createFromString(const s } else if (DataPackage::isType(data_field_identifier)) { - return std::unique_ptr(new _bool_publisher(data_field_identifier, nh)); + return std::unique_ptr(new bool_publisher(data_field_identifier, nh)); } else if (DataPackage::isType(data_field_identifier)) { - return std::unique_ptr(new _uint8_publisher(data_field_identifier, nh)); + return std::unique_ptr(new uint8_publisher(data_field_identifier, nh)); } else if (DataPackage::isType(data_field_identifier)) { - return std::unique_ptr(new _uint32_publisher(data_field_identifier, nh)); + return std::unique_ptr(new uint32_publisher(data_field_identifier, nh)); } else if (DataPackage::isType(data_field_identifier)) { - return std::unique_ptr(new _uint64_publisher(data_field_identifier, nh)); + return std::unique_ptr(new uint64_publisher(data_field_identifier, nh)); } else if (DataPackage::isType(data_field_identifier)) { - return std::unique_ptr(new _int32_publisher(data_field_identifier, nh)); + return std::unique_ptr(new int32_publisher(data_field_identifier, nh)); } else if (DataPackage::isType(data_field_identifier)) { - return std::unique_ptr(new _double_publisher(data_field_identifier, nh)); + return std::unique_ptr(new double_publisher(data_field_identifier, nh)); } else { From 23b71072fdf213453357898c5d0e98154dbdc410 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 23 Apr 2020 10:25:08 +0200 Subject: [PATCH 19/22] fixed target_moment typo --- ur_robot_driver/src/ros/data_field_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/src/ros/data_field_publisher.cpp b/ur_robot_driver/src/ros/data_field_publisher.cpp index ae69f53b7..5b3f97a25 100644 --- a/ur_robot_driver/src/ros/data_field_publisher.cpp +++ b/ur_robot_driver/src/ros/data_field_publisher.cpp @@ -88,7 +88,7 @@ std::unique_ptr DataFieldPublisher::createFromString(const s return std::unique_ptr( new ArrayDataPublisher(data_field_identifier, nh)); } - else if (data_field_identifier == "target_movement") + else if (data_field_identifier == "target_moment") { return std::unique_ptr( new ArrayDataPublisher(data_field_identifier, nh)); From 6a7fc80cfa1828293569d9a65ddc1a5185dabaa4 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 23 Apr 2020 10:26:58 +0200 Subject: [PATCH 20/22] added "robot_mode" and "safety_mode" to list of manually published rtde data fields --- ur_robot_driver/src/ros/hardware_interface.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 4ee374b7f..32594b01b 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -325,7 +325,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw "actual_digital_input_bits", "actual_digital_output_bits", "analog_io_types", - "tool_analog_input_types" }; + "tool_analog_input_types", + "robot_mode", + "safety_mode"}; std::vector recipe = ur_driver_->getRTDEOutputRecipe(); for (auto s : already_published) { From cfdd29881f027999c9f78742c62b90714889ba6e Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 23 Apr 2020 10:30:06 +0200 Subject: [PATCH 21/22] updated ur_rtde_msgts package metadata --- ur_rtde_msgs/package.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ur_rtde_msgs/package.xml b/ur_rtde_msgs/package.xml index 5a6a651c3..c15490e15 100644 --- a/ur_rtde_msgs/package.xml +++ b/ur_rtde_msgs/package.xml @@ -1,13 +1,14 @@ ur_rtde_msgs - 0.0.0 + 0.0.2 The ur_rtde_msgs package - Tristan Schnell + Felix Exner + Tristan Schnell From 451f6f4eb18e6b4cbaf01d7f0d929b2d960079ff Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 23 Apr 2020 10:44:46 +0200 Subject: [PATCH 22/22] formatting fix for hardware_interface --- ur_robot_driver/src/ros/hardware_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 32594b01b..4e1b4c15b 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -327,13 +327,13 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw "analog_io_types", "tool_analog_input_types", "robot_mode", - "safety_mode"}; + "safety_mode" }; std::vector recipe = ur_driver_->getRTDEOutputRecipe(); for (auto s : already_published) { recipe.erase(std::remove(recipe.begin(), recipe.end(), s), recipe.end()); } - //create node_handle in rtde_data namespace + // create node_handle in rtde_data namespace ros::NodeHandle rtde_nh(robot_hw_nh, "rtde_data"); rtde_data_pub_.reset(new rtde_interface::DataPackagePublisher(recipe, rtde_nh));