diff --git a/config/motor_driver.yaml b/config/motor_driver.yaml index fedd086..0f649ed 100755 --- a/config/motor_driver.yaml +++ b/config/motor_driver.yaml @@ -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. \ No newline at end of file diff --git a/include/roboclaw.h b/include/roboclaw.h index db2357a..c3b2ad3 100755 --- a/include/roboclaw.h +++ b/include/roboclaw.h @@ -12,19 +12,6 @@ #include #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_; diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp index 04af636..7dd8f26 100755 --- a/src/motor_driver.cpp +++ b/src/motor_driver.cpp @@ -1,6 +1,5 @@ #include "motor_driver.h" -#include "roboclaw.h" #include #include @@ -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("accel_quad_pulses_per_second", 600); this->declare_parameter("baud_rate", 38400); this->declare_parameter("device_name", "roboclaw"); this->declare_parameter("device_port", 123); + this->declare_parameter("do_debug", false); + this->declare_parameter("do_low_level_debug", false); this->declare_parameter("m1_p", 0.0); this->declare_parameter("m1_i", 0.0); this->declare_parameter("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( - "puck_joint_states", qos); + this->create_publisher("joint_states", + qos); } if (publish_odom_) { diff --git a/src/motor_driver.h b/src/motor_driver.h index 9067a92..4ee9d64 100755 --- a/src/motor_driver.h +++ b/src/motor_driver.h @@ -28,7 +28,10 @@ class MotorDriver : public rclcpp::Node { uint32_t baud_rate_; std::string device_name_; uint8_t device_port_; - rclcpp::Publisher::SharedPtr joint_state_publisher_; + bool do_debug_; + bool do_low_level_debug_; + rclcpp::Publisher::SharedPtr + joint_state_publisher_; float m1_p_; float m1_i_; float m1_d_; diff --git a/src/roboclaw.cpp b/src/roboclaw.cpp index 13b8f33..59e5d05 100755 --- a/src/roboclaw.cpp +++ b/src/roboclaw.cpp @@ -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); + } } }