Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Added calibration sequence for encoder diameter diff

Browse Source
This commit is contained in:
Pimpest
2025-04-29 14:59:27 +02:00
parent 6fefe74442
commit ed9c5bab45
4 changed files with 93 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
mg_odometry/src/mg_odom_publisher.cpp
View File

@ -47,6 +47,9 @@ class MgOdomPublisher : public rclcpp::Node {
zero_service_
= create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
calib_service_
= create_service<ZeroSrv>("/endCalib", std::bind(&MgOdomPublisher::calib, this, std::placeholders::_1));
}
private:
@ -54,6 +57,7 @@ class MgOdomPublisher : public rclcpp::Node {
rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_service_;
rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
rclcpp::Service<ZeroSrv>::SharedPtr calib_service_;
rclcpp::TimerBase::SharedPtr timer_;
LibSerial::SerialStream enc_serial_port_;
@ -87,6 +91,18 @@ class MgOdomPublisher : public rclcpp::Node {
}
}
void calib(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
RCLCPP_INFO(get_logger(), "Finishing wheel diameter calib");
if (enc_serial_port_.IsOpen()) {
enc_serial_port_ << "c;";
double left_gain = 0;
double right_gain = 0;
enc_serial_port_ >> left_gain >> right_gain;
RCLCPP_INFO(this->get_logger(), "The new gains are %lf %lf", left_gain, right_gain);
}
}
void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
RCLCPP_INFO(get_logger(), "Zeroing odometry");