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_);