Skip to content

Commit

Permalink
throttle messages from target plugin (#6)
Browse files Browse the repository at this point in the history
  • Loading branch information
John Stechschulte authored Oct 7, 2020
1 parent 9fd0de8 commit 6c9044d
Show file tree
Hide file tree
Showing 4 changed files with 32 additions and 28 deletions.
34 changes: 17 additions & 17 deletions moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_aruco.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,13 +97,13 @@ bool HandEyeArucoTarget::setTargetIntrinsicParams(int markers_x, int markers_y,
if (markers_x <= 0 || markers_y <= 0 || marker_size <= 0 || separation <= 0 || border_bits <= 0 ||
marker_dictionaries_.find(dictionary_id) == marker_dictionaries_.end())
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "Invalid target intrinsic params.\n"
<< "markers_x_ " << std::to_string(markers_x) << "\n"
<< "markers_y_ " << std::to_string(markers_y) << "\n"
<< "marker_size " << std::to_string(marker_size) << "\n"
<< "separation " << std::to_string(separation) << "\n"
<< "border_bits " << std::to_string(border_bits) << "\n"
<< "dictionary_id " << dictionary_id << "\n");
ROS_ERROR_STREAM_THROTTLE_NAMED(2., LOGNAME, "Invalid target intrinsic params.\n"
<< "markers_x_ " << std::to_string(markers_x) << "\n"
<< "markers_y_ " << std::to_string(markers_y) << "\n"
<< "marker_size " << std::to_string(marker_size) << "\n"
<< "separation " << std::to_string(separation) << "\n"
<< "border_bits " << std::to_string(border_bits) << "\n"
<< "dictionary_id " << dictionary_id << "\n");
return false;
}

Expand All @@ -124,18 +124,18 @@ bool HandEyeArucoTarget::setTargetDimension(double marker_measured_size, double
{
if (marker_measured_size <= 0 || marker_measured_separation <= 0)
{
ROS_ERROR_NAMED(LOGNAME, "Invalid target measured dimensions: marker_size %f, marker_seperation %f",
marker_measured_size, marker_measured_separation);
ROS_ERROR_THROTTLE_NAMED(2., LOGNAME, "Invalid target measured dimensions: marker_size %f, marker_seperation %f",
marker_measured_size, marker_measured_separation);
return false;
}

std::lock_guard<std::mutex> aruco_lock(aruco_mutex_);
marker_size_real_ = marker_measured_size;
marker_separation_real_ = 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");
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) << "\n");
return true;
}

Expand Down Expand Up @@ -187,7 +187,7 @@ bool HandEyeArucoTarget::detectTargetPose(cv::Mat& image)
cv::aruco::detectMarkers(image, dictionary, marker_corners, marker_ids, params_ptr);
if (marker_ids.empty())
{
ROS_DEBUG_STREAM_NAMED(LOGNAME, "No aruco marker detected.");
ROS_DEBUG_STREAM_THROTTLE_NAMED(1., LOGNAME, "No aruco marker detected.");
return false;
}

Expand All @@ -203,15 +203,15 @@ bool HandEyeArucoTarget::detectTargetPose(cv::Mat& image)
// Draw the markers and frame axis if at least one marker is detected
if (valid == 0)
{
ROS_WARN_STREAM_NAMED(LOGNAME, "Cannot estimate aruco board pose.");
ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Cannot estimate aruco board pose.");
return false;
}

if (std::log10(std::fabs(rotation_vect_[0])) > 10 || std::log10(std::fabs(rotation_vect_[1])) > 10 ||
std::log10(std::fabs(rotation_vect_[2])) > 10 || std::log10(std::fabs(translation_vect_[0])) > 10 ||
std::log10(std::fabs(translation_vect_[1])) > 10 || std::log10(std::fabs(translation_vect_[2])) > 10)
{
ROS_WARN_STREAM_NAMED(LOGNAME, "Invalid target pose, please check CameraInfo msg.");
ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Invalid target pose, please check CameraInfo msg.");
return false;
}

Expand All @@ -223,7 +223,7 @@ bool HandEyeArucoTarget::detectTargetPose(cv::Mat& image)
}
catch (const cv::Exception& e)
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "Aruco target detection exception: " << e.what());
ROS_ERROR_STREAM_THROTTLE_NAMED(1., LOGNAME, "Aruco target detection exception: " << e.what());
return false;
}

