Skip to content

Commit 7ca839c

Browse files
t-schnellt-schnell
committed
Added generalized publisher to automatically publish RTDE data package values to ROS topics
1 parent 1bcac0e commit 7ca839c

File tree

6 files changed

+363
-0
lines changed

6 files changed

+363
-0
lines changed

‎ur_robot_driver/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -105,6 +105,7 @@ add_library(ur_robot_driver
105105
src/ur/dashboard_client.cpp
106106
src/ur/tool_communication.cpp
107107
src/rtde/rtde_writer.cpp
108+
src/ros/data_field_publisher.cpp
108109
)
109110
target_link_libraries(ur_robot_driver ${catkin_LIBRARIES})
110111
add_dependencies(ur_robot_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

‎ur_robot_driver/include/ur_robot_driver/ros/data_field_publisher.h‎

Lines changed: 176 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,176 @@
1+
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2+
3+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
4+
// Copyright 2019 FZI Forschungszentrum Informatik
5+
//
6+
// Licensed under the Apache License, Version 2.0 (the "License");
7+
// you may not use this file except in compliance with the License.
8+
// You may obtain a copy of the License at
9+
//
10+
// http://www.apache.org/licenses/LICENSE-2.0
11+
//
12+
// Unless required by applicable law or agreed to in writing, software
13+
// distributed under the License is distributed on an "AS IS" BASIS,
14+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
// See the License for the specific language governing permissions and
16+
// limitations under the License.
17+
// -- END LICENSE BLOCK ------------------------------------------------
18+
19+
//----------------------------------------------------------------------
20+
/*!\file
21+
*
22+
* \author Tristan Schnell [email protected]
23+
* \date 2019-10-30
24+
*
25+
*/
26+
//----------------------------------------------------------------------
27+
#ifndef UR_DRIVER_DATA_FIELD_PUBLISHER_H_INCLUDED
28+
#define UR_DRIVER_DATA_FIELD_PUBLISHER_H_INCLUDED
29+
30+
#include <realtime_tools/realtime_publisher.h>
31+
#include <ur_robot_driver/rtde/data_package.h>
32+
33+
namespace ur_driver
34+
{
35+
namespace rtde_interface
36+
{
37+
/*!
38+
* \brief The DataFieldPublisher class implements an abstract wrapper for various ROS publishers
39+
* that publish fields of the RTDE data package. In addition, it contains a static factory to
40+
* create correct publishers for a data field.
41+
*/
42+
class DataFieldPublisher
43+
{
44+
public:
45+
DataFieldPublisher() = default;
46+
47+
/*!
48+
* \brief Publishes partial information from a data package.
49+
*
50+
* \param data_package The given data package to publish from
51+
*
52+
* \returns True if the realtime publisher could publish the data.
53+
*/
54+
virtual bool publish(const std::unique_ptr<DataPackage>& data_package) = 0;
55+
56+
/*!
57+
* \brief Creates a DataFieldPublisher object based on a given data field.
58+
*
59+
* \param data_field_identifier The name of the data field to publish
60+
* \param nh The used ROS node handle
61+
*
62+
* \returns A unique pointer to the created Publisher object
63+
*/
64+
static std::unique_ptr<DataFieldPublisher> createFromString(const std::string& data_field_identifier,
65+
ros::NodeHandle& nh);
66+
};
67+
68+
/*!
69+
* \brief Implements a publisher that directly publishes a datafield of a given type to a ROS topic
70+
* of a given message type.
71+
*
72+
*/
73+
template <typename DataT, typename MsgT>
74+
class DirectDataPublisher : public DataFieldPublisher
75+
{
76+
public:
77+
/*!
78+
* \brief Creates a DirectDataPublisher object.
79+
*
80+
* \param data_field_identifier The string identifier of the data field to publish
81+
* \param nh The used ROS node handle
82+
*/
83+
DirectDataPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh)
84+
: data_field_identifier_(data_field_identifier)
85+
{
86+
pub_.reset(new realtime_tools::RealtimePublisher<MsgT>(nh, "rtde_data/" + data_field_identifier_, 1));
87+
}
88+
89+
/*!
90+
* \brief Publishes the relevant data field from a data package.
91+
*
92+
* \param data_package The given data package to publish from
93+
*
94+
* \returns True if the realtime publisher could publish the data.
95+
*/
96+
virtual bool publish(const std::unique_ptr<DataPackage>& data_package)
97+
{
98+
if (data_package->getData(data_field_identifier_, data_))
99+
{
100+
if (pub_)
101+
{
102+
if (pub_->trylock())
103+
{
104+
pub_->msg_.data = data_;
105+
pub_->unlockAndPublish();
106+
return true;
107+
}
108+
}
109+
}
110+
return false;
111+
}
112+
113+
private:
114+
DataT data_;
115+
std::string data_field_identifier_;
116+
std::unique_ptr<realtime_tools::RealtimePublisher<MsgT>> pub_;
117+
};
118+
119+
/*!
120+
* \brief Implements a publisher that publishes a datafield containing an array of a given type and size to a ROS topic
121+
* of a given message type.
122+
*
123+
*/
124+
template <typename DataT, typename MsgT, size_t N>
125+
class ArrayDataPublisher : public DataFieldPublisher
126+
{
127+
public:
128+
/*!
129+
* \brief Creates a DirectDataPublisher object.
130+
*
131+
* \param data_field_identifier The string identifier of the data field to publish
132+
* \param nh The used ROS node handle
133+
*/
134+
ArrayDataPublisher(const std::string& data_field_identifier, ros::NodeHandle& nh)
135+
: data_field_identifier_(data_field_identifier)
136+
{
137+
pub_.reset(new realtime_tools::RealtimePublisher<MsgT>(nh, "rtde_data/" + data_field_identifier_, 1));
138+
pub_->msg_.data.resize(N);
139+
}
140+
141+
/*!
142+
* \brief Publishes the relevant data field from a data package.
143+
*
144+
* \param data_package The given data package to publish from
145+
*
146+
* \returns True if the realtime publisher could publish the data.
147+
*/
148+
virtual bool publish(const std::unique_ptr<DataPackage>& data_package)
149+
{
150+
if (data_package->getData(data_field_identifier_, data_))
151+
{
152+
if (pub_)
153+
{
154+
if (pub_->trylock())
155+
{
156+
for (size_t i = 0; i < N; i++)
157+
{
158+
pub_->msg_.data[i] = data_[i];
159+
}
160+
pub_->unlockAndPublish();
161+
return true;
162+
}
163+
}
164+
}
165+
return false;
166+
}
167+
168+
private:
169+
std::array<DataT, N> data_;
170+
std::string data_field_identifier_;
171+
std::unique_ptr<realtime_tools::RealtimePublisher<MsgT>> pub_;
172+
};
173+
} // namespace rtde_interface
174+
} // namespace ur_driver
175+
176+
#endif // ifndef UR_DRIVER_DATA_FIELD_PUBLISHER_H_INCLUDED

