modelec/ros2_roboclaw_driver
1
0
Fork 0
mirror of https://github.com/modelec/ros2_roboclaw_driver.git synced 2026-03-19 00:30:29 +01:00
Code Issues Packages Projects Releases Wiki Activity

Added debugging control configuration parameters.

Browse Source 
Renamed joint states topic name.
This commit is contained in:
Michael Wimble
2025-03-22 20:26:34 -07:00
parent b9c51591c0
commit 5bcef39c94
5 changed files with 74 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
config/motor_driver.yaml
View File

@@ -55,3 +55,7 @@ motor_driver_node:
# Topic name to be used to publish the RoboClaw status messages.
roboclaw_status_topic: "roboclaw_status"
# Debugging control.
do_debug: false # True => Log RoboClaw commands and responses.
do_low_level_debug: false # True => Log low-level serial and RoboClaw data.

73
include/roboclaw.h
View File

@@ -12,19 +12,6 @@
#include <string>
#include "ros2_roboclaw_driver/srv/reset_encoders.hpp"
// #include "roboclaw_cmd.h"
// #include "roboclaw_cmd_do_buffered_m1m2_drive_speed_accel_distance.h"
// #include "roboclaw_cmd_read_encoder.h"
// #include "roboclaw_cmd_read_encoder_speed.h"
// #include "roboclaw_cmd_read_firmware_version.h"
// #include "roboclaw_cmd_read_logic_battery_voltage.h"
// #include "roboclaw_cmd_read_main_battery_voltage.h"
// #include "roboclaw_cmd_read_motor_currents.h"
// #include "roboclaw_cmd_read_motor_velocity_pidq.h"
// #include "roboclaw_cmd_read_status.h"
// #include "roboclaw_cmd_read_temperature.h"
// #include "roboclaw_cmd_set_encoder_value.h"
// #include "roboclaw_cmd_set_pid.h"
/* There are possibly several clients that want to read sensor data,
such as encoder values, motor speeds, etc. Each time an actual read
@@ -107,7 +94,8 @@ class RoboClaw {
// Constructor.
RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent,
float m2MaxCurrent, std::string device_name, uint8_t device_port,
uint32_t baud_rate);
uint32_t baud_rate, bool do_debug = false,
bool do_low_level_debug = false);
~RoboClaw();
@@ -194,10 +182,10 @@ class RoboClaw {
private:
// True => print debug messages.
bool doDebug_;
bool do_debug_;
// True => print byte values as they are read and written.
bool doLowLevelDebug_;
bool do_low_level_debug_;
// Get RoboClaw error status as a string.
std::string getErrorString(uint16_t errorStatus);
@@ -352,45 +340,52 @@ class RoboClaw {
static RoboClaw *g_singleton;
char command_log_[256];
char response_log_[256];
class DebugLog {
public:
DebugLog() : next_read_log_index_(0), next_write_log_index_(0) {}
DebugLog(RoboClaw *roboclaw)
: roboclaw_(roboclaw),
next_read_log_index_(0),
next_write_log_index_(0) {}
~DebugLog() {}
void appendToReadLog(const char *format, va_list args) {
int written =
vsnprintf(&read_log_[next_read_log_index_],
sizeof(read_log_) - next_read_log_index_, format, args);
if (written > 0) {
next_read_log_index_ += written;
if (roboclaw_->do_debug_) {
int written =
vsnprintf(&read_log_[next_read_log_index_],
sizeof(read_log_) - next_read_log_index_, format, args);
if (written > 0) {
next_read_log_index_ += written;
}
}
}
void appendToWriteLog(const char *format, va_list args) {
int written =
vsnprintf(&write_log_[next_write_log_index_],
sizeof(write_log_) - next_write_log_index_, format, args);
if (written > 0) {
next_write_log_index_ += written;
}
if ((next_write_log_index_ > 0) && (write_log_[0] == '8')) {
RCUTILS_LOG_INFO("Whoops!");
if (roboclaw_->do_debug_) {
int written =
vsnprintf(&write_log_[next_write_log_index_],
sizeof(write_log_) - next_write_log_index_, format, args);
if (written > 0) {
next_write_log_index_ += written;
}
if ((next_write_log_index_ > 0) && (write_log_[0] == '8')) {
RCUTILS_LOG_INFO("Whoops!");
}
}
}
void showLog() {
RCUTILS_LOG_INFO("[RoboClaw::DebugLog] %s, READ: %s", write_log_,
read_log_);
read_log_[0] = '\0';
next_read_log_index_ = 0;
write_log_[0] = '\0';
next_write_log_index_ = 0;
if (roboclaw_->do_debug_) {
RCUTILS_LOG_INFO("[RoboClaw::DebugLog] %s, READ: %s", write_log_,
read_log_);
read_log_[0] = '\0';
next_read_log_index_ = 0;
write_log_[0] = '\0';
next_write_log_index_ = 0;
}
}
// private:
RoboClaw *roboclaw_; // Pointer to the RoboClaw instance (if needed)
char read_log_[256];
char write_log_[256];
uint16_t next_read_log_index_;

21
src/motor_driver.cpp
View File

@@ -1,6 +1,5 @@
#include "motor_driver.h"
#include "roboclaw.h"
#include <math.h>
#include <rcutils/logging_macros.h>
@@ -17,12 +16,16 @@
#include "roboclaw.h"
MotorDriver::MotorDriver()
: Node("motor_driver_node"), device_name_("foo_bar"),
wheel_radius_(0.10169), wheel_separation_(0.345) {
: Node("motor_driver_node"),
device_name_("foo_bar"),
wheel_radius_(0.10169),
wheel_separation_(0.345) {
this->declare_parameter<int>("accel_quad_pulses_per_second", 600);
this->declare_parameter<int>("baud_rate", 38400);
this->declare_parameter<std::string>("device_name", "roboclaw");
this->declare_parameter<int>("device_port", 123);
this->declare_parameter<bool>("do_debug", false);
this->declare_parameter<bool>("do_low_level_debug", false);
this->declare_parameter<float>("m1_p", 0.0);
this->declare_parameter<float>("m1_i", 0.0);
this->declare_parameter<float>("m1_d", 0.0);
@@ -83,6 +86,8 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
this->get_parameter("baud_rate", baud_rate_);
this->get_parameter("device_name", device_name_);
this->get_parameter("device_port", device_port_);
this->get_parameter("do_debug", do_debug_);
this->get_parameter("do_low_level_debug", do_low_level_debug_);
this->get_parameter("m1_p", m1_p_);
this->get_parameter("m1_i", m1_i_);
this->get_parameter("m1_d", m1_d_);
@@ -111,6 +116,9 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
RCUTILS_LOG_INFO("baud_rate: %d", baud_rate_);
RCUTILS_LOG_INFO("device_name: %s", device_name_.c_str());
RCUTILS_LOG_INFO("device_port: %d", device_port_);
RCUTILS_LOG_INFO("do_debug: %s", do_debug_ ? "True" : "False");
RCUTILS_LOG_INFO("do_low_level_debug: %s",
do_low_level_debug_ ? "True" : "False");
RCUTILS_LOG_INFO("m1_p: %f", m1_p_);
RCUTILS_LOG_INFO("m1_i: %f", m1_i_);
RCUTILS_LOG_INFO("m1_d: %f", m1_d_);
@@ -137,7 +145,8 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
RoboClaw::TPIDQ m2Pid = {m2_p_, m2_i_, m2_d_, m2_qpps_, m2_max_current_};
new RoboClaw(m1Pid, m2Pid, m1_max_current_, m2_max_current_,
device_name_.c_str(), device_port_, baud_rate_);
device_name_.c_str(), device_port_, baud_rate_, do_debug_,
do_low_level_debug_);
RCUTILS_LOG_INFO("Main battery: %f",
RoboClaw::singleton()->getMainBatteryLevel());
@@ -153,8 +162,8 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
if (publish_joint_states_) {
joint_state_publisher_ =
this->create_publisher<sensor_msgs::msg::JointState>(
"puck_joint_states", qos);
this->create_publisher<sensor_msgs::msg::JointState>("joint_states",
qos);
}
if (publish_odom_) {

5
src/motor_driver.h
View File

@@ -28,7 +28,10 @@ class MotorDriver : public rclcpp::Node {
uint32_t baud_rate_;
std::string device_name_;
uint8_t device_port_;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
bool do_debug_;
bool do_low_level_debug_;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr
joint_state_publisher_;
float m1_p_;
float m1_i_;
float m1_d_;

34
src/roboclaw.cpp
View File

@@ -36,16 +36,18 @@ std::mutex RoboClaw::buffered_command_mutex_;
RoboClaw::RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent,
float m2MaxCurrent, std::string device_name,
uint8_t device_port, uint32_t baud_rate)
: doDebug_(true),
doLowLevelDebug_(true),
uint8_t device_port, uint32_t baud_rate, bool(do_debug),
bool do_low_level_debug)
: do_debug_(do_debug),
do_low_level_debug_(do_low_level_debug),
baud_rate_(baud_rate),
device_port_(device_port),
maxCommandRetries_(3),
maxM1Current_(m1MaxCurrent),
maxM2Current_(m2MaxCurrent),
device_name_(device_name),
portAddress_(128) {
portAddress_(128),
debug_log_(this) {
openPort();
RCUTILS_LOG_INFO("[RoboClaw::RoboClaw] RoboClaw software version: %s",
getVersion().c_str());
@@ -514,18 +516,10 @@ uint8_t RoboClaw::readByteWithTimeout2() {
"[RoboClaw::readByteWithTimeout2 Failed to read 1 byte");
}
static const bool kDEBUG_READBYTE = true;
if (doLowLevelDebug_) {
if (do_debug_ || do_low_level_debug_) {
appendToReadLog("%02X ", buffer[0]);
}
if (kDEBUG_READBYTE) {
if ((buffer[0] < 0x21) || (buffer[0] > 0x7F)) {
// RCUTILS_LOG_INFO("read ..> char: ?? 0x%02X <--", buffer[0]);
// fprintf(stderr, "..> char: ?? 0x%02X <--", buffer[0]);
} else {
// RCUTILS_LOG_INFO("read ..> char: %c 0x%02X <--", buffer[0],
// buffer[0]); fprintf(stderr, "..> char: %c 0x%02X <--", buffer[0],
// buffer[0]);
if (do_low_level_debug_) {
RCUTILS_LOG_INFO("Read: %02X", buffer[0]);
}
}
@@ -658,11 +652,17 @@ void RoboClaw::writeByte2(uint8_t byte) {
result = ::write(device_port_, &byte, 1);
// RCUTILS_LOG_INFO("--> wrote: 0x%02X, result: %ld", byte, result); //
// ####
if (doLowLevelDebug_) {
if (do_debug_ || do_low_level_debug_) {
if (result == 1) {
appendToWriteLog("%02X ", byte);
if (do_low_level_debug_) {
RCUTILS_LOG_INFO("Write: %02X", byte);
}
} else {
appendToWriteLog(">%02X< ", byte);
appendToWriteLog("~%02X ", byte);
if (do_low_level_debug_) {
RCUTILS_LOG_ERROR("Write fail: %02X", byte);
}
}
}