Rtde recipe publishing

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})

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 <realtime_tools/realtime_publisher.h>
+#include <ur_robot_driver/rtde/data_package.h>
+#include <ur_rtde_msgs/BitRegisterArray.h>
+#include <ur_msgs/Digital.h>
+
+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<DataFieldPublisher> 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 <typename DataT, typename MsgT>
+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, "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_.data = data_;
+        pub_.unlockAndPublish();
+        return true;
+      }
+    }
+    return false;
+  }
+
+private:
+  DataT data_;
+  std::string data_field_identifier_;
+  realtime_tools::RealtimePublisher<MsgT> 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 <typename DataT, typename MsgT, size_t N>
+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, "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 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<DataT, N> data_;
+  std::string data_field_identifier_;
+  realtime_tools::RealtimePublisher<MsgT> 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<uint32_t, ARRAY_SIZE>(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<ARRAY_SIZE> data_;
+  std::string data_field_identifier_;
+  realtime_tools::RealtimePublisher<ur_rtde_msgs::BitRegisterArray> pub_;
+};
+}  // namespace rtde_interface
+}  // namespace ur_driver
+
+#endif  // ifndef UR_DRIVER_DATA_FIELD_PUBLISHER_H_INCLUDED

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 <ur_robot_driver/rtde/data_package.h>
+#include <ur_robot_driver/ros/data_field_publisher.h>
+#include <std_msgs/Int32.h>
+
+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<std::string>& 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<std::string> recipe_;
+  std::list<std::unique_ptr<DataFieldPublisher>> publishers_;
+};
+
+}  // namespace rtde_interface
+}  // namespace ur_driver
+
+#endif  // ifndef UR_DRIVER_DATA_PACKAGE_PUBLISHER_H_INCLUDED