Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Select QoS reliability policy in DepthCloud Plugin #1159

Merged
merged 2 commits into from
Mar 6, 2024
Merged

Select QoS reliability policy in DepthCloud Plugin #1159

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
eaefcae
Select QoS reliability policy in DepthCloud Plugin
ahcorde Mar 1, 2024
9f2b09f
removed unrelated comment
ahcorde Mar 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions ...default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,8 @@ protected Q_SLOTS:

void transformerChangedCallback();

void updateQosProfile();

// Property callbacks
virtual void updateTopic();
virtual void updateTopicFilter();
Expand Down Expand Up @@ -160,6 +162,8 @@ protected Q_SLOTS:
rclcpp::Time subscription_start_time_;

// RVIZ properties
rviz_common::properties::EditableEnumProperty * reliability_policy_property_;
rmw_qos_profile_t qos_profile_;
rviz_common::properties::Property * topic_filter_property_;
rviz_common::properties::IntProperty * queue_size_property_;
rviz_common::properties::BoolProperty * use_auto_size_property_;
Expand Down
38 changes: 36 additions & 2 deletions rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,18 @@ DepthCloudDisplay::DepthCloudDisplay()
QRegExp depth_filter("depth");
depth_filter.setCaseSensitivity(Qt::CaseInsensitive);

reliability_policy_property_ = new rviz_common::properties::EditableEnumProperty(
"Reliability Policy",
"Best effort",
"Set the reliability policy: When choosing 'Best effort', no guarantee will be given that the "
"messages will be delivered, choosing 'Reliable', messages will be sent until received.", this,
SLOT(updateQosProfile()));
reliability_policy_property_->addOption("System Default");
reliability_policy_property_->addOption("Reliable");
reliability_policy_property_->addOption("Best effort");

qos_profile_ = rmw_qos_profile_sensor_data;

topic_filter_property_ =
new rviz_common::properties::Property(
"Topic Filter", true,
Expand Down Expand Up @@ -162,6 +174,26 @@ DepthCloudDisplay::DepthCloudDisplay()
use_occlusion_compensation_property_, SLOT(updateOcclusionTimeOut()), this);
}

void DepthCloudDisplay::updateQosProfile()
{
updateQueueSize();
qos_profile_ = rmw_qos_profile_default;
qos_profile_.depth = queue_size_;

auto policy = reliability_policy_property_->getString().toStdString();

if (policy == "Best effort") {
qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;

} else if (policy == "Reliable") {
qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
} else {
qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
}

updateTopic();
}

void DepthCloudDisplay::transformerChangedCallback()
{
updateTopic();
Expand Down Expand Up @@ -225,6 +257,7 @@ void DepthCloudDisplay::updateQueueSize()
depthmap_tf_filter_->setQueueSize(static_cast<uint32_t>(queue_size_property_->getInt()));
}
queue_size_ = queue_size_property_->getInt();
qos_profile_.depth = queue_size_;
}

void DepthCloudDisplay::updateUseAutoSize()
Expand Down Expand Up @@ -311,7 +344,8 @@ void DepthCloudDisplay::subscribe()
depthmap_sub_->subscribe(
rviz_ros_node_->get_raw_node().get(),
depthmap_topic,
depthmap_transport);
depthmap_transport,
qos_profile_);

depthmap_tf_filter_ =
std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::Image,
Expand Down Expand Up @@ -353,7 +387,7 @@ void DepthCloudDisplay::subscribe()
// subscribe to color image topic
rgb_sub_->subscribe(
rviz_ros_node_->get_raw_node().get(),
color_topic, color_transport, rclcpp::SensorDataQoS().get_rmw_qos_profile());
color_topic, color_transport, qos_profile_);

// connect message filters to synchronizer
sync_depth_color_->connectInput(*depthmap_tf_filter_, *rgb_sub_);
Expand Down