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

Add aditional services to mg_odometry for easier callibration #2

Merged
Pimpest merged 8 commits from odometry-calib into main 2025-03-10 11:45:41 +00:00
Conversation 0 Commits 8 Files Changed 10 +160 -6
10 changed files with 160 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 4cd994cf7a - Show all commits

7
mg_odometry/src/mg_odom_publisher.cpp
View File

@ -6,6 +6,7 @@
#include <rclcpp/service.hpp> #include <rclcpp/service.hpp>
#include <string> #include <string>
#include <libserial/SerialStream.h> #include <libserial/SerialStream.h>
#include <sys/types.h>
#include "mg_msgs/srv/set_width.hpp" #include "mg_msgs/srv/set_width.hpp"
#include "std_srvs/srv/empty.hpp" #include "std_srvs/srv/empty.hpp"
@ -56,9 +57,9 @@ class MgOdomPublisher : public rclcpp::Node {
void set_width(const std::shared_ptr<SetWidthSrv::Request> request) { void set_width(const std::shared_ptr<SetWidthSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width); RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width);
if (!enc_serial_port_.IsOpen()) { if (enc_serial_port_.IsOpen()) {
union { union {
std::array<double, sizeof(double)> bytes; std::array<u_char, sizeof(double)> bytes;
double data; double data;
} value{}; } value{};
value.data = request->width; value.data = request->width;
@ -71,7 +72,7 @@ class MgOdomPublisher : public rclcpp::Node {
void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) { void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
RCLCPP_INFO(get_logger(), "Zeroing odometry"); RCLCPP_INFO(get_logger(), "Zeroing odometry");
if (!enc_serial_port_.IsOpen()) { if (enc_serial_port_.IsOpen()) {
enc_serial_port_ << "z;"; enc_serial_port_ << "z;";
} }
} }