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

Try and fix random disconnects

Browse Source
This commit is contained in:
Pimpest
2026-04-17 07:40:08 +02:00
parent 35081044de
commit a69c0b97e9
2 changed files with 42 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
toid_control/src/toid_control.cpp
View File

@@ -82,6 +82,15 @@ hardware_interface::return_type toid::StepperInterface::write(const rclcpp::Time
boost::system::error_code ec;
ec = serial_port_.close(ec);
ec = serial_port_.open(serial_port_name_, ec);
if(!ec.failed()) {
try {
for(int i = 0; i < 64; i++) {
asio::write(serial_port_, asio::buffer("\x00"));
}
} catch(const std::runtime_error& err) {
}
}
}
return hardware_interface::return_type::OK;
}

33
toid_odometry/src/odometry.cpp
View File

@@ -139,6 +139,23 @@ private:
tf_static_broadcaster_->sendTransform(t);
}
void serial_ec_recovery(boost::system::error_code ec) {
if(ec.failed()) {
boost::system::error_code ec;
ec = this->serial_.close(ec);
ec = this->serial_.open(serial_port_path_, ec);
if(!ec.failed()) {
try {
for(int i = 0; i < 64; i++) {
asio::write(this->serial_, asio::buffer("\x00"));
}
} catch(const std::runtime_error& err) {
}
}
}
}
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};
@@ -146,6 +163,7 @@ private:
msg.crc = crcFooter(msg);
asio::write(this->serial_, asio::buffer(cmd), ec);
asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec);
serial_ec_recovery(ec);
}
void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> req) {
@@ -155,6 +173,7 @@ private:
msg.crc = crcFooter(msg);
asio::write(this->serial_, asio::buffer(cmd), ec);
asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec);
serial_ec_recovery(ec);
}
void end_calib(const std::shared_ptr<ZeroSrv::Request>) {
@@ -166,12 +185,15 @@ private:
if(!ec.failed()) {
RCLCPP_INFO(this->get_logger(), "Calculated ratio: %lf, %lf", resp.left_gain, resp.right_gain);
}
serial_ec_recovery(ec);
}
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);
serial_ec_recovery(ec);
}
void publish_robot_state() {
@@ -245,6 +267,17 @@ private:
ec.what().c_str()
);
}
if(!ec.failed()) {
try {
for(int i = 0; i < 64; i++) {
asio::write(serial_, asio::buffer("\x00"));
}
} catch(const std::runtime_error& err) {
}
}
return false;
}
return state.crc == crcFooter(state);