Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ported over odometry and control using new serial communication

Browse Source
This commit is contained in:
Pimpest
2026-02-08 00:49:45 +01:00
parent 05e7398731
commit fc5fecdfc1
20 changed files with 1213 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
toid_odometry/CMakeLists.txt Normal file
View File

@@ -0,0 +1,47 @@
cmake_minimum_required(VERSION 3.8)
project(toid_odometry)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
set(PACKAGE_DEPS
rclcpp
Boost
std_srvs
tf2
tf2_ros
tf2_geometry_msgs
toid_msgs
)
foreach(PACKAGE ${PACKAGE_DEPS})
find_package(${PACKAGE} REQUIRED)
endforeach()
set(SOURCES
src/odometry.cpp
)
add_executable(toid_odometry ${SOURCES})
target_include_directories(
toid_odometry
PRIVATE
include
)
ament_target_dependencies(toid_odometry ${PACKAGE_DEPS})
install(TARGETS toid_odometry DESTINATION lib/${PROJECT_NAME})
target_compile_features(
toid_odometry PUBLIC
c_std_99
cxx_std_17
)
ament_package()

47
toid_odometry/include/toid_odometry/crc.h Normal file
View File

@@ -0,0 +1,47 @@
#pragma once
#include <stdint.h>
const unsigned char CRC_TABLE[256] = {
0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
};
#define crcFooter(data_type) _crc((const uint8_t *) &(data_type), sizeof(data_type)-1)
#define crcNoFooter(data_type) _crc((const uint8_t *) &(data_type), sizeof(data_type))
inline unsigned char _crc(const uint8_t *data, unsigned int size) {
uint8_t crc = 0;
while(size--) {
crc = CRC_TABLE[ (crc ^ *(data++)) & 0xff];
}
return crc;
}

79
toid_odometry/include/toid_odometry/msgs.h Normal file
View File

@@ -0,0 +1,79 @@
#pragma once
#include "crc.h"
typedef enum tCmdId {
TMSG_REQ_STATE = 0x7F,
TMSG_SET_WIDTH,
TMSG_SET_RATIO,
TMSG_END_CALIB,
TMSG_SET_SPEED,
TMSG_ZERO,
TMSG_DELIM,
} tCmdId;
typedef struct tMsgRatio {
double ratio;
unsigned char crc;
}__attribute__((packed)) tMsgRatio;
typedef struct tMsgWidth {
double width;
unsigned char crc;
}__attribute__((packed)) tMsgWidth;
typedef struct tMsgSpeed {
double vl;
double vr;
unsigned char crc;
}__attribute__((packed)) tMsgSpeed;
typedef struct tRespState {
double x;
double y;
double theta;
double vl;
double vr;
unsigned char crc;
}__attribute__((packed)) tRespState;
typedef struct tRespEndCalib {
double left_gain;
double right_gain;
unsigned char crc;
}__attribute__((packed)) tRespEndCalib;
typedef union tMsgBuf {
tMsgRatio msg_ratio;
tMsgWidth msg_width;
tMsgSpeed msg_speed;
}__attribute__((packed)) tMsgBuf;
#define TCMD_ID(id) ((uint16_t)(id << 8 | TMSG_DELIM))
#define TGET_CMD(cmd) ((cmd >> 8) & 0xFF)
enum {
TSTATE_IDLE,
TSTATE_BUILD_MSG,
};
inline unsigned push_tMsgBuf(tMsgBuf *buf, unsigned cnt, unsigned char byte, tCmdId id) {
unsigned size = 0;
switch (id) {
case TMSG_SET_RATIO:
size = sizeof(tMsgRatio);
break;
case TMSG_SET_WIDTH:
size = sizeof(tMsgRatio);
break;
case TMSG_SET_SPEED:
size = sizeof(tMsgRatio);
break;
default:
return 0;
}
if(cnt < size) {
((unsigned char*)buf)[cnt] = byte;
}
return cnt == size-1;
}

26
toid_odometry/package.xml Normal file
View File

@@ -0,0 +1,26 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>toid_odometry</name>
<version>0.0.0</version>
<description>Odometry publisher for robotoid</description>
<maintainer email="82343504+pimpest@users.noreply.github.com">pimpest</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>boost</depend>
<depend>std_srvs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>toid_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

211
toid_odometry/src/odometry.cpp Normal file
View File

