moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
interactive_marker_display.cpp
Go to the documentation of this file.
1/*
2 * Copyright (c) 2008, Willow Garage, Inc.
3 * Copyright (c) 2019, Open Source Robotics Foundation, Inc.
4 * All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted 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 * 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 OWNER 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#include <map>
31#include <memory>
32#include <string>
33#include <utility>
34#include <vector>
35
36#include <rclcpp/node_options.hpp>
37
38#include <rviz_common/properties/bool_property.hpp>
39#include <rviz_common/validate_floats.hpp>
40#include <rviz_common/display_context.hpp>
41#include <rviz_common/logging.hpp>
42#include <rviz_common/frame_manager_iface.hpp>
43#include <rviz_common/transformation/frame_transformer.hpp>
44
45#include <rviz_default_plugins/displays/interactive_markers/interactive_marker_namespace_property.hpp>
46
48
50{
51namespace displays
52{
53bool validateFloats(const visualization_msgs::msg::InteractiveMarker& msg)
54{
55 bool valid = true;
56 valid = valid && rviz_common::validateFloats(msg.pose);
57 valid = valid && rviz_common::validateFloats(msg.scale);
58 for (const auto& control : msg.controls)
59 {
60 valid = valid && rviz_common::validateFloats(control.orientation);
61 for (const auto& marker : control.markers)
62 {
63 valid = valid && rviz_common::validateFloats(marker.pose);
64 valid = valid && rviz_common::validateFloats(marker.scale);
65 valid = valid && rviz_common::validateFloats(marker.color);
66 valid = valid && rviz_common::validateFloats(marker.points);
67 }
68 }
69 return valid;
70}
71
73{
74 interactive_marker_namespace_property_ =
75 new InteractiveMarkerNamespaceProperty("Interactive Markers Namespace", "",
76 "Namespace of the interactive marker server to connect to.", this,
77 SLOT(namespaceChanged()));
78
79 show_descriptions_property_ =
80 new rviz_common::properties::BoolProperty("Show Descriptions", true,
81 "Whether or not to show the descriptions of each Interactive Marker.",
82 this, SLOT(updateShowDescriptions()));
83
84 show_axes_property_ = new rviz_common::properties::BoolProperty(
85 "Show Axes", false, "Whether or not to show the axes of each Interactive Marker.", this, SLOT(updateShowAxes()));
86
87 show_visual_aids_property_ = new rviz_common::properties::BoolProperty(
88 "Show Visual Aids", false, "Whether or not to show visual helpers while moving/rotating Interactive Markers.",
89 this, SLOT(updateShowVisualAids()));
90
91 enable_transparency_property_ = new rviz_common::properties::BoolProperty(
92 "Enable Transparency", true,
93 "Whether or not to allow transparency for auto-completed markers (e.g. rings and arrows).", this,
95}
96
98{
99 auto ros_node_abstraction = context_->getRosNodeAbstraction().lock();
100 if (!ros_node_abstraction)
101 {
102 return;
103 }
104 interactive_marker_namespace_property_->initialize(ros_node_abstraction);
105
106 rclcpp::NodeOptions options;
107 options.arguments({ "--ros-args", "-r",
108 "__node:=" + std::string("interactive_marker_display_") +
109 std::to_string(reinterpret_cast<std::size_t>(this)) });
110 pnode_ = rclcpp::Node::make_shared("_", "", options);
111 private_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
112 private_executor_->add_node(pnode_);
113 // start executor on a different thread now
114 private_executor_thread_ = std::thread([this]() { private_executor_->spin(); });
115 auto frame_transformer = context_->getFrameManager()->getTransformer();
116 // rclcpp::Node::SharedPtr node = ros_node_abstraction->get_raw_node();
117 interactive_marker_client_ = std::make_unique<interactive_markers::InteractiveMarkerClient>(
118 pnode_, frame_transformer, fixed_frame_.toStdString());
119
120 interactive_marker_client_->setInitializeCallback(
121 [this](const visualization_msgs::srv::GetInteractiveMarkers::Response::SharedPtr& msg) {
122 return initializeCallback(msg);
123 });
124 interactive_marker_client_->setUpdateCallback(
125 [this](const visualization_msgs::msg::InteractiveMarkerUpdate::ConstSharedPtr& msg) {
126 return updateCallback(msg);
127 });
128 interactive_marker_client_->setResetCallback([this]() { return resetCallback(); });
129 interactive_marker_client_->setStatusCallback(
130 [this](interactive_markers::InteractiveMarkerClient::Status status, const std::string& message) {
131 return statusCallback(status, message);
132 });
133
134 subscribe();
135}
136
138{
139 subscribe();
140}
141
143{
144 unsubscribe();
145}
146
148{
149 unsubscribe();
150
151 if (interactive_marker_namespace_property_->isEmpty())
152 {
153 setStatus(rviz_common::properties::StatusProperty::Error, "Interactive Marker Client",
154 QString("Error connecting: empty namespace"));
155 return;
156 }
157
158 subscribe();
159}
160
161void InteractiveMarkerDisplay::subscribe()
162{
163 const std::string topic_namespace = interactive_marker_namespace_property_->getNamespaceStd();
164 if (isEnabled() && !topic_namespace.empty())
165 {
166 interactive_marker_client_->connect(topic_namespace);
167 }
168}
169
170void InteractiveMarkerDisplay::publishFeedback(visualization_msgs::msg::InteractiveMarkerFeedback& feedback)
171{
172 interactive_marker_client_->publishFeedback(feedback);
173}
174
175void InteractiveMarkerDisplay::onStatusUpdate(rviz_common::properties::StatusProperty::Level level,
176 const std::string& name, const std::string& text)
177{
178 setStatusStd(level, name, text);
179}
180
181void InteractiveMarkerDisplay::unsubscribe()
182{
183 if (interactive_marker_client_)
184 {
185 interactive_marker_client_->disconnect();
186 }
187 Display::reset();
188}
189
190// rviz_common 16.0.1 (Rolling) removed the deprecated (float, float) overload
191// in favor of (std::chrono::nanoseconds, std::chrono::nanoseconds); see
192// ros2/rviz#1533. Header declaration in interactive_marker_display.hpp is
193// guarded the same way; remove this branch when Kilted goes EOL.
194// For Rolling, L-turtle, and newer
195#if RCLCPP_VERSION_GTE(30, 0, 0)
196void InteractiveMarkerDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
197// For Kilted and older
198#else
199void InteractiveMarkerDisplay::update(float wall_dt, float ros_dt)
200#endif
201{
202 (void)wall_dt;
203 (void)ros_dt;
204
205 interactive_marker_client_->update();
206
207 for (const auto& name_marker_pair : interactive_markers_map_)
208 {
209 name_marker_pair.second->update();
210 }
211}
212
213void InteractiveMarkerDisplay::updateMarkers(const std::vector<visualization_msgs::msg::InteractiveMarker>& markers)
214{
215 for (const visualization_msgs::msg::InteractiveMarker& marker : markers)
216 {
217 if (!validateFloats(marker))
218 {
219 setStatusStd(rviz_common::properties::StatusProperty::Error, marker.name, "Marker contains invalid floats!");
220 continue;
221 }
222 RVIZ_COMMON_LOG_DEBUG_STREAM("Processing interactive marker '" << marker.name << "'. " << marker.controls.size());
223
224 auto int_marker_entry = interactive_markers_map_.find(marker.name);
225
226 if (int_marker_entry == interactive_markers_map_.end())
227 {
228 int_marker_entry =
229 interactive_markers_map_
230 .insert(std::make_pair(marker.name, std::make_shared<InteractiveMarker>(getSceneNode(), context_)))
231 .first;
232 connect(int_marker_entry->second.get(), SIGNAL(userFeedback(visualization_msgs::msg::InteractiveMarkerFeedback&)),
233 this, SLOT(publishFeedback(visualization_msgs::msg::InteractiveMarkerFeedback&)));
234 connect(
235 int_marker_entry->second.get(),
236 SIGNAL(statusUpdate(rviz_common::properties::StatusProperty::Level, const std::string&, const std::string&)),
237 this,
238 SLOT(onStatusUpdate(rviz_common::properties::StatusProperty::Level, const std::string&, const std::string&)));
239 }
240
241 if (int_marker_entry->second->processMessage(marker))
242 {
243 int_marker_entry->second->setShowAxes(show_axes_property_->getBool());
244 int_marker_entry->second->setShowVisualAids(show_visual_aids_property_->getBool());
245 int_marker_entry->second->setShowDescription(show_descriptions_property_->getBool());
246 }
247 else
248 {
249 unsubscribe();
250 return;
251 }
252 }
253}
254
255void InteractiveMarkerDisplay::eraseAllMarkers()
256{
257 interactive_markers_map_.clear();
258 deleteStatusStd("Interactive Marker Client");
259}
260
261void InteractiveMarkerDisplay::eraseMarkers(const std::vector<std::string>& erases)
262{
263 for (const std::string& marker_name : erases)
264 {
265 interactive_markers_map_.erase(marker_name);
266 deleteStatusStd(marker_name);
267 }
268}
269
270void InteractiveMarkerDisplay::updatePoses(
271 const std::vector<visualization_msgs::msg::InteractiveMarkerPose>& marker_poses)
272{
273 for (const visualization_msgs::msg::InteractiveMarkerPose& marker_pose : marker_poses)
274 {
275 if (!rviz_common::validateFloats(marker_pose.pose))
276 {
277 setStatusStd(rviz_common::properties::StatusProperty::Error, marker_pose.name,
278 "Pose message contains invalid floats!");
279 return;
280 }
281
282 auto int_marker_entry = interactive_markers_map_.find(marker_pose.name);
283
284 if (int_marker_entry != interactive_markers_map_.end())
285 {
286 int_marker_entry->second->processMessage(marker_pose);
287 }
288 else
289 {
290 setStatusStd(rviz_common::properties::StatusProperty::Error, marker_pose.name,
291 "Pose received for non-existing marker '" + marker_pose.name);
292 unsubscribe();
293 return;
294 }
295 }
296}
297
298void InteractiveMarkerDisplay::initializeCallback(
299 const visualization_msgs::srv::GetInteractiveMarkers::Response::SharedPtr& msg)
300{
301 eraseAllMarkers();
302 updateMarkers(msg->markers);
303}
304
305void InteractiveMarkerDisplay::updateCallback(const visualization_msgs::msg::InteractiveMarkerUpdate::ConstSharedPtr& msg)
306{
307 updateMarkers(msg->markers);
308 updatePoses(msg->poses);
309 eraseMarkers(msg->erases);
310}
311
312void InteractiveMarkerDisplay::resetCallback()
313{
314 eraseAllMarkers();
315 deleteStatusStd("Interactive Marker Client");
316}
317
318void InteractiveMarkerDisplay::statusCallback(interactive_markers::InteractiveMarkerClient::Status status,
319 const std::string& message)
320{
321 rviz_common::properties::StatusProperty::Level rviz_level;
322 switch (status)
323 {
324 case interactive_markers::InteractiveMarkerClient::STATUS_DEBUG:
325 rviz_level = rviz_common::properties::StatusProperty::Ok;
326 break;
327 case interactive_markers::InteractiveMarkerClient::STATUS_INFO:
328 rviz_level = rviz_common::properties::StatusProperty::Ok;
329 break;
330 case interactive_markers::InteractiveMarkerClient::STATUS_WARN:
331 rviz_level = rviz_common::properties::StatusProperty::Warn;
332 break;
333 case interactive_markers::InteractiveMarkerClient::STATUS_ERROR:
334 rviz_level = rviz_common::properties::StatusProperty::Error;
335 break;
336 default:
337 RVIZ_COMMON_LOG_WARNING("Unexpected status level from interactive marker client received");
338 rviz_level = rviz_common::properties::StatusProperty::Error;
339 }
340 setStatusStd(rviz_level, "Interactive Marker Client", message);
341}
342
344{
345 if (interactive_marker_client_)
346 {
347 interactive_marker_client_->setTargetFrame(fixed_frame_.toStdString());
348 }
349 Display::reset();
350}
351
353{
354 Display::reset();
355 unsubscribe();
356 subscribe();
357}
358
360{
361 bool show = show_descriptions_property_->getBool();
362
363 for (const auto& name_marker_pair : interactive_markers_map_)
364 {
365 name_marker_pair.second->setShowDescription(show);
366 }
367}
368
370{
371 bool show = show_axes_property_->getBool();
372
373 for (const auto& name_marker_pair : interactive_markers_map_)
374 {
375 name_marker_pair.second->setShowAxes(show);
376 }
377}
378
380{
381 bool show = show_visual_aids_property_->getBool();
382
383 for (const auto& name_marker_pair : interactive_markers_map_)
384 {
385 name_marker_pair.second->setShowVisualAids(show);
386 }
387}
388
390{
391 // This is not very efficient, but works
392 unsubscribe();
393 interactive_marker_client_->setEnableAutocompleteTransparency(enable_transparency_property_->getBool());
394 subscribe();
395}
396
397} // namespace displays
398} // namespace rviz_default_plugins
399
400#include <pluginlib/class_list_macros.hpp> // NOLINT(build/include_order)
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
void publishFeedback(visualization_msgs::msg::InteractiveMarkerFeedback &feedback)
void onStatusUpdate(rviz_common::properties::StatusProperty::Level level, const std::string &name, const std::string &text)
bool validateFloats(const visualization_msgs::msg::InteractiveMarker &msg)