‎ur_robot_driver/include/ur_robot_driver/ros/data_package_publisher.h‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2+
3+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
4+
// Copyright 2019 FZI Forschungszentrum Informatik
5+
//
6+
// Licensed under the Apache License, Version 2.0 (the "License");
7+
// you may not use this file except in compliance with the License.
8+
// You may obtain a copy of the License at
9+
//
10+
// http://www.apache.org/licenses/LICENSE-2.0
11+
//
12+
// Unless required by applicable law or agreed to in writing, software
13+
// distributed under the License is distributed on an "AS IS" BASIS,
14+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
// See the License for the specific language governing permissions and
16+
// limitations under the License.
17+
// -- END LICENSE BLOCK ------------------------------------------------
18+
19+
//----------------------------------------------------------------------
20+
/*!\file
21+
*
22+
* \author Tristan Schnell [email protected]
23+
* \date 2019-10-30
24+
*
25+
*/
26+
//----------------------------------------------------------------------
27+
#ifndef UR_DRIVER_DATA_PACKAGE_PUBLISHER_H_INCLUDED
28+
#define UR_DRIVER_DATA_PACKAGE_PUBLISHER_H_INCLUDED
29+
30+
#include <ur_robot_driver/rtde/data_package.h>
31+
#include <ur_robot_driver/ros/data_field_publisher.h>
32+
#include <std_msgs/Int32.h>
33+
34+
namespace ur_driver
35+
{
36+
namespace rtde_interface
37+
{
38+
class DataPackagePublisher
39+
{
40+
public:
41+
DataPackagePublisher() = delete;
42+
43+
DataPackagePublisher(const std::vector<std::string>& recipe, ros::NodeHandle& nh) : recipe_(recipe)
44+
{
45+
for (auto str : recipe)
46+
{
47+
publishers_.push_back(DataFieldPublisher::createFromString(str, nh));
48+
}
49+
}
50+
51+
void publishData(const std::unique_ptr<DataPackage>& data_package)
52+
{
53+
for (auto const& i : publishers_)
54+
{
55+
i->publish(data_package);
56+
}
57+
}
58+
59+
private:
60+
std::vector<std::string> recipe_;
61+
std::list<std::unique_ptr<DataFieldPublisher>> publishers_;
62+
};
63+
64+
} // namespace rtde_interface
65+
} // namespace ur_driver
66+
67+
#endif // ifndef UR_DRIVER_DATA_PACKAGE_PUBLISHER_H_INCLUDED