@@ -0,0 +1,211 @@
#include <iostream>
#include <chrono>
#include <boost/asio.hpp>
#include "rclcpp/rclcpp.hpp"
#include "toid_odometry/msgs.h"
#include "toid_odometry/crc.h"
#include "toid_msgs/srv/send_double.hpp"
#include "std_srvs/srv/empty.hpp"
#include "tf2_ros/transform_broadcaster.hpp"
#include "tf2_ros/static_transform_broadcaster.hpp"
#include "tf2/LinearMath/Transform.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/convert.hpp"
namespace asio = boost::asio;
class ToidOdomNode : public rclcpp::Node {
using SendDoubleSrv = toid_msgs::srv::SendDouble;
using ZeroSrv = std_srvs::srv::Empty;
public:
ToidOdomNode() : rclcpp::Node("OdometryNode"), serial_(io_ctx_) {
using std::placeholders::_1;
this->declare_parameter("base_frame", "base_footprint");
this->declare_parameter("odometry_frame", "odom");
this->declare_parameter("map_frame", "map");
this->declare_parameter("serial_port", "/dev/ttyACM1");
this->declare_parameter("initial_pose", "0 0 0");
this->declare_parameter("mock_odom", false);
odometry_frame_id_ = this->get_parameter("odometry_frame").as_string();
base_frame_id_ = this->get_parameter("base_frame").as_string();
map_frame_id_ = this->get_parameter("map_frame").as_string();
serial_port_path_ = this->get_parameter("serial_port").as_string();
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = map_frame_id_;
t.child_frame_id = odometry_frame_id_;
t.transform = this->make_transform(0,0,0);
tf_static_broadcaster_->sendTransform(t);
startpose_listener_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"/initialpose", 1, std::bind(&ToidOdomNode::set_pose,this,_1)
);
if(!this->get_parameter("mock_odom").as_bool()) {
timer_ = this->create_wall_timer(
std::chrono::seconds(1/100),
[this]{this->publish_robot_state();});
set_width_service_ = create_service<SendDoubleSrv> (
"/set_width", std::bind(&ToidOdomNode::set_width, this, _1));
set_ratio_service_ = create_service<SendDoubleSrv> (
"/set_ratio", std::bind(&ToidOdomNode::set_ratio, this, _1));
calib_service_ = create_service<ZeroSrv> (
"/end_calib", std::bind(&ToidOdomNode::end_calib, this, _1));
zero_service_ = create_service<ZeroSrv> (
"/zero", std::bind(&ToidOdomNode::zero, this, _1));
}
}
private:
void set_pose(const geometry_msgs::msg::PoseWithCovarianceStamped &pose) {
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = map_frame_id_;
t.child_frame_id = odometry_frame_id_;
t.transform.rotation = pose.pose.pose.orientation;
auto[x,y,z] =pose.pose.pose.position;
t.transform.translation.x = x;
t.transform.translation.y = y;
t.transform.translation.z = z;
RCLCPP_INFO(this->get_logger(), "Setting new initial_pose");
tf_static_broadcaster_->sendTransform(t);
}
void set_width(const std::shared_ptr<SendDoubleSrv::Request> req) {
boost::system::error_code ec;
const std::array<uint8_t, 2> cmd{TMSG_SET_WIDTH, TMSG_DELIM};
tMsgWidth msg = { req->data, 0};
msg.crc = crcFooter(msg);
asio::write(this->serial_, asio::buffer(cmd), ec);
asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec);
}
void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> req) {
boost::system::error_code ec;
const std::array<uint8_t, 2> cmd{TMSG_SET_RATIO, TMSG_DELIM};
tMsgRatio msg = { req->data, 0 };
msg.crc = crcFooter(msg);
asio::write(this->serial_, asio::buffer(cmd), ec);
asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec);
}
void end_calib(const std::shared_ptr<ZeroSrv::Request>) {
boost::system::error_code ec;
const std::array<uint8_t, 2> cmd{TMSG_END_CALIB, TMSG_DELIM};
asio::write(this->serial_, asio::buffer(cmd), ec);
tRespEndCalib resp;
asio::read(this->serial_, asio::buffer(&resp, sizeof(resp)) , ec);
if(!ec.failed()) {
RCLCPP_INFO(this->get_logger(), "Calculated ratio: %lf, %lf", resp.left_gain, resp.right_gain);
}
}
void zero(const std::shared_ptr<ZeroSrv::Request>) {
boost::system::error_code ec;
const std::array<uint8_t, 2> cmd{TMSG_ZERO, TMSG_DELIM};
asio::write(this->serial_, asio::buffer(cmd), ec);
}
void publish_robot_state() {
tRespState state;
if(!read_robot_state(state)) {
return;
}
send_transform(state);
}
geometry_msgs::msg::Transform make_transform(double x, double y, double theta){
geometry_msgs::msg::Transform t;
tf2::Quaternion q;
q.setRPY(0, 0, theta);
const tf2::Transform tf(q, tf2::Vector3(x, y, 0));
tf2::convert(tf, t);
return t;
}
void send_transform(tRespState &state) {
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = this->odometry_frame_id_;
t.child_frame_id = this->base_frame_id_;
t.transform = make_transform(state.x, state.y, state.theta);
tf_broadcaster_->sendTransform(t);
}
bool read_robot_state(tRespState &state) {
const std::array<uint8_t, 2> cmd{TMSG_REQ_STATE, TMSG_DELIM};
try {
asio::write(this->serial_, asio::buffer(cmd));
asio::read(this->serial_, asio::buffer(&state, sizeof(state)));
} catch (const std::exception &e){
RCLCPP_ERROR_SKIPFIRST_THROTTLE(
this->get_logger(),
*this->get_clock(),
5000,
"%s",
e.what()
);
boost::system::error_code ec;
if(serial_.close(ec)) {}
if(serial_.open(serial_port_path_,ec)) {
RCLCPP_ERROR_THROTTLE(
this->get_logger(),
*this->get_clock(),
5000,
"%s",
ec.what().c_str()
);
}
return false;
}
return state.crc == crcFooter(state);
}
private:
asio::io_context io_ctx_;
asio::serial_port serial_;
rclcpp::TimerBase::SharedPtr timer_;
std::string odometry_frame_id_;
std::string base_frame_id_;
std::string map_frame_id_;
std::string serial_port_path_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_service_;
rclcpp::Service<ZeroSrv>::SharedPtr calib_service_;
rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr startpose_listener_;
};
int main(const int argc,const char** argv) {
rclcpp::init(argc,argv);
rclcpp::spin(std::make_shared<ToidOdomNode>());
rclcpp::shutdown();
return 0;
}