From 375f43d03da74a5ac6b993adec2e02accdb29605 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Fri, 7 May 2021 21:25:17 -0600 Subject: [PATCH 1/4] only recreate target object when parameters change --- .../handeye_target_widget.h | 6 +++++ .../src/handeye_target_widget.cpp | 27 ++++++++++++++----- .../src/handeye_target_aruco.cpp | 7 +++-- .../src/handeye_target_charuco.cpp | 15 +++++------ 4 files changed, 37 insertions(+), 18 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h index 84468d3..40d1fc6 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h @@ -37,6 +37,8 @@ #ifndef MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_TARGET_WIDGET_ #define MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_TARGET_WIDGET_ +#include + // qt #include #include @@ -149,6 +151,9 @@ private Q_SLOTS: // Called when the item of camera_info_topic_field_ combobox is selected void cameraInfoComboBoxChanged(const QString& topic); + // Called when any change is made to the target parameters + void targetParameterChanged(const QString&); + Q_SIGNALS: void cameraInfoChanged(sensor_msgs::CameraInfo msg); @@ -192,6 +197,7 @@ private Q_SLOTS: ros::NodeHandle nh_; std::unique_ptr > target_plugins_loader_; pluginlib::UniquePtr target_; + std::atomic_flag target_is_ready_; image_transport::ImageTransport it_; image_transport::Subscriber image_sub_; image_transport::Publisher image_pub_; diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index 5da1342..128b478 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -90,6 +90,7 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) , it_(nh_) , target_plugins_loader_(nullptr) , target_(nullptr) + , target_is_ready_(ATOMIC_FLAG_INIT) , target_param_layout_(new QFormLayout()) { // Target setting tab area ----------------------------------------------- @@ -304,18 +305,25 @@ bool TargetTabWidget::loadInputWidgetsForTargetType(const std::string& plugin_na { switch (param.parameter_type_) { - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: - target_param_inputs_.insert(std::make_pair(param.name_, new QLineEdit())); + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: { + QLineEdit* line_edit = new QLineEdit(); + connect(line_edit, SIGNAL(textChanged(QString)), this, SLOT(targetParameterChanged(QString))); + target_param_inputs_.insert(std::make_pair(param.name_, line_edit)); target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param.value_.i).c_str()); break; - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: - target_param_inputs_.insert(std::make_pair(param.name_, new QLineEdit())); + } + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: { + QLineEdit* line_edit = new QLineEdit(); + connect(line_edit, SIGNAL(textChanged(QString)), this, SLOT(targetParameterChanged(QString))); + target_param_inputs_.insert(std::make_pair(param.name_, line_edit)); target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param.value_.f).c_str()); break; - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: + } + case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: { QComboBox* combo_box = new QComboBox(); + connect(combo_box, SIGNAL(currentTextChanged(QString)), this, SLOT(targetParameterChanged(QString))); for (const std::string& value : param.enum_values_) { combo_box->addItem(tr(value.c_str())); @@ -324,6 +332,7 @@ bool TargetTabWidget::loadInputWidgetsForTargetType(const std::string& plugin_na target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); static_cast(target_param_inputs_[param.name_])->setCurrentIndex(param.value_.e); break; + } } } } @@ -338,7 +347,7 @@ bool TargetTabWidget::loadInputWidgetsForTargetType(const std::string& plugin_na bool TargetTabWidget::createTargetInstance() { - if (!target_) + if (!target_ || target_is_ready_.test_and_set()) return false; try @@ -462,6 +471,7 @@ void TargetTabWidget::targetTypeComboboxChanged(const QString& text) { loadInputWidgetsForTargetType(text.toStdString()); } + target_is_ready_.clear(); } void TargetTabWidget::createTargetImageBtnClicked(bool clicked) @@ -549,4 +559,9 @@ void TargetTabWidget::cameraInfoComboBoxChanged(const QString& topic) } } +void TargetTabWidget::targetParameterChanged(const QString&) +{ + target_is_ready_.clear(); +} + } // namespace moveit_rviz_plugin diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp index c514bcd..7fce234 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp @@ -133,10 +133,9 @@ bool HandEyeArucoTarget::setTargetDimension(double marker_measured_size, double std::lock_guard aruco_lock(aruco_mutex_); marker_size_real_ = marker_measured_size; marker_separation_real_ = marker_measured_separation; - ROS_INFO_STREAM_THROTTLE_NAMED(2., LOGNAME, - "Set target real dimensions: \n" - << "marker_measured_size " << std::to_string(marker_measured_size) << "\n" - << "marker_measured_separation " << std::to_string(marker_measured_separation) + ROS_INFO_STREAM_NAMED(LOGNAME, "Set target real dimensions: \n" + << " marker_measured_size " << std::to_string(marker_measured_size) << "\n" + << " marker_measured_separation " << std::to_string(marker_measured_separation) << "\n"); return true; } diff --git a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp index 5d44c64..1cfff9a 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp +++ b/moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp @@ -141,10 +141,9 @@ bool HandEyeCharucoTarget::setTargetDimension(double board_size_meters, double m } std::lock_guard charuco_lock(charuco_mutex_); - ROS_INFO_STREAM_THROTTLE_NAMED(2., LOGNAME, - "Set target real dimensions: \n" - << "board_size_meters " << std::to_string(board_size_meters) << "\n" - << "marker_size_meters " << std::to_string(marker_size_meters) << "\n" + ROS_INFO_STREAM_NAMED(LOGNAME, "Set target real dimensions: \n" + << " board_size_meters " << std::to_string(board_size_meters) << "\n" + << " marker_size_meters " << std::to_string(marker_size_meters) << "\n" << "\n"); board_size_meters_ = board_size_meters; marker_size_meters_ = marker_size_meters; @@ -185,7 +184,7 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image) std::lock_guard base_lock(base_mutex_); try { - // Detect aruco board + // Detect ChArUco board charuco_mutex_.lock(); cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(dictionary_id_); float square_size_meters = board_size_meters_ / std::max(squares_x_, squares_y_); @@ -204,7 +203,7 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image) cv::aruco::detectMarkers(image, dictionary, marker_corners, marker_ids, params_ptr); if (marker_ids.empty()) { - ROS_DEBUG_STREAM_THROTTLE_NAMED(1., LOGNAME, "No aruco marker detected. Dictionary ID: " << dictionary_id_); + ROS_DEBUG_STREAM_THROTTLE_NAMED(1., LOGNAME, "No ArUco marker detected. Dictionary ID: " << dictionary_id_); return false; } @@ -214,14 +213,14 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image) cv::aruco::interpolateCornersCharuco(marker_corners, marker_ids, image, board, charuco_corners, charuco_ids, camera_matrix_, distortion_coeffs_); - // Estimate aruco board pose + // Estimate ChArUco board pose bool valid = cv::aruco::estimatePoseCharucoBoard(charuco_corners, charuco_ids, board, camera_matrix_, distortion_coeffs_, rotation_vect_, translation_vect_); // Draw the markers and frame axis if at least one marker is detected if (!valid) { - ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Cannot estimate aruco board pose."); + ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Cannot estimate ChArUco board pose."); return false; } From a3eca7c0bf003cd81c195815d3de730e77a62249 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Fri, 7 May 2021 21:32:27 -0600 Subject: [PATCH 2/4] better implementation in createTargetInstance --- .../src/handeye_target_widget.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index 128b478..da7a181 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -347,9 +347,12 @@ bool TargetTabWidget::loadInputWidgetsForTargetType(const std::string& plugin_na bool TargetTabWidget::createTargetInstance() { - if (!target_ || target_is_ready_.test_and_set()) + if (!target_) return false; + if (target_is_ready_.test_and_set()) + return true; + try { // TODO: load parameters from GUI @@ -377,6 +380,7 @@ bool TargetTabWidget::createTargetInstance() { QMessageBox::warning(this, tr("Exception while loading a handeye target plugin"), tr(ex.what())); target_ = nullptr; + target_is_ready_.clear(); return false; } From 74597faca263ec5ccda9325088fb2dd256e2ea92 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Tue, 18 May 2021 13:50:45 -0600 Subject: [PATCH 3/4] cleaning up some parameter stuff --- .../src/handeye_target_widget.cpp | 49 +++++++------------ .../handeye_target_base.h | 32 ++++++++++++ 2 files changed, 49 insertions(+), 32 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index da7a181..636435f 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -40,6 +40,8 @@ namespace moveit_rviz_plugin { const std::string LOGNAME = "handeye_target_widget"; +using MoveItCalParam = moveit_handeye_calibration::HandEyeTargetBase::Parameter; + void RosTopicComboBox::addMsgsFilterType(QString msgs_type) { message_types_.insert(msgs_type); @@ -175,19 +177,19 @@ void TargetTabWidget::loadWidget(const rviz::Config& config) int param_int; float param_float; QString param_enum; - for (const moveit_handeye_calibration::HandEyeTargetBase::Parameter& param : target_plugin_params_) + for (const MoveItCalParam& param : target_plugin_params_) { switch (param.parameter_type_) { - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: + case MoveItCalParam::ParameterType::Int: if (config.mapGetInt(param.name_.c_str(), ¶m_int)) static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param_int).c_str()); break; - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: + case MoveItCalParam::ParameterType::Float: if (config.mapGetFloat(param.name_.c_str(), ¶m_float)) static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param_float).c_str()); break; - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: + case MoveItCalParam::ParameterType::Enum: if (config.mapGetString(param.name_.c_str(), ¶m_enum)) { int index = static_cast(target_param_inputs_[param.name_])->findText(param_enum); @@ -233,16 +235,16 @@ void TargetTabWidget::saveWidget(rviz::Config& config) config.mapSetValue("target_type", target_type_->currentText()); QString param_value; - for (const moveit_handeye_calibration::HandEyeTargetBase::Parameter& param : target_plugin_params_) + for (const MoveItCalParam& param : target_plugin_params_) { switch (param.parameter_type_) { - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: + case MoveItCalParam::ParameterType::Int: + case MoveItCalParam::ParameterType::Float: param_value = static_cast(target_param_inputs_[param.name_])->text(); config.mapSetValue(param.name_.c_str(), param_value); break; - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: + case MoveItCalParam::ParameterType::Enum: param_value = static_cast(target_param_inputs_[param.name_])->currentText(); config.mapSetValue(param.name_.c_str(), param_value); break; @@ -305,23 +307,16 @@ bool TargetTabWidget::loadInputWidgetsForTargetType(const std::string& plugin_na { switch (param.parameter_type_) { - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: { + case MoveItCalParam::ParameterType::Int: + case MoveItCalParam::ParameterType::Float: { QLineEdit* line_edit = new QLineEdit(); connect(line_edit, SIGNAL(textChanged(QString)), this, SLOT(targetParameterChanged(QString))); target_param_inputs_.insert(std::make_pair(param.name_, line_edit)); target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); - static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param.value_.i).c_str()); + static_cast(target_param_inputs_[param.name_])->setText(param.toString().c_str()); break; } - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: { - QLineEdit* line_edit = new QLineEdit(); - connect(line_edit, SIGNAL(textChanged(QString)), this, SLOT(targetParameterChanged(QString))); - target_param_inputs_.insert(std::make_pair(param.name_, line_edit)); - target_param_layout_->addRow(param.name_.c_str(), target_param_inputs_[param.name_]); - static_cast(target_param_inputs_[param.name_])->setText(std::to_string(param.value_.f).c_str()); - break; - } - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: { + case MoveItCalParam::ParameterType::Enum: { QComboBox* combo_box = new QComboBox(); connect(combo_box, SIGNAL(currentTextChanged(QString)), this, SLOT(targetParameterChanged(QString))); for (const std::string& value : param.enum_values_) @@ -358,20 +353,10 @@ bool TargetTabWidget::createTargetInstance() // TODO: load parameters from GUI for (const auto& param : target_plugin_params_) { - switch (param.parameter_type_) + if (!target_->setParameter(param)) { - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Int: - target_->setParameter(param.name_, - static_cast(target_param_inputs_[param.name_])->text().toInt()); - break; - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float: - target_->setParameter(param.name_, - static_cast(target_param_inputs_[param.name_])->text().toFloat()); - break; - case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum: - target_->setParameter( - param.name_, static_cast(target_param_inputs_[param.name_])->currentText().toStdString()); - break; + target_is_ready_.clear(); + return false; } } target_->initialize(); diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h index a613bda..b92860e 100644 --- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h +++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h @@ -107,6 +107,21 @@ class HandEyeTargetBase else ROS_ERROR("Invalid default option for enum parameter %s", name.c_str()); } + + std::string toString() const + { + switch (parameter_type_) + { + case ParameterType::Int: + return std::to_string(value_.i); + case ParameterType::Float: + return std::to_string(value_.f); + case ParameterType::Enum: + return enum_values_[value_.e]; + default: + return std::string(); + } + } }; const std::string LOGNAME = "handeye_target_base"; @@ -262,6 +277,23 @@ class HandEyeTargetBase return parameters_; } + /** + * @brief Set target parameter using param type + * @return True if successful setting parameter + */ + virtual bool setParameter(Parameter param) + { + for (auto& this_param : parameters_) + { + if (param.name_ == this_param.name_ && param.parameter_type_ == this_param.parameter_type_) + { + this_param.value_ = param.value_; + return true; + } + } + return false; + } + /** * @brief Set target parameter to integer value * @return True if successful setting parameter From a6cbf5ab0125c061271f11b15775b300e6325241 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Tue, 18 May 2021 14:26:05 -0600 Subject: [PATCH 4/4] switched to atomic bool --- .../handeye_target_widget.h | 2 +- .../src/handeye_target_widget.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h index 40d1fc6..375a29f 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h @@ -197,7 +197,7 @@ private Q_SLOTS: ros::NodeHandle nh_; std::unique_ptr > target_plugins_loader_; pluginlib::UniquePtr target_; - std::atomic_flag target_is_ready_; + std::atomic target_is_ready_; image_transport::ImageTransport it_; image_transport::Subscriber image_sub_; image_transport::Publisher image_pub_; diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp index 636435f..50c5a73 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp @@ -92,7 +92,7 @@ TargetTabWidget::TargetTabWidget(QWidget* parent) , it_(nh_) , target_plugins_loader_(nullptr) , target_(nullptr) - , target_is_ready_(ATOMIC_FLAG_INIT) + , target_is_ready_(false) , target_param_layout_(new QFormLayout()) { // Target setting tab area ----------------------------------------------- @@ -345,7 +345,7 @@ bool TargetTabWidget::createTargetInstance() if (!target_) return false; - if (target_is_ready_.test_and_set()) + if (target_is_ready_.exchange(true)) return true; try @@ -355,7 +355,7 @@ bool TargetTabWidget::createTargetInstance() { if (!target_->setParameter(param)) { - target_is_ready_.clear(); + target_is_ready_.store(false); return false; } } @@ -365,7 +365,7 @@ bool TargetTabWidget::createTargetInstance() { QMessageBox::warning(this, tr("Exception while loading a handeye target plugin"), tr(ex.what())); target_ = nullptr; - target_is_ready_.clear(); + target_is_ready_.store(false); return false; } @@ -460,7 +460,7 @@ void TargetTabWidget::targetTypeComboboxChanged(const QString& text) { loadInputWidgetsForTargetType(text.toStdString()); } - target_is_ready_.clear(); + target_is_ready_.store(false); } void TargetTabWidget::createTargetImageBtnClicked(bool clicked) @@ -550,7 +550,7 @@ void TargetTabWidget::cameraInfoComboBoxChanged(const QString& topic) void TargetTabWidget::targetParameterChanged(const QString&) { - target_is_ready_.clear(); + target_is_ready_.store(false); } } // namespace moveit_rviz_plugin