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

Only recreate target object when parameters change #86

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 all 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<bool> target_is_ready_;
image_transport::ImageTransport it_;
image_transport::Subscriber image_sub_;
image_transport::Publisher image_pub_;
Expand Down
64 changes: 34 additions & 30 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 @@ -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);
Expand Down Expand Up @@ -90,6 +92,7 @@ TargetTabWidget::TargetTabWidget(QWidget* parent)
, it_(nh_)
, target_plugins_loader_(nullptr)
, target_(nullptr)
, target_is_ready_(false)
, target_param_layout_(new QFormLayout())
{
// Target setting tab area -----------------------------------------------
Expand Down Expand Up @@ -174,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(), &param_int))
static_cast<QLineEdit*>(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(), &param_float))
static_cast<QLineEdit*>(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(), &param_enum))
{
int index = static_cast<QComboBox*>(target_param_inputs_[param.name_])->findText(param_enum);
Expand Down Expand Up @@ -232,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<QLineEdit*>(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<QComboBox*>(target_param_inputs_[param.name_])->currentText();
config.mapSetValue(param.name_.c_str(), param_value);
break;
Expand Down Expand Up @@ -304,18 +307,18 @@ 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()));
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 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<QLineEdit*>(target_param_inputs_[param.name_])->setText(std::to_string(param.value_.f).c_str());
static_cast<QLineEdit*>(target_param_inputs_[param.name_])->setText(param.toString().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_)
{
combo_box->addItem(tr(value.c_str()));
Expand All @@ -324,6 +327,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,25 +345,18 @@ bool TargetTabWidget::createTargetInstance()
if (!target_)
return false;

if (target_is_ready_.exchange(true))
return true;

try
{
// 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<QLineEdit*>(target_param_inputs_[param.name_])->text().toInt());
break;
case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Float:
target_->setParameter(param.name_,
static_cast<QLineEdit*>(target_param_inputs_[param.name_])->text().toFloat());
break;
case moveit_handeye_calibration::HandEyeTargetBase::Parameter::ParameterType::Enum:
target_->setParameter(
param.name_, static_cast<QComboBox*>(target_param_inputs_[param.name_])->currentText().toStdString());
break;
target_is_ready_.store(false);
return false;
}
}
target_->initialize();
Expand All @@ -368,6 +365,7 @@ bool TargetTabWidget::createTargetInstance()
{
QMessageBox::warning(this, tr("Exception while loading a handeye target plugin"), tr(ex.what()));
target_ = nullptr;
target_is_ready_.store(false);
return false;
}

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

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

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

} // namespace moveit_rviz_plugin
32 changes: 32 additions & 0 deletions ...andeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down Expand Up @@ -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
Expand Down
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_);
JStech marked this conversation as resolved.
Show resolved Hide resolved
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