diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 57f550358..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 @@ -112,6 +114,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}) @@ -120,6 +123,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}) 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..ed4ad1db3 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h @@ -0,0 +1,226 @@ +// 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 +#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 DataPackage& 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_(nh, 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_.data = 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 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 ArrayDataPublisher 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_(nh, 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 DataPackage& data_package) + { + if (data_package.getData(data_field_identifier_, data_)) + { + 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_; + 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, 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 + +#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..d672bb059 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h @@ -0,0 +1,82 @@ +// 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 +{ +/*! + * \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) + { + publishers_.push_back(DataFieldPublisher::createFromString(str, nh)); + } + } + + /*! + * \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_) + { + 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/geometry_data_publishers.h b/ur_robot_driver/include/ur_robot_driver/ros/geometry_data_publishers.h new file mode 100644 index 000000000..679e008b8 --- /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, 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, 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, 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/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/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..0ac66a8d4 --- /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, 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, 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, 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, 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, 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..3ead753b2 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/ros/sensor_data_publishers.h @@ -0,0 +1,88 @@ +// 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, 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 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..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 @@ -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. * @@ -114,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 { @@ -139,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/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/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_robot_driver/src/ros/data_field_publisher.cpp b/ur_robot_driver/src/ros/data_field_publisher.cpp new file mode 100644 index 000000000..5b3f97a25 --- /dev/null +++ b/ur_robot_driver/src/ros/data_field_publisher.cpp @@ -0,0 +1,268 @@ +#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 "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 + +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; + +std::unique_ptr DataFieldPublisher::createFromString(const std::string& data_field_identifier, + ros::NodeHandle& nh) +{ + 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_moment") + { + 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)); + } + 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 + { + 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..4e1b4c15b 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -303,6 +303,40 @@ 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", + "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", + "robot_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 + 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 // doing. Using this with other controllers might lead to unexpected behaviors. @@ -394,6 +428,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)) { 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 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/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/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/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/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/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 diff --git a/ur_rtde_msgs/package.xml b/ur_rtde_msgs/package.xml new file mode 100644 index 000000000..c15490e15 --- /dev/null +++ b/ur_rtde_msgs/package.xml @@ -0,0 +1,66 @@ + + + ur_rtde_msgs + 0.0.2 + The ur_rtde_msgs package + + + + + Felix Exner + Tristan Schnell + + + + + + Apache 2.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + message_generation + message_runtime + message_runtime + ur_msgs + ur_msgs + ur_msgs + + + + + + + +