|
| 1 | +// Copyright 2020 Tier IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "initial_pose_button_panel.hpp" |
| 16 | + |
| 17 | +#include <QFileDialog> |
| 18 | +#include <QHBoxLayout> |
| 19 | +#include <QLineEdit> |
| 20 | +#include <QPainter> |
| 21 | +#include <QPushButton> |
| 22 | +#include <pluginlib/class_list_macros.hpp> |
| 23 | +#include <rviz_common/display_context.hpp> |
| 24 | + |
| 25 | +#include <memory> |
| 26 | +#include <string> |
| 27 | +#include <thread> |
| 28 | +#include <vector> |
| 29 | + |
| 30 | +namespace autoware_localization_rviz_plugin |
| 31 | +{ |
| 32 | +InitialPoseButtonPanel::InitialPoseButtonPanel(QWidget * parent) : rviz_common::Panel(parent) |
| 33 | +{ |
| 34 | + topic_label_ = new QLabel("PoseWithCovarianceStamped "); |
| 35 | + topic_label_->setAlignment(Qt::AlignCenter); |
| 36 | + |
| 37 | + topic_edit_ = new QLineEdit("/sensing/gnss/pose_with_covariance"); |
| 38 | + connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic())); |
| 39 | + |
| 40 | + initialize_button_ = new QPushButton("Wait for subscribe topic"); |
| 41 | + initialize_button_->setEnabled(false); |
| 42 | + connect(initialize_button_, SIGNAL(clicked(bool)), SLOT(pushInitializeButton())); |
| 43 | + |
| 44 | + status_label_ = new QLabel("Not Initialize"); |
| 45 | + status_label_->setAlignment(Qt::AlignCenter); |
| 46 | + status_label_->setStyleSheet("QLabel { background-color : gray;}"); |
| 47 | + |
| 48 | + QSizePolicy q_size_policy(QSizePolicy::Expanding, QSizePolicy::Expanding); |
| 49 | + initialize_button_->setSizePolicy(q_size_policy); |
| 50 | + |
| 51 | + auto * topic_layout = new QHBoxLayout; |
| 52 | + topic_layout->addWidget(topic_label_); |
| 53 | + topic_layout->addWidget(topic_edit_); |
| 54 | + |
| 55 | + auto * v_layout = new QVBoxLayout; |
| 56 | + v_layout->addLayout(topic_layout); |
| 57 | + v_layout->addWidget(initialize_button_); |
| 58 | + v_layout->addWidget(status_label_); |
| 59 | + |
| 60 | + setLayout(v_layout); |
| 61 | +} |
| 62 | +void InitialPoseButtonPanel::onInitialize() |
| 63 | +{ |
| 64 | + rclcpp::Node::SharedPtr raw_node = |
| 65 | + this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); |
| 66 | + |
| 67 | + pose_cov_sub_ = raw_node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>( |
| 68 | + topic_edit_->text().toStdString(), 10, |
| 69 | + std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1)); |
| 70 | + |
| 71 | + client_ = raw_node-> create_client<autoware_localization_msgs::srv::PoseWithCovarianceStamped>( |
| 72 | + "/localization/util/initialize_pose"); |
| 73 | +} |
| 74 | + |
| 75 | +void InitialPoseButtonPanel::callbackPoseCov( |
| 76 | + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) |
| 77 | +{ |
| 78 | + pose_cov_msg_ = *msg; |
| 79 | + initialize_button_->setText("Pose Initializer Let's GO!"); |
| 80 | + initialize_button_->setEnabled(true); |
| 81 | +} |
| 82 | + |
| 83 | +void InitialPoseButtonPanel::editTopic() |
| 84 | +{ |
| 85 | + pose_cov_sub_.reset(); |
| 86 | + rclcpp::Node::SharedPtr raw_node = |
| 87 | + this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); |
| 88 | + pose_cov_sub_ = raw_node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>( |
| 89 | + topic_edit_->text().toStdString(), 10, |
| 90 | + std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1)); |
| 91 | + initialize_button_->setText("Wait for subscribe topic"); |
| 92 | + initialize_button_->setEnabled(false); |
| 93 | +} |
| 94 | + |
| 95 | +void InitialPoseButtonPanel::pushInitializeButton() |
| 96 | +{ |
| 97 | + // lock button |
| 98 | + initialize_button_->setEnabled(false); |
| 99 | + |
| 100 | + status_label_->setStyleSheet("QLabel { background-color : dodgerblue;}"); |
| 101 | + status_label_->setText("Initializing..."); |
| 102 | + |
| 103 | + std::thread thread([this] { |
| 104 | + auto req = |
| 105 | + std::make_shared<autoware_localization_msgs::srv::PoseWithCovarianceStamped::Request>(); |
| 106 | + req->pose_with_covariance = pose_cov_msg_; |
| 107 | + |
| 108 | + client_->async_send_request( |
| 109 | + req, |
| 110 | + [this]( |
| 111 | + rclcpp::Client<autoware_localization_msgs::srv::PoseWithCovarianceStamped>::SharedFuture |
| 112 | + result) { |
| 113 | + status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); |
| 114 | + status_label_->setText("OK!!!"); |
| 115 | + |
| 116 | + // unlock button |
| 117 | + initialize_button_->setEnabled(true); |
| 118 | + }); |
| 119 | + }); |
| 120 | + |
| 121 | + thread.detach(); |
| 122 | +} |
| 123 | + |
| 124 | +} // end namespace autoware_localization_rviz_plugin |
| 125 | + |
| 126 | +PLUGINLIB_EXPORT_CLASS( |
| 127 | + autoware_localization_rviz_plugin::InitialPoseButtonPanel, rviz_common::Panel) |
0 commit comments