Skip to content

Only recreate target object when parameters change #86

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

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Only recreate target object when parameters change #86

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
375f43d
only recreate target object when parameters change
May 8, 2021
a3eca7c
better implementation in createTargetInstance
May 8, 2021
74597fa
cleaning up some parameter stuff
May 18, 2021
a6cbf5a
switched to atomic bool
May 18, 2021
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
6 changes: 6 additions & 0 deletions ...ration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_target_widget.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@
#ifndef MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_TARGET_WIDGET_
#define MOVEIT_HANDEYE_CALIBRATION_RVIZ_PLUGIN_HANDEYE_TARGET_WIDGET_

#include <atomic>

// qt
#include <QSet>
#include <QLabel>
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -192,6 +197,7 @@ private Q_SLOTS:
ros::NodeHandle nh_;
std::unique_ptr<pluginlib::ClassLoader<moveit_handeye_calibration::HandEyeTargetBase> > target_plugins_loader_;
pluginlib::UniquePtr<moveit_handeye_calibration::HandEyeTargetBase> target_;
std::atomic_flag target_is_ready_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
Expand Down
29 changes: 24 additions & 5 deletions moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 -----------------------------------------------
Expand Down Expand Up @@ -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<QLineEdit*>(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<QLineEdit*>(target_param_inputs_[param.name_])->setText(std::to_string(param.value_.f).c_str());
break;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This case looks the exact same as above. Can you combine?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I went to do this and realized that there's a one character difference, in lines 313 and 321: one uses param.value_.i and the other param.value_.f. I'm not thinking of a more elegant way to combine them . . .

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You could make it a lambda function that lives inside loadInputWidgetsForTargetType function and have that one parameter be the input value. And make that parameter const std::string& to avoid having to template it for different value types.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't quite follow your suggestion, @jliukkonen, but I did realize that the parameter could know how to make a string of itself, and so that's what I did and used it here to combine the int and float cases. It also led me to some other situations where things could be simplified related to parameter types.

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()));
Expand All @@ -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<QComboBox*>(target_param_inputs_[param.name_])->setCurrentIndex(param.value_.e);
break;
}
}
}
}
Expand All @@ -341,6 +350,9 @@ bool TargetTabWidget::createTargetInstance()
if (!target_)
return false;

if (target_is_ready_.test_and_set())
return true;

try
{
// TODO: load parameters from GUI
Expand Down Expand Up @@ -368,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;
}

Expand Down Expand Up @@ -462,6 +475,7 @@ void TargetTabWidget::targetTypeComboboxChanged(const QString& text)
{
loadInputWidgetsForTargetType(text.toStdString());
}
target_is_ready_.clear();
}

void TargetTabWidget::createTargetImageBtnClicked(bool clicked)
Expand Down Expand Up @@ -549,4 +563,9 @@ void TargetTabWidget::cameraInfoComboBoxChanged(const QString& topic)
}
}

void TargetTabWidget::targetParameterChanged(const QString&)
{
target_is_ready_.clear();
}

} // namespace moveit_rviz_plugin
7 changes: 3 additions & 4 deletions moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,10 +133,9 @@ bool HandEyeArucoTarget::setTargetDimension(double marker_measured_size, double
std::lock_guard<std::mutex> 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;
}
Expand Down
15 changes: 7 additions & 8 deletions moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,10 +141,9 @@ bool HandEyeCharucoTarget::setTargetDimension(double board_size_meters, double m
}

std::lock_guard<std::mutex> 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;
Expand Down Expand Up @@ -185,7 +184,7 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image)
std::lock_guard<std::mutex> base_lock(base_mutex_);
try
{
// Detect aruco board
// Detect ChArUco board
charuco_mutex_.lock();
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(dictionary_id_);
float square_size_meters = board_size_meters_ / std::max(squares_x_, squares_y_);
Expand All @@ -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;
}

Expand All @@ -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;
}

Expand Down