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

Updated firmware for new pcb

Browse Source
This commit is contained in:
Pimpest
2025-04-11 15:41:02 +02:00
parent a4691caeed
commit f064bcfc69
4 changed files with 34 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
mg_odometry/src/mg_odom_publisher.cpp
View File

@ -8,7 +8,7 @@
#include <libserial/SerialStream.h>
#include <sys/types.h>
#include "mg_msgs/srv/set_width.hpp"
#include "mg_msgs/srv/send_double.hpp"
#include "std_srvs/srv/empty.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
@ -25,8 +25,8 @@
constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
using SetWidthSrv = mg_msgs::srv::SetWidth;
using ZeroSrv = std_srvs::srv::Empty;
using SendDoubleSrv = mg_msgs::srv::SendDouble;
using ZeroSrv = std_srvs::srv::Empty;
class MgOdomPublisher : public rclcpp::Node {
public:
@ -40,8 +40,10 @@ class MgOdomPublisher : public rclcpp::Node {
timer_ = this->create_wall_timer(std::chrono::milliseconds(TIMEOUT),
std::bind(&MgOdomPublisher::timer_callback, this));
set_width_service_ = create_service<SetWidthSrv>(
set_width_service_ = create_service<SendDoubleSrv>(
"/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
set_ratio_service_ = create_service<SendDoubleSrv>(
"/set_ratio", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
zero_service_
= create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
@ -49,26 +51,42 @@ class MgOdomPublisher : public rclcpp::Node {
private:
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::Service<SetWidthSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_service_;
rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
rclcpp::TimerBase::SharedPtr timer_;
LibSerial::SerialStream enc_serial_port_;
void set_width(const std::shared_ptr<SetWidthSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width);
void set_width(const std::shared_ptr<SendDoubleSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->data);
if (enc_serial_port_.IsOpen()) {
union {
std::array<u_char, sizeof(double)> bytes;
double data;
} value{};
value.data = request->width;
value.data = request->data;
enc_serial_port_ << "w;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
}
}
void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data);
if (enc_serial_port_.IsOpen()) {
union {
std::array<u_char, sizeof(double)> bytes;
double data;
} value{};
value.data = request->data;
enc_serial_port_ << "r;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
}
}
void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
RCLCPP_INFO(get_logger(), "Zeroing odometry");