Skip to content

Commit 188eb92

Browse files
committed
first draft and devcontainer
1 parent c7a05d9 commit 188eb92

File tree

8 files changed

+283
-4
lines changed

8 files changed

+283
-4
lines changed

.devcontainer/devcontainer.json

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
// For format details, see https://aka.ms/devcontainer.json. For config options, see the
2+
// README at: https://github.com/devcontainers/templates/tree/main/src/docker-existing-dockerfile
3+
{
4+
"name": "rviz_rolling",
5+
"image": "ros:rolling-ros-base"
6+
7+
// Features to add to the dev container. More info: https://containers.dev/features.
8+
// "features": {},
9+
10+
// Use 'forwardPorts' to make a list of ports inside the container available locally.
11+
// "forwardPorts": [],
12+
13+
// Uncomment the next line to run commands after the container is created.
14+
// "postCreateCommand": "cat /etc/os-release",
15+
16+
// Configure tool-specific properties.
17+
// "customizations": {}
18+
19+
20+
// Uncomment to connect as an existing user other than the container default. More info: https://aka.ms/dev-containers-non-root.
21+
// "remoteUser": "devcontainer"
22+
"workspaceMount": "source=${~/ros2_rolling},target=/ros2_rolling,type=bind",
23+
"workspaceFolder": "/ros2_rolling/src/ros2/rviz",
24+
//"overrideCommand": false
25+
"runArgs": [
26+
"--network=host"
27+
]
28+
}
Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
// Copyright (c) 2012, Willow Garage, Inc.
2+
// All rights reserved.
3+
//
4+
// Redistribution and use in source and binary forms, with or without
5+
// modification, are permitted provided that the following conditions are met:
6+
//
7+
// * Redistributions of source code must retain the above copyright
8+
// notice, this list of conditions and the following disclaimer.
9+
//
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+
//
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+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28+
// POSSIBILITY OF SUCH DAMAGE.
29+
30+
#ifndef RVIZ_COMMON__PROPERTIES__ROS_TOPIC_MULTI_PROPERTY_HPP_
31+
#define RVIZ_COMMON__PROPERTIES__ROS_TOPIC_MULTI_PROPERTY_HPP_
32+
33+
#include <string>
34+
#include <unordered_set>
35+
36+
#include "rviz_common/properties/ros_topic_property.hpp"
37+
#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
38+
#include "rviz_common/visibility_control.hpp"
39+
40+
namespace rviz_common
41+
{
42+
namespace properties
43+
{
44+
45+
class RVIZ_COMMON_PUBLIC RosTopicMultiProperty : public RosTopicProperty
46+
{
47+
Q_OBJECT
48+
public:
49+
50+
void setMessageTypes(const std::unordered_set<QString> & message_types);
51+
52+
std::unordered_set<QString> getMessageTypes() const
53+
{return message_types_;}
54+
55+
protected Q_SLOTS:
56+
virtual void fillTopicList() override;
57+
58+
private:
59+
// hide the parent class methods which only take a single type
60+
using RosTopicProperty::setMessageType;
61+
using RosTopicProperty::getMessageType;
62+
63+
std::unordered_set<QString> message_types_; // TODO is there a QT-friendly type?
64+
};
65+
66+
} // end namespace properties
67+
} // end namespace rviz_common
68+
69+
#endif // RVIZ_COMMON__PROPERTIES__ROS_TOPIC_MULTI_PROPERTY_HPP_

rviz_common/include/rviz_common/properties/ros_topic_property.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,7 @@ class RVIZ_COMMON_PUBLIC RosTopicProperty : public EditableEnumProperty
7676
protected Q_SLOTS:
7777
virtual void fillTopicList();
7878

79-
private:
79+
protected:
8080
ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node_;
8181
QString message_type_;
8282
};
Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
// Copyright (c) 2012, Willow Garage, Inc.
2+
// All rights reserved.
3+
//
4+
// Redistribution and use in source and binary forms, with or without
5+
// modification, are permitted provided that the following conditions are met:
6+
//
7+
// * Redistributions of source code must retain the above copyright
8+
// notice, this list of conditions and the following disclaimer.
9+
//
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+
//
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+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28+
// POSSIBILITY OF SUCH DAMAGE.
29+
30+
31+
#include <map>
32+
#include <string>
33+
34+
#include <QApplication> // NOLINT: cpplint can't handle Qt imports
35+
36+
#include "rviz_common/properties/ros_topic_multi_property.hpp"
37+
#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
38+
39+
namespace rviz_common
40+
{
41+
namespace properties
42+
{
43+
44+
void RosTopicMultiProperty::setMessageTypes(const std::unordered_set<QString> & message_types)
45+
{
46+
message_types_ = message_types;
47+
}
48+
49+
void RosTopicMultiProperty::fillTopicList()
50+
{
51+
QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
52+
clearOptions();
53+
54+
std::map<std::string, std::vector<std::string>> published_topics =
55+
rviz_ros_node_.lock()->get_topic_names_and_types();
56+
57+
for (const auto & topic : published_topics) {
58+
// Only add topics whose type matches.
59+
for (const auto & type : topic.second) {
60+
if (message_types_.find(QString::fromStdString(type)) != message_types_.end()) {
61+
addOptionStd(topic.first);
62+
}
63+
}
64+
}
65+
sortOptions();
66+
QApplication::restoreOverrideCursor();
67+
}
68+
69+
} // end namespace properties
70+
} // end namespace rviz_common

