m5p3nc3r/ros-raspbot-v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ensure ros controller doesn't timeout on cmd_vel

Browse Source 
The ros controller will timeout if it doesn't receive a cmd_vel message
in a given period, stopping the wheels.
webui sends messages at 100ms intervals, so the update rate needs to be 10Hz
This may need to be reviewed when using other input systems.
This commit is contained in:
Matt Spencer
2026-05-27 23:10:20 +00:00
parent ec554bcf2c
commit c6d02bbe43
2 changed files with 37 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
raspbot_v2/src/raspbot_v2_hardware_interface/src/raspbot_hardware_interface.cpp
+22 -1
View File
@@ -7,6 +7,7 @@
#include <algorithm>
#include <cerrno>
#include <chrono>
#include <cmath>
#include <cstring>
#include <string>
@@ -112,6 +113,8 @@ hardware_interface::return_type RaspbotHardwareInterface::read(
hardware_interface::return_type RaspbotHardwareInterface::write(
const rclcpp::Time &, const rclcpp::Duration &)
{
const std::array<double, 4> prev_commands = wheel_commands_;
wheel_commands_[0] = get_command<double>("joint_wheel_front_left/velocity");
wheel_commands_[1] = get_command<double>("joint_wheel_front_right/velocity");
wheel_commands_[2] = get_command<double>("joint_wheel_rear_left/velocity");
@@ -122,6 +125,14 @@ hardware_interface::return_type RaspbotHardwareInterface::write(
if (std::isnan(v)) {v = 0.0;}
}
static constexpr const char * kNames[] = {"FL", "FR", "RL", "RR"};
for (size_t i = 0; i < 4; ++i) {
if (prev_commands[i] != 0.0 && wheel_commands_[i] == 0.0) {
RCLCPP_WARN(get_logger(), "Wheel %s commanded to zero (was %.3f rad/s)",
kNames[i], prev_commands[i]);
}
}
ctrl_muto(MOTOR_FL, to_motor_speed(wheel_commands_[0]));
ctrl_muto(MOTOR_FR, to_motor_speed(wheel_commands_[1]));
ctrl_muto(MOTOR_RL, to_motor_speed(wheel_commands_[2]));
@@ -141,7 +152,17 @@ bool RaspbotHardwareInterface::ctrl_muto(int motor_id, int speed)
const uint8_t abs_speed = static_cast<uint8_t>(std::abs(clamped));
// Register 0x01: [motor_id, direction, speed] — mirrors Raspbot_Lib Ctrl_Muto
uint8_t buf[] = {0x01, static_cast<uint8_t>(motor_id), dir, abs_speed};
return ::write(i2c_fd_, buf, sizeof(buf)) == static_cast<ssize_t>(sizeof(buf));
const auto t0 = std::chrono::steady_clock::now();
const bool ok = ::write(i2c_fd_, buf, sizeof(buf)) == static_cast<ssize_t>(sizeof(buf));
const auto dt_us = std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::steady_clock::now() - t0).count();
if (dt_us > 1000) {
RCLCPP_WARN(get_logger(), "ctrl_muto motor %d: %ld µs", motor_id, dt_us);
}
return ok;
}
int RaspbotHardwareInterface::to_motor_speed(double rad_s) const