Expand Down
24 changes: 13 additions & 11 deletions moveit_calibration_plugins/handeye_calibration_target/src/handeye_target_charuco.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,8 @@ bool HandEyeCharucoTarget::setTargetIntrinsicParams(int squares_x, int squares_y
margin_size_pixels < 0 || border_size_bits <= 0 || square_size_pixels <= marker_size_pixels ||
0 == marker_dictionaries_.count(dictionary_id))
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "Invalid target intrinsic params.\n"
ROS_ERROR_STREAM_THROTTLE_NAMED(2., LOGNAME,
"Invalid target intrinsic params.\n"
<< "squares_x " << std::to_string(squares_x) << "\n"
<< "squares_y " << std::to_string(squares_y) << "\n"
<< "marker_size_pixels " << std::to_string(marker_size_pixels) << "\n"
Expand Down Expand Up @@ -133,16 +134,17 @@ bool HandEyeCharucoTarget::setTargetDimension(double board_size_meters, double m
if (board_size_meters <= 0 || marker_size_meters <= 0 ||
board_size_meters < marker_size_meters * std::max(squares_x_, squares_y_))
{
ROS_ERROR_NAMED(LOGNAME, "Invalid target measured dimensions. Longest board dimension: %f. Marker size: %f",
board_size_meters, marker_size_meters);
ROS_ERROR_THROTTLE_NAMED(2., LOGNAME,
"Invalid target measured dimensions. Longest board dimension: %f. Marker size: %f",
board_size_meters, marker_size_meters);
return false;
}

std::lock_guard<std::mutex> charuco_lock(charuco_mutex_);
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");
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"
<< "\n");
board_size_meters_ = board_size_meters;
marker_size_meters_ = marker_size_meters;
return true;
Expand Down Expand Up @@ -201,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_NAMED(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 @@ -218,14 +220,14 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image)
// Draw the markers and frame axis if at least one marker is detected
if (!valid)
{
ROS_WARN_STREAM_NAMED(LOGNAME, "Cannot estimate aruco board pose.");
ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Cannot estimate aruco board pose.");
return false;
}

if (cv::norm(rotation_vect_) > 3.2 || std::log10(std::fabs(translation_vect_[0])) > 4 ||
std::log10(std::fabs(translation_vect_[1])) > 4 || std::log10(std::fabs(translation_vect_[2])) > 4)
{
ROS_WARN_STREAM_NAMED(LOGNAME, "Invalid target pose, please check CameraInfo msg.");
ROS_WARN_STREAM_THROTTLE_NAMED(1., LOGNAME, "Invalid target pose, please check CameraInfo msg.");
return false;
}

Expand All @@ -237,7 +239,7 @@ bool HandEyeCharucoTarget::detectTargetPose(cv::Mat& image)
}
catch (const cv::Exception& e)
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "ChArUco target detection exception: " << e.what());
ROS_ERROR_STREAM_THROTTLE_NAMED(1., LOGNAME, "ChArUco target detection exception: " << e.what());
return false;
}

Expand Down
1 change: 1 addition & 0 deletions moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_aruco_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ class MoveItHandEyeTargetTester : public ::testing::Test
{
try
{
ros::Time::init();
target_plugins_loader_.reset(new pluginlib::ClassLoader<moveit_handeye_calibration::HandEyeTargetBase>(
"moveit_calibration_plugins", "moveit_handeye_calibration::HandEyeTargetBase"));
target_ = target_plugins_loader_->createUniqueInstance("HandEyeTarget/Aruco");
Expand Down
1 change: 1 addition & 0 deletions moveit_calibration_plugins/handeye_calibration_target/test/handeye_target_charuco_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ class MoveItHandEyeTargetTester : public ::testing::Test
{
try
{
ros::Time::init();
target_plugins_loader_.reset(new pluginlib::ClassLoader<moveit_handeye_calibration::HandEyeTargetBase>(
"moveit_calibration_plugins", "moveit_handeye_calibration::HandEyeTargetBase"));
target_ = target_plugins_loader_->createUniqueInstance("HandEyeTarget/Charuco");
Expand Down

0 comments on commit 6c9044d

Please sign in to comment.