127 lines
4.1 KiB
C++
127 lines
4.1 KiB
C++
#include <array>
|
|
#include <chrono>
|
|
#include <cstddef>
|
|
#include <functional>
|
|
#include <memory>
|
|
#include <rclcpp/service.hpp>
|
|
#include <string>
|
|
#include <libserial/SerialStream.h>
|
|
|
|
#include "mg_msgs/srv/set_width.hpp"
|
|
#include "std_srvs/srv/empty.hpp"
|
|
|
|
#include "geometry_msgs/msg/transform_stamped.hpp"
|
|
#include "tf2_ros/transform_broadcaster.h"
|
|
|
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
#include "tf2/LinearMath/Transform.hpp"
|
|
#include "tf2/LinearMath/Quaternion.hpp"
|
|
#include "tf2/convert.hpp"
|
|
|
|
#include "rclcpp/rclcpp.hpp"
|
|
|
|
#define TIMEOUT 10u
|
|
|
|
constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
|
|
|
|
using SetWidthSrv = mg_msgs::srv::SetWidth;
|
|
using ZeroSrv = std_srvs::srv::Empty;
|
|
|
|
class MgOdomPublisher : public rclcpp::Node {
|
|
public:
|
|
MgOdomPublisher() : Node("mg_odom_publisher") {
|
|
this->declare_parameter("odom", "odom");
|
|
this->declare_parameter("target", "base-link");
|
|
this->declare_parameter("serial_path", ENCODER_SERIAL_PATH_DEFAULT);
|
|
|
|
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
|
|
|
|
timer_ = this->create_wall_timer(std::chrono::milliseconds(TIMEOUT),
|
|
std::bind(&MgOdomPublisher::timer_callback, this));
|
|
|
|
set_width_service_ = create_service<SetWidthSrv>(
|
|
"/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
|
|
|
|
zero_service_
|
|
= create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
|
|
}
|
|
|
|
private:
|
|
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
|
rclcpp::Service<SetWidthSrv>::SharedPtr set_width_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);
|
|
|
|
if (!enc_serial_port_.IsOpen()) {
|
|
union {
|
|
std::array<double, sizeof(double)> bytes;
|
|
double data;
|
|
} value{};
|
|
value.data = request->width;
|
|
|
|
enc_serial_port_ << "w;";
|
|
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");
|
|
|
|
if (!enc_serial_port_.IsOpen()) {
|
|
enc_serial_port_ << "z;";
|
|
}
|
|
}
|
|
|
|
void timer_callback() {
|
|
double _x = NAN;
|
|
double _y = NAN;
|
|
double _theta = NAN;
|
|
|
|
RCLCPP_DEBUG(this->get_logger(), "Callback called");
|
|
|
|
try {
|
|
if (!enc_serial_port_.IsOpen()) {
|
|
enc_serial_port_.Open(this->get_parameter("serial_path").as_string());
|
|
}
|
|
|
|
enc_serial_port_ << "g;";
|
|
|
|
if (!(enc_serial_port_ >> _x >> _y >> _theta)) {
|
|
RCLCPP_INFO(this->get_logger(), "Reading Serial Port failed. Closing...");
|
|
throw "Unfortunately bug with libserial";
|
|
}
|
|
|
|
RCLCPP_DEBUG(this->get_logger(), "Got following from rpi:{ x: %lf, y: %lf, z: %lf}\n", _x, _y, _theta);
|
|
|
|
make_transform(_x, _y, _theta);
|
|
} catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "%s\n", e.what()); }
|
|
}
|
|
|
|
void make_transform(double x, double y, double theta) {
|
|
geometry_msgs::msg::TransformStamped t;
|
|
|
|
t.header.stamp = this->get_clock()->now();
|
|
t.header.frame_id = this->get_parameter("odom").as_string();
|
|
t.child_frame_id = this->get_parameter("target").as_string();
|
|
|
|
tf2::Quaternion q;
|
|
q.setRPY(0, 0, theta);
|
|
const tf2::Transform tf(tf2::Transform(q, tf2::Vector3(x, y, 0)));
|
|
tf2::convert(tf, t.transform);
|
|
|
|
tf_broadcaster_->sendTransform(t);
|
|
}
|
|
};
|
|
|
|
int main(const int argc, const char** argv) {
|
|
rclcpp::init(argc, argv);
|
|
rclcpp::spin(std::make_shared<MgOdomPublisher>());
|
|
rclcpp::shutdown();
|
|
|
|
return 0;
|
|
}
|