‎ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,7 @@
5353

5454
#include <ur_dashboard_msgs/RobotMode.h>
5555
#include <ur_dashboard_msgs/SafetyMode.h>
56+
#include "ur_robot_driver/ros/data_package_publisher.h"
5657

5758
namespace ur_driver
5859
{
@@ -239,6 +240,7 @@ class HardwareInterface : public hardware_interface::RobotHW
239240
std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>> tool_data_pub_;
240241
std::unique_ptr<realtime_tools::RealtimePublisher<ur_dashboard_msgs::RobotMode>> robot_mode_pub_;
241242
std::unique_ptr<realtime_tools::RealtimePublisher<ur_dashboard_msgs::SafetyMode>> safety_mode_pub_;
243+
std::unique_ptr<rtde_interface::DataPackagePublisher> rtde_data_pub_;
242244

243245
ros::ServiceServer set_speed_slider_srv_;
244246
ros::ServiceServer set_io_srv_;

‎ur_robot_driver/src/ros/data_field_publisher.cpp‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
#include "ur_robot_driver/ros/data_field_publisher.h"
2+
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
3+
4+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
5+
// Copyright 2019 FZI Forschungszentrum Informatik
6+
//
7+
// Licensed under the Apache License, Version 2.0 (the "License");
8+
// you may not use this file except in compliance with the License.
9+
// You may obtain a copy of the License at
10+
//
11+
// http://www.apache.org/licenses/LICENSE-2.0
12+
//
13+
// Unless required by applicable law or agreed to in writing, software
14+
// distributed under the License is distributed on an "AS IS" BASIS,
15+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16+
// See the License for the specific language governing permissions and
17+
// limitations under the License.
18+
// -- END LICENSE BLOCK ------------------------------------------------
19+
20+
//----------------------------------------------------------------------
21+
/*!\file
22+
*
23+
* \author Tristan Schnell [email protected]
24+
* \date 2019-10-30
25+
*
26+
*/
27+
//----------------------------------------------------------------------
28+
29+
#include "ur_robot_driver/exceptions.h"
30+
#include <std_msgs/Bool.h>
31+
#include <std_msgs/UInt8.h>
32+
#include <std_msgs/UInt32.h>
33+
#include <std_msgs/UInt64.h>
34+
#include <std_msgs/Int32.h>
35+
#include <std_msgs/Float64.h>
36+
#include <std_msgs/Float64MultiArray.h>
37+
#include <std_msgs/Int32MultiArray.h>
38+
#include <std_msgs/UInt32MultiArray.h>
39+
#include <std_msgs/String.h>
40+
41+
namespace ur_driver
42+
{
43+
namespace rtde_interface
44+
{
45+
using _bool_publisher = DirectDataPublisher<bool, std_msgs::Bool>;
46+
using _uint8_publisher = DirectDataPublisher<uint8_t, std_msgs::UInt8>;
47+
using _uint32_publisher = DirectDataPublisher<uint32_t, std_msgs::UInt32>;
48+
using _uint64_publisher = DirectDataPublisher<uint64_t, std_msgs::UInt64>;
49+
using _int32_publisher = DirectDataPublisher<int32_t, std_msgs::Int32>;
50+
using _double_publisher = DirectDataPublisher<double, std_msgs::Float64>;
51+
using _vector3d_publisher = ArrayDataPublisher<double, std_msgs::Float64MultiArray, 3>;
52+
using _vector6d_publisher = ArrayDataPublisher<double, std_msgs::Float64MultiArray, 6>;
53+
using _vector6int32_publisher = ArrayDataPublisher<int32_t, std_msgs::Int32MultiArray, 6>;
54+
using _vector6uint32_publisher = ArrayDataPublisher<uint32_t, std_msgs::UInt32MultiArray, 6>;
55+
using _string_publisher = DirectDataPublisher<std::string, std_msgs::String>;
56+
57+
std::unique_ptr<DataFieldPublisher> DataFieldPublisher::createFromString(const std::string& data_field_identifier,
58+
ros::NodeHandle& nh)
59+
{
60+
if (DataPackage::isType<bool>(data_field_identifier))
61+
{
62+
return std::unique_ptr<DataFieldPublisher>(new _bool_publisher(data_field_identifier, nh));
63+
}
64+
else if (DataPackage::isType<uint8_t>(data_field_identifier))
65+
{
66+
return std::unique_ptr<DataFieldPublisher>(new _uint8_publisher(data_field_identifier, nh));
67+
}
68+
else if (DataPackage::isType<uint32_t>(data_field_identifier))
69+
{
70+
return std::unique_ptr<DataFieldPublisher>(new _uint32_publisher(data_field_identifier, nh));
71+
}
72+
else if (DataPackage::isType<uint64_t>(data_field_identifier))
73+
{
74+
return std::unique_ptr<DataFieldPublisher>(new _uint64_publisher(data_field_identifier, nh));
75+
}
76+
else if (DataPackage::isType<int32_t>(data_field_identifier))
77+
{
78+
return std::unique_ptr<DataFieldPublisher>(new _int32_publisher(data_field_identifier, nh));
79+
}
80+
else if (DataPackage::isType<double_t>(data_field_identifier))
81+
{
82+
return std::unique_ptr<DataFieldPublisher>(new _double_publisher(data_field_identifier, nh));
83+
}
84+
else if (DataPackage::isType<vector3d_t>(data_field_identifier))
85+
{
86+
return std::unique_ptr<DataFieldPublisher>(new _vector3d_publisher(data_field_identifier, nh));
87+
}
88+
else if (DataPackage::isType<vector6d_t>(data_field_identifier))
89+
{
90+
return std::unique_ptr<DataFieldPublisher>(new _vector6d_publisher(data_field_identifier, nh));
91+
}
92+
else if (DataPackage::isType<vector6int32_t>(data_field_identifier))
93+
{
94+
return std::unique_ptr<DataFieldPublisher>(new _vector6int32_publisher(data_field_identifier, nh));
95+
}
96+
else if (DataPackage::isType<vector6uint32_t>(data_field_identifier))
97+
{
98+
return std::unique_ptr<DataFieldPublisher>(new _vector6uint32_publisher(data_field_identifier, nh));
99+
}
100+
else if (DataPackage::isType<std::string>(data_field_identifier))
101+
{
102+
return std::unique_ptr<DataFieldPublisher>(new _string_publisher(data_field_identifier, nh));
103+
}
104+
else
105+
{
106+
throw UrException("No supported publisher for RTDE data field " + data_field_identifier);
107+
}
108+
}
109+
} // namespace rtde_interface
110+
} // namespace ur_driver

0 commit comments

Comments
 (0)