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

Added the ability to set postion

Browse Source
This commit is contained in:
Pimpest
2025-05-08 02:25:34 +02:00
parent 07cc056853
commit da3d1d9d15
9 changed files with 131 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
mg_odometry/src/mg_odom_publisher.cpp
View File

@ -9,6 +9,7 @@
#include <sys/types.h>
#include "mg_msgs/srv/send_double.hpp"
#include "mg_msgs/srv/send_pose2d.hpp"
#include "std_srvs/srv/empty.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
@ -26,6 +27,7 @@
constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
using SendDoubleSrv = mg_msgs::srv::SendDouble;
using SendPoseSrv = mg_msgs::srv::SendPose2d;
using ZeroSrv = std_srvs::srv::Empty;
class MgOdomPublisher : public rclcpp::Node {
@ -44,6 +46,8 @@ class MgOdomPublisher : public rclcpp::Node {
"/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));
set_pose_service_ = create_service<SendPoseSrv>(
"/set_pose", std::bind(&MgOdomPublisher::set_pose, this, std::placeholders::_1));
zero_service_
= create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
@ -56,6 +60,7 @@ class MgOdomPublisher : public rclcpp::Node {
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_service_;
rclcpp::Service<SendPoseSrv>::SharedPtr set_pose_service_;
rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
rclcpp::Service<ZeroSrv>::SharedPtr calib_service_;
rclcpp::TimerBase::SharedPtr timer_;
@ -76,6 +81,31 @@ class MgOdomPublisher : public rclcpp::Node {
}
}
void set_pose(const std::shared_ptr<SendPoseSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting pose to: %lf %lf %lf", request->x, request->y, request->theta);
if (enc_serial_port_.IsOpen()) {
union {
std::array<u_char, sizeof(double)> bytes;
double data;
} value{};
value.data = request->x;
enc_serial_port_ << "X;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
value.data = request->y;
enc_serial_port_ << "Y;";
for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
value.data = request->theta;
enc_serial_port_ << "T;";
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);