36#include <rclcpp/node_options.hpp>
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>
45#include <rviz_default_plugins/displays/interactive_markers/interactive_marker_namespace_property.hpp>
56 valid = valid && rviz_common::validateFloats(msg.pose);
57 valid = valid && rviz_common::validateFloats(msg.scale);
58 for (
const auto&
control : msg.controls)
60 valid = valid && rviz_common::validateFloats(
control.orientation);
61 for (
const auto& marker :
control.markers)
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);
74 interactive_marker_namespace_property_ =
75 new InteractiveMarkerNamespaceProperty(
"Interactive Markers Namespace",
"",
76 "Namespace of the interactive marker server to connect to.",
this,
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.",
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()));
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.",
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,
99 auto ros_node_abstraction = context_->getRosNodeAbstraction().lock();
100 if (!ros_node_abstraction)
104 interactive_marker_namespace_property_->initialize(ros_node_abstraction);
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_);
114 private_executor_thread_ = std::thread([
this]() { private_executor_->spin(); });
115 auto frame_transformer = context_->getFrameManager()->getTransformer();
117 interactive_marker_client_ = std::make_unique<interactive_markers::InteractiveMarkerClient>(
118 pnode_, frame_transformer, fixed_frame_.toStdString());
120 interactive_marker_client_->setInitializeCallback(
121 [
this](
const visualization_msgs::srv::GetInteractiveMarkers::Response::SharedPtr& msg) {
122 return initializeCallback(msg);
124 interactive_marker_client_->setUpdateCallback(
125 [
this](
const visualization_msgs::msg::InteractiveMarkerUpdate::ConstSharedPtr& msg) {
126 return updateCallback(msg);
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);
151 if (interactive_marker_namespace_property_->isEmpty())
153 setStatus(rviz_common::properties::StatusProperty::Error,
"Interactive Marker Client",
154 QString(
"Error connecting: empty namespace"));
161void InteractiveMarkerDisplay::subscribe()
163 const std::string topic_namespace = interactive_marker_namespace_property_->getNamespaceStd();
164 if (isEnabled() && !topic_namespace.empty())
166 interactive_marker_client_->connect(topic_namespace);
172 interactive_marker_client_->publishFeedback(feedback);
176 const std::string& name,
const std::string& text)
178 setStatusStd(level, name, text);
181void InteractiveMarkerDisplay::unsubscribe()
183 if (interactive_marker_client_)
185 interactive_marker_client_->disconnect();
195#if RCLCPP_VERSION_GTE(30, 0, 0)
205 interactive_marker_client_->update();
207 for (
const auto& name_marker_pair : interactive_markers_map_)
209 name_marker_pair.second->update();
213void InteractiveMarkerDisplay::updateMarkers(
const std::vector<visualization_msgs::msg::InteractiveMarker>& markers)
215 for (
const visualization_msgs::msg::InteractiveMarker& marker : markers)
219 setStatusStd(rviz_common::properties::StatusProperty::Error, marker.name,
"Marker contains invalid floats!");
222 RVIZ_COMMON_LOG_DEBUG_STREAM(
"Processing interactive marker '" << marker.name <<
"'. " << marker.controls.size());
224 auto int_marker_entry = interactive_markers_map_.find(marker.name);
226 if (int_marker_entry == interactive_markers_map_.end())
229 interactive_markers_map_
230 .insert(std::make_pair(marker.name, std::make_shared<InteractiveMarker>(getSceneNode(), context_)))
232 connect(int_marker_entry->second.get(), SIGNAL(userFeedback(visualization_msgs::msg::InteractiveMarkerFeedback&)),
233 this, SLOT(
publishFeedback(visualization_msgs::msg::InteractiveMarkerFeedback&)));
235 int_marker_entry->second.get(),
236 SIGNAL(statusUpdate(rviz_common::properties::StatusProperty::Level,
const std::string&,
const std::string&)),
238 SLOT(
onStatusUpdate(rviz_common::properties::StatusProperty::Level,
const std::string&,
const std::string&)));
241 if (int_marker_entry->second->processMessage(marker))
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());
255void InteractiveMarkerDisplay::eraseAllMarkers()
257 interactive_markers_map_.clear();
258 deleteStatusStd(
"Interactive Marker Client");
261void InteractiveMarkerDisplay::eraseMarkers(
const std::vector<std::string>& erases)
263 for (
const std::string& marker_name : erases)
265 interactive_markers_map_.erase(marker_name);
266 deleteStatusStd(marker_name);
270void InteractiveMarkerDisplay::updatePoses(
271 const std::vector<visualization_msgs::msg::InteractiveMarkerPose>& marker_poses)
273 for (
const visualization_msgs::msg::InteractiveMarkerPose& marker_pose : marker_poses)
275 if (!rviz_common::validateFloats(marker_pose.pose))
277 setStatusStd(rviz_common::properties::StatusProperty::Error, marker_pose.name,
278 "Pose message contains invalid floats!");
282 auto int_marker_entry = interactive_markers_map_.find(marker_pose.name);
284 if (int_marker_entry != interactive_markers_map_.end())
286 int_marker_entry->second->processMessage(marker_pose);
290 setStatusStd(rviz_common::properties::StatusProperty::Error, marker_pose.name,
291 "Pose received for non-existing marker '" + marker_pose.name);
298void InteractiveMarkerDisplay::initializeCallback(
299 const visualization_msgs::srv::GetInteractiveMarkers::Response::SharedPtr& msg)
302 updateMarkers(msg->markers);
305void InteractiveMarkerDisplay::updateCallback(
const visualization_msgs::msg::InteractiveMarkerUpdate::ConstSharedPtr& msg)
307 updateMarkers(msg->markers);
308 updatePoses(msg->poses);
309 eraseMarkers(msg->erases);
312void InteractiveMarkerDisplay::resetCallback()
315 deleteStatusStd(
"Interactive Marker Client");
318void InteractiveMarkerDisplay::statusCallback(interactive_markers::InteractiveMarkerClient::Status status,
319 const std::string& message)
321 rviz_common::properties::StatusProperty::Level rviz_level;
324 case interactive_markers::InteractiveMarkerClient::STATUS_DEBUG:
325 rviz_level = rviz_common::properties::StatusProperty::Ok;
327 case interactive_markers::InteractiveMarkerClient::STATUS_INFO:
328 rviz_level = rviz_common::properties::StatusProperty::Ok;
330 case interactive_markers::InteractiveMarkerClient::STATUS_WARN:
331 rviz_level = rviz_common::properties::StatusProperty::Warn;
333 case interactive_markers::InteractiveMarkerClient::STATUS_ERROR:
334 rviz_level = rviz_common::properties::StatusProperty::Error;
337 RVIZ_COMMON_LOG_WARNING(
"Unexpected status level from interactive marker client received");
338 rviz_level = rviz_common::properties::StatusProperty::Error;
340 setStatusStd(rviz_level,
"Interactive Marker Client", message);
345 if (interactive_marker_client_)
347 interactive_marker_client_->setTargetFrame(fixed_frame_.toStdString());
361 bool show = show_descriptions_property_->getBool();
363 for (
const auto& name_marker_pair : interactive_markers_map_)
365 name_marker_pair.second->setShowDescription(show);
371 bool show = show_axes_property_->getBool();
373 for (
const auto& name_marker_pair : interactive_markers_map_)
375 name_marker_pair.second->setShowAxes(show);
381 bool show = show_visual_aids_property_->getBool();
383 for (
const auto& name_marker_pair : interactive_markers_map_)
385 name_marker_pair.second->setShowVisualAids(show);
393 interactive_marker_client_->setEnableAutocompleteTransparency(enable_transparency_property_->getBool());
400#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
Displays Interactive Markers.
InteractiveMarkerDisplay()
void publishFeedback(visualization_msgs::msg::InteractiveMarkerFeedback &feedback)
void onDisable() override
void updateEnableTransparency()
void update(float wall_dt, float ros_dt) override
void fixedFrameChanged() override
void updateShowDescriptions()
void onStatusUpdate(rviz_common::properties::StatusProperty::Level level, const std::string &name, const std::string &text)
void updateShowVisualAids()
void onInitialize() override
bool validateFloats(const visualization_msgs::msg::InteractiveMarker &msg)