rviz_default_plugins/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,7 @@ find_package(gz_math_vendor REQUIRED)
6565
find_package(gz-math REQUIRED)
6666

6767
find_package(image_transport REQUIRED)
68+
find_package(image_transport_plugins REQUIRED)
6869
find_package(interactive_markers REQUIRED)
6970
find_package(laser_geometry REQUIRED)
7071
find_package(map_msgs REQUIRED)
@@ -246,6 +247,7 @@ target_include_directories(rviz_default_plugins PUBLIC
246247
target_link_libraries(rviz_default_plugins PUBLIC
247248
${geometry_msgs_TARGETS}
248249
image_transport::image_transport
250+
${image_transport_plugins_TARGETS}
249251
interactive_markers::interactive_markers
250252
laser_geometry::laser_geometry
251253
${map_msgs_TARGETS}
@@ -284,6 +286,7 @@ ament_export_targets(rviz_default_plugins HAS_LIBRARY_TARGET)
284286
ament_export_dependencies(
285287
geometry_msgs
286288
image_transport
289+
image_transport_plugins
287290
interactive_markers
288291
laser_geometry
289292
map_msgs

rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp

Lines changed: 27 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,10 +48,15 @@
4848
# include "rviz_common/properties/bool_property.hpp"
4949
# include "rviz_common/properties/float_property.hpp"
5050
# include "rviz_common/properties/int_property.hpp"
51+
# include "rviz_common/properties/enum_property.hpp"
5152

5253
# include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp"
5354
# include "rviz_default_plugins/visibility_control.hpp"
54-
#include "rviz_default_plugins/displays/image/image_transport_display.hpp"
55+
# include "rviz_default_plugins/displays/image/image_transport_display.hpp"
56+
57+
# include <sensor_msgs/msg/image.hpp>
58+
# include <sensor_msgs/msg/compressed_image.hpp>
59+
//# include <theora_image_transport/msg/packet.hpp>
5560
#endif
5661

5762

@@ -85,17 +90,38 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay : public
8590
void update(float wall_dt, float ros_dt) override;
8691
void reset() override;
8792

93+
QString getTransport();
94+
std::string getTransportStd();
95+
8896
public Q_SLOTS:
8997
virtual void updateNormalizeOptions();
98+
bool setTransport(const QString & str);
99+
bool setTransportStd(const std::string & std_str);
90100

91101
protected:
92102
// overrides from Display
93103
void onEnable() override;
94104
void onDisable() override;
105+
void subscribe() override;
106+
void unsubscribe() override;
107+
108+
void incomingMessage(const sensor_msgs::msg::Image::ConstSharedPtr & img_msg);
95109

96110
/* This is called by incomingMessage(). */
97111
void processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) override;
98112

113+
image_transport::Subscriber subscription_;
114+
rviz_common::properties::EnumProperty* image_transport_property_;
115+
const std::unordered_map<QString, QString> transport_message_types_ =
116+
{
117+
{"raw", QString::fromStdString(rosidl_generator_traits::name<sensor_msgs::msg::Image>())},
118+
{"compressed", QString::fromStdString(rosidl_generator_traits::name<sensor_msgs::msg::CompressedImage>())},
119+
/*{"theora", QString::fromStdString(rosidl_generator_traits::name<theora_image_transport::msg::Packet>())}*/
120+
};
121+
122+
protected Q_SLOTS:
123+
void updateTransport();
124+
99125
private:
100126
void setupScreenRectangle();
101127
void setupRenderPanel();

rviz_default_plugins/package.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
<depend>geometry_msgs</depend>
4141
<depend>gz_math_vendor</depend>
4242
<depend>image_transport</depend>
43+
<depend>image_transport_plugins</depend>
4344
<depend>interactive_markers</depend>
4445
<depend>laser_geometry</depend>
4546
<depend>nav_msgs</depend>

0 commit comments

Comments
 (0)