Skip to content

Commit 5a0bde5

Browse files
ahcordeahcorde
authored
Added point_cloud_transport (#1008)
Signed-off-by: Alejandro Hernández Cordero <[email protected]>
1 parent f2fa3e7 commit 5a0bde5

File tree

7 files changed

+332
-5
lines changed

7 files changed

+332
-5
lines changed

‎rviz_default_plugins/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,7 @@ find_package(laser_geometry REQUIRED)
7070
find_package(map_msgs REQUIRED)
7171
find_package(nav_msgs REQUIRED)
7272
find_package(pluginlib REQUIRED)
73+
find_package(point_cloud_transport REQUIRED)
7374
find_package(rclcpp REQUIRED)
7475
find_package(resource_retriever REQUIRED)
7576
find_package(sensor_msgs REQUIRED)
@@ -182,6 +183,7 @@ set(rviz_default_plugins_source_files
182183
src/rviz_default_plugins/displays/pointcloud/transformers/rgb8_pc_transformer.cpp
183184
src/rviz_default_plugins/displays/pointcloud/transformers/rgbf32_pc_transformer.cpp
184185
src/rviz_default_plugins/displays/pointcloud/transformers/xyz_pc_transformer.cpp
186+
src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp
185187
src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp
186188
src/rviz_default_plugins/displays/pointcloud/point_cloud_to_point_cloud2.cpp
187189
src/rviz_default_plugins/displays/pointcloud/point_cloud_transformer_factory.cpp
@@ -246,6 +248,7 @@ target_link_libraries(rviz_default_plugins PUBLIC
246248
laser_geometry::laser_geometry
247249
${map_msgs_TARGETS}
248250
${nav_msgs_TARGETS}
251+
point_cloud_transport::point_cloud_transport
249252
rclcpp::rclcpp
250253
rviz_common::rviz_common
251254
rviz_ogre_vendor::OgreMain

‎rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.hpp

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
/*
2+
* Copyright (c) 2023, Open Source Robotics Foundation, Inc.
3+
* All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* * Redistributions of source code must retain the above copyright
9+
* notice, this list of conditions and the following disclaimer.
10+
* * Redistributions in binary form must reproduce the above copyright
11+
* notice, this list of conditions and the following disclaimer in the
12+
* documentation and/or other materials provided with the distribution.
13+
* * Neither the name of the copyright holder nor the names of its contributors
14+
* may be used to endorse or promote products derived from
15+
* this software without specific prior written permission.
16+
*
17+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21+
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
* POSSIBILITY OF SUCH DAMAGE.
28+
*/
29+
30+
#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__GET_TRANSPORT_FROM_TOPIC_HPP_
31+
#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__GET_TRANSPORT_FROM_TOPIC_HPP_
32+
33+
#include <string>
34+
35+
#include "rviz_default_plugins/visibility_control.hpp"
36+
37+
namespace rviz_default_plugins
38+
{
39+
namespace displays
40+
{
41+
42+
RVIZ_DEFAULT_PLUGINS_PUBLIC
43+
std::string getPointCloud2TransportFromTopic(const std::string & topic);
44+
45+
RVIZ_DEFAULT_PLUGINS_PUBLIC
46+
std::string getPointCloud2BaseTopicFromTopic(const std::string & topic);
47+
48+
49+
} // end namespace displays
50+
} // end namespace rviz_default_plugins
51+
52+
#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__GET_TRANSPORT_FROM_TOPIC_HPP_

‎rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud2_display.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@
3434

3535
#include "sensor_msgs/msg/point_cloud2.hpp"
3636

37-
#include "rviz_common/message_filter_display.hpp"
37+
#include "rviz_default_plugins/displays/pointcloud/point_cloud_transport_display.hpp"
3838

3939
#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp"
4040
#include "rviz_default_plugins/visibility_control.hpp"
@@ -67,7 +67,7 @@ struct Offsets
6767
* all being 8 bits.
6868
*/
6969
class RVIZ_DEFAULT_PLUGINS_PUBLIC PointCloud2Display : public
70-
rviz_common::MessageFilterDisplay<sensor_msgs::msg::PointCloud2>
70+
rviz_default_plugins::displays::PointCloud2TransportDisplay<sensor_msgs::msg::PointCloud2>
7171
{
7272
public:
7373
PointCloud2Display();

‎rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_transport_display.hpp

Lines changed: 208 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,208 @@
1+
/*
2+
* Copyright (c) 2023, Open Source Robotics Foundation, Inc.
3+
* All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted (subject to the limitations in the disclaimer
7+
* below) provided that the following conditions are met:
8+
*
9+
* * Redistributions of source code must retain the above copyright
10+
* notice, this list of conditions and the following disclaimer.
11+
* * Redistributions in binary form must reproduce the above copyright
12+
* notice, this list of conditions and the following disclaimer in the
13+
* documentation and/or other materials provided with the distribution.
14+
* * Neither the name of the copyright holder nor the names of its
15+
* contributors may be used to endorse or promote products derived from
16+
* this software without specific prior written permission.
17+
*
18+
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
19+
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
21+
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22+
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23+
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24+
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25+
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26+
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27+
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28+
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29+
* POSSIBILITY OF SUCH DAMAGE.
30+
*/
31+
32+
#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TRANSPORT_DISPLAY_HPP_
33+
#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TRANSPORT_DISPLAY_HPP_
34+
35+
#include <memory>
36+
37+
#include "get_transport_from_topic.hpp"
38+
#include "point_cloud_transport/point_cloud_transport.hpp"
39+
#include "point_cloud_transport/subscriber_filter.hpp"
40+
#include "rviz_common/ros_topic_display.hpp"
41+
42+
namespace rviz_default_plugins
43+
{
44+
namespace displays
45+
{
46+
47+
template<class MessageType>
48+
class PointCloud2TransportDisplay : public rviz_common::_RosTopicDisplay
49+
{
50+
// No Q_OBJECT macro here, moc does not support Q_OBJECT in a templated class.
51+
52+
public:
53+
/// Convenience typedef so subclasses don't have to use
54+
/// the long templated class name to refer to their super class.
55+
typedef PointCloud2TransportDisplay<MessageType> PC2RDClass;
56+
57+
PointCloud2TransportDisplay()
58+
: messages_received_(0)
59+
{
60+
QString message_type = QString::fromStdString(rosidl_generator_traits::name<MessageType>());
61+
topic_property_->setMessageType(message_type);
62+
topic_property_->setDescription(message_type + " topic to subscribe to.");
63+
}
64+
65+
/**
66+
* When overriding this method, the onInitialize() method of this superclass has to be called.
67+
* Otherwise, the ros node will not be initialized.
68+
*/
69+
void onInitialize() override
70+
{
71+
_RosTopicDisplay::onInitialize();
72+
}
73+
74+
~PointCloud2TransportDisplay() override
75+
{
76+
unsubscribe();
77+
}
78+
79+
void reset() override
80+
{
81+
Display::reset();
82+
messages_received_ = 0;
83+
}
84+
85+
void setTopic(const QString & topic, const QString & datatype) override
86+
{
87+
(void) datatype;
88+
topic_property_->setString(topic);
89+
}
90+
91+
protected:
92+
void updateTopic() override
93+
{
94+
resetSubscription();
95+
}
96+
97+
virtual void subscribe()
98+
{
99+
if (!isEnabled()) {
100+
return;
101+
}
102+
103+
if (topic_property_->isEmpty()) {
104+
setStatus(
105+
rviz_common::properties::StatusProperty::Error, "Topic",
106+
QString("Error subscribing: Empty topic name"));
107+
return;
108+
}
109+
110+
try {
111+
subscription_ = std::make_shared<point_cloud_transport::SubscriberFilter>();
112+
subscription_->subscribe(
113+
rviz_ros_node_.lock()->get_raw_node(),
114+
getPointCloud2BaseTopicFromTopic(topic_property_->getTopicStd()),
115+
getPointCloud2TransportFromTopic(topic_property_->getTopicStd()),
116+
qos_profile.get_rmw_qos_profile());
117+
subscription_start_time_ = rviz_ros_node_.lock()->get_raw_node()->now();
118+
subscription_callback_ = subscription_->registerCallback(
119+
std::bind(
120+
&PointCloud2TransportDisplay<MessageType>::incomingMessage, this, std::placeholders::_1));
121+
setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK");
122+
} catch (rclcpp::exceptions::InvalidTopicNameError & e) {
123+
setStatus(
124+
rviz_common::properties::StatusProperty::Error, "Topic",
125+
QString("Error subscribing: ") + e.what());
126+
}
127+
}
128+
129+
void transformerChangedCallback() override
130+
{
131+
resetSubscription();
132+
}
133+
134+
void resetSubscription()
135+
{
136+
unsubscribe();
137+
reset();
138+
subscribe();
139+
context_->queueRender();
140+
}
141+
142+
virtual void unsubscribe()
143+
{
144+
subscription_.reset();
145+
}
146+
147+
void onEnable() override
148+
{
149+
subscribe();
150+
}
151+
152+
void onDisable() override
153+
{
154+
unsubscribe();
155+
reset();
156+
}
157+
158+
/// Incoming message callback.
159+
/**
160+
* Checks if the message pointer
161+
* is valid, increments messages_received_, then calls
162+
* processMessage().
163+
*/
164+
void incomingMessage(const typename MessageType::ConstSharedPtr msg)
165+
{
166+
if (!msg) {
167+
return;
168+
}
169+
170+
++messages_received_;
171+
QString topic_str = QString::number(messages_received_) + " messages received";
172+
// Append topic subscription frequency if we can lock rviz_ros_node_.
173+
std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_interface =
174+
rviz_ros_node_.lock();
175+
if (node_interface != nullptr) {
176+
const double duration =
177+
(node_interface->get_raw_node()->now() - subscription_start_time_).seconds();
178+
const double subscription_frequency =
179+
static_cast<double>(messages_received_) / duration;
180+
topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz.";
181+
}
182+
setStatus(
183+
rviz_common::properties::StatusProperty::Ok,
184+
"Topic",
185+
topic_str);
186+
187+
processMessage(msg);
188+
}
189+
190+
191+
/// Implement this to process the contents of a message.
192+
/**
193+
* This is called by incomingMessage().
194+
*/
195+
virtual void processMessage(typename MessageType::ConstSharedPtr msg) = 0;
196+
197+
uint32_t messages_received_;
198+
rclcpp::Time subscription_start_time_;
199+
200+
std::shared_ptr<point_cloud_transport::SubscriberFilter> subscription_;
201+
message_filters::Connection subscription_callback_;
202+
};
203+
204+
} // end namespace displays
205+
} // end namespace rviz_default_plugins
206+
207+
208+
#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__POINTCLOUD__POINT_CLOUD_TRANSPORT_DISPLAY_HPP_

‎rviz_default_plugins/package.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@
4545
<depend>nav_msgs</depend>
4646
<depend>map_msgs</depend>
4747
<depend>pluginlib</depend>
48+
<depend>point_cloud_transport</depend>
4849
<depend>rclcpp</depend>
4950
<depend>resource_retriever</depend>
5051
<depend>rviz_common</depend>

‎rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
/*
2+
* Copyright (c) 2023, Open Source Robotics Foundation, Inc.
3+
* All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* * Redistributions of source code must retain the above copyright
9+
* notice, this list of conditions and the following disclaimer.
10+
* * Redistributions in binary form must reproduce the above copyright
11+
* notice, this list of conditions and the following disclaimer in the
12+
* documentation and/or other materials provided with the distribution.
13+
* * Neither the name of the copyright holder nor the names of its contributors
14+
* may be used to endorse or promote products derived from
15+
* this software without specific prior written permission.
16+
*
17+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21+
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
* POSSIBILITY OF SUCH DAMAGE.
28+
*/
29+
30+
#include <string>
31+
32+
#include "rviz_default_plugins/displays/pointcloud/get_transport_from_topic.hpp"
33+
34+
namespace rviz_default_plugins
35+
{
36+
namespace displays
37+
{
38+
39+
bool isPointCloud2RawTransport(const std::string & topic)
40+
{
41+
std::string last_subtopic = topic.substr(topic.find_last_of('/') + 1);
42+
return last_subtopic != "draco" && last_subtopic != "zlib" &&
43+
last_subtopic != "pcl" && last_subtopic != "zstd";
44+
}
45+
46+
std::string getPointCloud2TransportFromTopic(const std::string & topic)
47+
{
48+
if (isPointCloud2RawTransport(topic)) {
49+
return "raw";
50+
}
51+
return topic.substr(topic.find_last_of('/') + 1);
52+
}
53+
54+
std::string getPointCloud2BaseTopicFromTopic(const std::string & topic)
55+
{
56+
if (isPointCloud2RawTransport(topic)) {
57+
return topic;
58+
}
59+
return topic.substr(0, topic.find_last_of('/'));
60+
}
61+
62+
} // end namespace displays
63+
} // end namespace rviz_default_plugins

0 commit comments

Comments
 (0)