From e3e7cf8ad10db72ced21049da0c12b617435c311 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 10 Aug 2022 15:11:44 +0000 Subject: [PATCH 1/2] publish calibrated zero_offset & latch publishers There is no reason to publish messages with 2Hz to say calibration completed. --- .../joint_calibration_controller.h | 4 ++- .../src/joint_calibration_controller.cpp | 27 +++++++++++-------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/pr2_calibration_controllers/include/pr2_calibration_controllers/joint_calibration_controller.h b/pr2_calibration_controllers/include/pr2_calibration_controllers/joint_calibration_controller.h index fe4504c..abcf3e1 100755 --- a/pr2_calibration_controllers/include/pr2_calibration_controllers/joint_calibration_controller.h +++ b/pr2_calibration_controllers/include/pr2_calibration_controllers/joint_calibration_controller.h @@ -43,6 +43,7 @@ #include "robot_mechanism_controllers/joint_velocity_controller.h" #include "realtime_tools/realtime_publisher.h" #include "std_msgs/Empty.h" +#include "std_msgs/Float32.h" #include "pr2_controllers_msgs/QueryCalibrationState.h" @@ -65,12 +66,13 @@ class JointCalibrationController : public pr2_controller_interface::Controller protected: pr2_mechanism_model::RobotState* robot_; ros::NodeHandle node_; - ros::Time last_publish_time_; ros::ServiceServer is_calibrated_srv_; boost::scoped_ptr > pub_calibrated_; + boost::scoped_ptr > pub_zero_offset_; enum { INITIALIZED, BEGINNING, MOVING_TO_LOW, MOVING_TO_HIGH, CALIBRATED }; int state_; + bool announced_calibration_success_; int countdown_; double search_velocity_; diff --git a/pr2_calibration_controllers/src/joint_calibration_controller.cpp b/pr2_calibration_controllers/src/joint_calibration_controller.cpp index 0634d9b..6e0951b 100755 --- a/pr2_calibration_controllers/src/joint_calibration_controller.cpp +++ b/pr2_calibration_controllers/src/joint_calibration_controller.cpp @@ -43,8 +43,7 @@ using namespace std; namespace controller { JointCalibrationController::JointCalibrationController() -: robot_(NULL), last_publish_time_(0), - actuator_(NULL), joint_(NULL) +: robot_(NULL), actuator_(NULL), joint_(NULL) { } @@ -96,6 +95,7 @@ bool JointCalibrationController::init(pr2_mechanism_model::RobotState *robot, ro state_ = INITIALIZED; joint_->calibrated_ = false; + announced_calibration_success_ = false; if (actuator_->state_.zero_offset_ != 0) { if (force_calibration) @@ -173,8 +173,8 @@ bool JointCalibrationController::init(pr2_mechanism_model::RobotState *robot, ro is_calibrated_srv_ = node_.advertiseService("is_calibrated", &JointCalibrationController::isCalibrated, this); // "Calibrated" topic - pub_calibrated_.reset(new realtime_tools::RealtimePublisher(node_, "calibrated", 1)); - + pub_calibrated_.reset(new realtime_tools::RealtimePublisher(node_, "calibrated", 1, true)); + pub_zero_offset_.reset(new realtime_tools::RealtimePublisher(node_, "zero_offset", 1, true)); return true; } @@ -185,6 +185,7 @@ void JointCalibrationController::starting() state_ = INITIALIZED; joint_->calibrated_ = false; actuator_->state_.zero_offset_ = 0.0; + announced_calibration_success_ = false; } @@ -258,13 +259,17 @@ void JointCalibrationController::update() break; } case CALIBRATED: - if (pub_calibrated_) { - if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime()){ - assert(pub_calibrated_); - if (pub_calibrated_->trylock()) { - last_publish_time_ = robot_->getTime(); - pub_calibrated_->unlockAndPublish(); - } + if (!announced_calibration_success_ && pub_zero_offset_ && pub_calibrated_) { + if (pub_zero_offset_->trylock()) { + if (pub_calibrated_->trylock()) { + pub_calibrated_->unlockAndPublish(); + pub_zero_offset_->msg_.data = actuator_->state_.zero_offset_; + pub_zero_offset_->unlockAndPublish(); + announced_calibration_success_ = true; + } + else { + pub_zero_offset_->unlock(); + } } } break; From 8cf343c65bbaf59536c0f70bd53434864d3635af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Wed, 10 Aug 2022 15:12:40 +0000 Subject: [PATCH 2/2] calibration: turn countdown_ into parameter For smooth slow motions through the zero crossing it can be helpful to increase the distance before passing through it from low to high. As a longer default time changes the well-established startup initialization to the worse (more collisions) it's better to turn it into a parameter without changing the default. --- .../joint_calibration_controller.h | 1 + .../src/joint_calibration_controller.cpp | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/pr2_calibration_controllers/include/pr2_calibration_controllers/joint_calibration_controller.h b/pr2_calibration_controllers/include/pr2_calibration_controllers/joint_calibration_controller.h index abcf3e1..89ea5db 100755 --- a/pr2_calibration_controllers/include/pr2_calibration_controllers/joint_calibration_controller.h +++ b/pr2_calibration_controllers/include/pr2_calibration_controllers/joint_calibration_controller.h @@ -74,6 +74,7 @@ class JointCalibrationController : public pr2_controller_interface::Controller int state_; bool announced_calibration_success_; int countdown_; + int tics_moving_past_calibration_reading_; double search_velocity_; double original_position_; diff --git a/pr2_calibration_controllers/src/joint_calibration_controller.cpp b/pr2_calibration_controllers/src/joint_calibration_controller.cpp index 6e0951b..5d42be5 100755 --- a/pr2_calibration_controllers/src/joint_calibration_controller.cpp +++ b/pr2_calibration_controllers/src/joint_calibration_controller.cpp @@ -90,6 +90,9 @@ bool JointCalibrationController::init(pr2_mechanism_model::RobotState *robot, ro return false; } + tics_moving_past_calibration_reading_ = 200; + node_.getParam("tics_moving_past_calibration_reading", tics_moving_past_calibration_reading_); + bool force_calibration = false; node_.getParam("force_calibration", force_calibration); @@ -227,7 +230,7 @@ void JointCalibrationController::update() } } else - countdown_ = 200; + countdown_ = tics_moving_past_calibration_reading_; break; case MOVING_TO_HIGH: { vc_.setCommand(search_velocity_);