modelec/ros2_roboclaw_driver
1
0
Fork 0
mirror of https://github.com/modelec/ros2_roboclaw_driver.git synced 2026-01-18 16:47:26 +01:00
Code Issues Packages Projects Releases Wiki Activity

Snapshot

Browse Source
This commit is contained in:
Michael Wimble
2025-03-18 18:33:41 -07:00
parent a14b590112
commit ffeec2f77f
3 changed files with 602 additions and 415 deletions
Download Patch File Download Diff File Expand all files Collapse all files

152
include/roboclaw.h
View File

@@ -1,10 +1,13 @@
#pragma once
#include <cstdint>
#include <rcutils/logging_macros.h>
#include <chrono>
#include <rclcpp/logger.hpp>
#include <sstream>
#include <string>
#include <sys/types.h>
#include "ros2_roboclaw_driver/srv/reset_encoders.hpp"
@@ -26,8 +29,6 @@
#define SetDWORDval(arg) \
(uint8_t)(arg >> 24), (uint8_t)(arg >> 16), (uint8_t)(arg >> 8), (uint8_t)arg
#define kDebugIO true
class RoboClaw {
public:
// Bit positions used to build alarms.
@@ -86,10 +87,17 @@ public:
~RoboClaw();
void appendToLog(const char *format, ...) {
void appendToReadLog(const char *format, ...) {
va_list args;
va_start(args, format);
debug_log_.appendToLog(format, args);
debug_log_.appendToReadLog(format, args);
va_end(args);
}
void appendToWriteLog(const char *format, ...) {
va_list args;
va_start(args, format);
debug_log_.appendToWriteLog(format, args);
va_end(args);
}
@@ -158,6 +166,12 @@ protected:
void writeN2(bool sendCRC, uint8_t cnt, ...);
private:
// True => print debug messages.
bool doDebug_;
// True => print byte values as they are read and written.
bool doLowLevelDebug_;
EncodeResult cache_getEncoderCommandResult(WHICH_ENC command);
// Get RoboClaw error status bits.
@@ -318,6 +332,9 @@ private:
// Read one byte from device with timeout.
uint8_t readByteWithTimeout();
// Read one byte from device with timeout.
uint8_t readByteWithTimeout2();
// Perform error recovery to re-open a failed device port.
void restartPort();
@@ -390,25 +407,40 @@ private:
class DebugLog {
public:
DebugLog() : next_log_index_(0) {}
DebugLog() : next_read_log_index_(0), next_write_log_index_(0) {}
~DebugLog() {}
void appendToLog(const char *format, va_list args) {
int written = vsnprintf(&log_[next_log_index_],
sizeof(log_) - next_log_index_, format, args);
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_log_index_ += written;
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;
}
}
void showLog() {
RCUTILS_LOG_INFO("%s", log_);
next_log_index_ = 0;
RCUTILS_LOG_INFO("%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:
char log_[256];
uint16_t next_log_index_;
char read_log_[256];
char write_log_[256];
uint16_t next_read_log_index_;
uint16_t next_write_log_index_;
};
class CmdSetPid : public Cmd {
@@ -418,9 +450,10 @@ private:
: Cmd(roboclaw, "SetPid", motor), p_(p), i_(i), d_(d), qpps_(qpps) {}
void send() override {
roboclaw_.appendToLog("SetPid: motor: %d (%s) p: %f, i: %f, d: %f, "
"qpps: %d, bytes: ",
motor_, motorNames_[motor_], p_, i_, d_, qpps_);
roboclaw_.appendToWriteLog("SetPid: motor: %d (%s) p: %f, i: %f, d: %f, "
"qpps: %d, WROTE: ",
motor_, motorNames_[motor_], p_, i_, d_,
qpps_);
uint32_t kp = int(p_ * 65536.0);
uint32_t ki = int(i_ * 65536.0);
uint32_t kd = int(d_ * 65536.0);
@@ -436,13 +469,96 @@ private:
uint32_t qpps_;
};
class CmdReadFirmwareVersion : public Cmd {
public:
CmdReadFirmwareVersion(RoboClaw &roboclaw, std::string &version)
: Cmd(roboclaw, "ReadFirmwareVersion", kNone), version_(version) {}
void send() override {
roboclaw_.appendToWriteLog("ReadFirmwareVersion: WROTE: ");
roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, kGETVERSION);
try {
uint16_t crc = 0;
uint8_t i;
uint8_t datum;
std::stringstream version;
roboclaw_.updateCrc(crc, roboclaw_.portAddress_);
roboclaw_.updateCrc(crc, kGETVERSION);
for (i = 0; i < 48; i++) {
datum = roboclaw_.readByteWithTimeout2();
roboclaw_.updateCrc(crc, datum);
if (datum == 0) {
uint16_t responseCrc = 0;
datum = roboclaw_.readByteWithTimeout2();
responseCrc = datum << 8;
datum = roboclaw_.readByteWithTimeout2();
responseCrc |= datum;
if (responseCrc == crc) {
version_ = version.str();
// roboclaw_.debug_log_.appendToReadLog("RR%d", 0);
// roboclaw_.debug_log_.appendToWriteLog("WW%d", 0);
roboclaw_.debug_log_.showLog();
return;
} else {
roboclaw_.debug_log_.showLog();
RCUTILS_LOG_ERROR("[RoboClaw::CmdReadFirmwareVersion] invalid "
"CRC expected: 0x%02X, "
"got: 0x%02X",
crc, responseCrc);
}
} else {
version << (char)datum;
}
}
RCUTILS_LOG_ERROR(
"[RoboClaw::CmdReadFirmwareVersion] unexpected long string");
throw new TRoboClawException(
"[RoboClaw::getVersion] unexpected long string");
} catch (...) {
RCUTILS_LOG_ERROR(
"[RoboClaw::CmdReadFirmwareVersion] Uncaught exception !!!");
}
RCUTILS_LOG_ERROR(
"[RoboClaw::CmdReadFirmwareVersion] RETRY COUNT EXCEEDED");
}
std::string &version_;
};
class CmdSetEncoderValue : public Cmd {
public:
CmdSetEncoderValue(RoboClaw &roboclaw, kMotor motor, long value)
: Cmd(roboclaw, "SetEncoderValue", motor), value_(value) {}
void send() override {
roboclaw_.appendToWriteLog(
"SetEncoderValue: motor: %d (%s) value: %ld, WROTE: ", motor_,
motorNames_[motor_], value_);
// roboclaw_.SetEncoder(motor_ == kM1 ? kSETM1ENCODER : kSETM2ENCODER,
// value_);
try {
roboclaw_.writeN2(true, 6, roboclaw_.portAddress_,
motor_ == kM1 ? kSETM1ENCODER : kSETM2ENCODER,
SetDWORDval(value_));
roboclaw_.debug_log_.showLog();
return;
} catch (...) {
RCUTILS_LOG_ERROR(
"[RoboClaw::CmdSetEncoderValue] Uncaught exception !!!");
}
}
long value_;
};
friend class Cmd; // Make Cmd a friend class of RoboClaw
protected:
DebugLog debug_log_;
#ifdef kDebugIO
static const char *commandNames_[];
static const char *motorNames_[];
#endif
};

101
src/CMakeLists.txt Normal file
View File

@@ -0,0 +1,101 @@
cmake_minimum_required(VERSION 3.5)
project(ros2_roboclaw_driver)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON) # Ensure the standard is enforced
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
add_compile_options(-g)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs)
find_package(rclcpp REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
include_directories(
${geometry_msgs_INCLUDE_DIRS}
${nav_msgs_INCLUDE_DIRS}
${rclcpp_INCLUDE_DIRS}
${sensor_msgs_INCLUDE_DIRS}
${std_msgs_INCLUDE_DIRS}
${CMAKE_CURRENT_SOURCE_DIR}/include
/usr/include # Ensure standard system headers are included
)
set(msg_files
"msg/RoboClawStatus.msg"
)
set(srv_files
"srv/ResetEncoders.srv"
)
rosidl_generate_interfaces(${PROJECT_NAME}
${msg_files}
${srv_files}
)
rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
add_executable(
ros2_roboclaw_driver_node
src/motor_driver_node.cpp
src/motor_driver.cpp
src/roboclaw.cpp
)
ament_target_dependencies(
ros2_roboclaw_driver_node
rclcpp
geometry_msgs
nav_msgs
sensor_msgs
std_msgs
)
target_link_libraries(ros2_roboclaw_driver_node "${cpp_typesupport_target}")
# Install config files.
install(DIRECTORY
config
DESTINATION share/${PROJECT_NAME}/
)
# Install launch files.
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}/
)
install(TARGETS
ros2_roboclaw_driver_node
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
# set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
# set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_export_dependencies(rosidl_default_runtime)
ament_package()

764
src/roboclaw.cpp
View File

@@ -7,7 +7,6 @@
#include <stdio.h>
#include <sys/types.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>
#include <boost/assign.hpp>
@@ -17,283 +16,285 @@
#include "ros2_roboclaw_driver/srv/reset_encoders.h"
#if kDebugIO
const char *RoboClaw::motorNames_[] = {"M1", "M2", "NONE"};
const char *RoboClaw::commandNames_[] = {
"Drive Forward M1", // 0
"Drive Backward M1", // 1
"Set Min MB", // 2
"Set Max MB", // 3
"Drive Forward M2", // 4
"Drive Backward M2", // 5
"Drive 7 Bit M1", // 6
"Drive 7 Bit M2", // 7
"Drive Forward Mixed", // 8
"Drive Backward Mixed", // 9
"Turn Right Mixed", // 10
"Turn Left Mixed", // 11
"Drive FB Mixed", // 12
"Drive LR Mixed", // 13
"Set Serial Timeout", // 14
"Read Serial Timeout", // 15
"16",
"17",
"18",
"19",
"20",
"Read Firmware Version", // 21
"22",
"23",
"Read Main Battery Voltage", // 24
"Read Logic Battery Voltage", // 25
"Set Min LB", // 26
"Set Max LB", // 27
"28",
"29",
"30",
"31",
"32",
"33",
"34",
"35",
"36",
"37",
"38",
"39",
"40",
"41",
"42",
"43",
"44",
"45",
"46",
"47",
"Read Motor PWMs", // 48
"Read Motor Currents", // 49
"50",
"51",
"52",
"53",
"54",
"55",
"56",
"Set Main Battery Volages", // 57
"Set Logic Battery Voltages", // 58
"Read Main Battery Voltages", // 59
"Read Logic Battery Voltages", // 60
"61",
"62",
"63",
"64",
"65",
"66",
"67",
"Set Default Duty Cycle Accel M1", // 68
"Set Default Duty Cycle Accel M2", // 69
"Set Default Speed M1", // 70
"Set Default Speed M2", // 71
"Read Default Speeds", // 72
"73",
"Set S3, S4, S5 Modes", // 74
"Read S3, S4, S5 Modes", // 75
"Set Deadband", // 76
"Read Deadband", // 77
"78",
"79",
"Restore Defaults", // 80
"Read Default Duty Cycle Accels", // 81
"Read Temperature", // 82
"Read Temperature 2", // 83
"84",
"85",
"86",
"87",
"88",
"89",
"Read Status", // 90
"Read Encoder Modes", // 91
"Set M1 Encoder Mode", // 92
"Set M2 Encoder Mode", // 93
"Write Settings to EEPROM", // 94
"Read Settings from EEPROM", // 95
"96",
"97",
"Set Standard Config", // 98
"Read Standard Config", // 99
"Set CTRL Modes", // 100
"Read CTRL Modes", // 101
"Set CTRL1", // 102
"Set CTRL2", // 103
"Read CTRLs", // 104
"Set Auto Home Duty/Speet/Timeout M1", // 105
"Set Auto Home Duty/Speet/Timeout M2", // 106
"Read Auto Home", // 107
"108",
"Set Speed Error Limits", // 109
"Read Speed Error Limits", // 110
"111",
"Set Speed Position Error Limits", // 112
"Read Speed Position Error Limits", // 113
"114",
"Set Battery Voltage Offsets", // 115
"Read Battery Voltage Offsets", // 116
"Set Current Blanking %%s", // 117
"Read Current Blanking %%s", // 118
"119",
"120",
"121",
"122",
"123",
"124",
"125",
"126",
"127",
"128",
"129",
"130",
"131",
"132",
"Set M1 Max Current", // 133
"Set M2 Max Current", // 134
"Read M1 Max Current", // 135
"Read M2 Max Current", // 136
"137",
"138",
"139",
"140",
"141",
"142",
"143",
"144",
"145",
"146",
"147",
"Set PWM Mode", // 148
"Read PWM Mode", // 149
"150",
"151",
"152",
"153",
"154",
"155",
"156",
"157",
"158",
"159",
"160",
"161",
"162",
"163",
"164",
"165",
"166",
"167",
"168",
"169",
"170",
"171",
"172",
"173",
"174",
"175",
"176",
"177",
"178",
"179",
"180",
"181",
"182",
"183",
"184",
"185",
"186",
"187",
"188",
"189",
"190",
"191",
"192",
"193",
"194",
"195",
"196",
"197",
"198",
"199",
"200",
"201",
"202",
"203",
"204",
"205",
"206",
"207",
"208",
"209",
"210",
"211",
"212",
"213",
"214",
"215",
"216",
"217",
"218",
"219",
"220",
"221",
"222",
"223",
"224",
"225",
"226",
"227",
"228",
"229",
"230",
"231",
"232",
"233",
"234",
"235",
"236",
"237",
"238",
"239",
"240",
"241",
"242",
"243",
"244",
"245",
"246",
"247",
"248",
"249",
"250",
"251",
"Read User EEPROM Memory Location", // 252
"Write User EEPROM Memory Location", // 253
"254",
"255"};
#endif
const char *RoboClaw::commandNames_[] = {"0",
"1",
"2",
"3",
"4",
"5",
"6",
"7",
"8",
"9",
"10",
"11",
"12",
"13",
"14",
"15",
"16",
"17",
"18",
"19",
"20",
"Read Firmware Version",
"22",
"23",
"24",
"25",
"26",
"27",
"Set Velocity PID M1",
"Set Velocity PID M2",
"30",
"31",
"32",
"33",
"34",
"35",
"36",
"37",
"38",
"39",
"40",
"41",
"42",
"43",
"44",
"45",
"46",
"47",
"48",
"49",
"50",
"51",
"52",
"53",
"54",
"55",
"56",
"57",
"58",
"59",
"60",
"61",
"62",
"63",
"64",
"65",
"66",
"67",
"68",
"69",
"70",
"71",
"72",
"73",
"74",
"75",
"76",
"77",
"78",
"79",
"80",
"81",
"82",
"83",
"84",
"85",
"86",
"87",
"88",
"89",
"90",
"91",
"92",
"93",
"94",
"95",
"96",
"97",
"98",
"99",
"100",
"101",
"102",
"103",
"104",
"105",
"106",
"107",
"108",
"109",
"110",
"111",
"112",
"113",
"114",
"115",
"116",
"117",
"118",
"119",
"120",
"121",
"122",
"123",
"124",
"125",
"126",
"127",
"128",
"129",
"130",
"131",
"132",
"133",
"134",
"135",
"136",
"137",
"138",
"139",
"140",
"141",
"142",
"143",
"144",
"145",
"146",
"147",
"148",
"149",
"150",
"151",
"152",
"153",
"154",
"155",
"156",
"157",
"158",
"159",
"160",
"161",
"162",
"163",
"164",
"165",
"166",
"167",
"168",
"169",
"170",
"171",
"172",
"173",
"174",
"175",
"176",
"177",
"178",
"179",
"180",
"181",
"182",
"183",
"184",
"185",
"186",
"187",
"188",
"189",
"190",
"191",
"192",
"193",
"194",
"195",
"196",
"197",
"198",
"199",
"200",
"201",
"202",
"203",
"204",
"205",
"206",
"207",
"208",
"209",
"210",
"211",
"212",
"213",
"214",
"215",
"216",
"217",
"218",
"219",
"220",
"221",
"222",
"223",
"224",
"225",
"226",
"227",
"228",
"229",
"230",
"231",
"232",
"233",
"234",
"235",
"236",
"237",
"238",
"239",
"240",
"241",
"242",
"243",
"244",
"245",
"246",
"247",
"248",
"249",
"250",
"251",
"252",
"253",
"254",
"255"};
RoboClaw::RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent,
float m2MaxCurrent, std::string device_name,
uint8_t device_port)
: device_port_(device_port), maxCommandRetries_(3),
maxM1Current_(m1MaxCurrent), maxM2Current_(m2MaxCurrent),
device_name_(device_name), portAddress_(128) {
: doDebug_(true), doLowLevelDebug_(true), device_port_(device_port),
maxCommandRetries_(3), maxM1Current_(m1MaxCurrent),
maxM2Current_(m2MaxCurrent), device_name_(device_name),
portAddress_(128) {
openPort();
RCUTILS_LOG_INFO("[RoboClaw::RoboClaw] RoboClaw software version: %s",
getVersion().c_str());
setM1PID(m1Pid.p, m1Pid.i, m1Pid.d, m1Pid.qpps);
setM2PID(m2Pid.p, m2Pid.i, m2Pid.d, m2Pid.qpps);
ros2_roboclaw_driver::srv::ResetEncoders::Request resetRequest;
resetRequest.left = 0;
resetRequest.right = 0;
ros2_roboclaw_driver::srv::ResetEncoders::Response response;
resetEncoders(resetRequest, response);
CmdSetEncoderValue m1(*this, kM1, 0);
m1.execute();
CmdSetEncoderValue m2(*this, kM2, 0);
m2.execute();
// ros2_roboclaw_driver::srv::ResetEncoders::Request resetRequest;
// resetRequest.left = 0;
// resetRequest.right = 0;
// ros2_roboclaw_driver::srv::ResetEncoders::Response response;
// resetEncoders(resetRequest, response);
g_singleton = this;
readSensorGroup();
}
@@ -917,55 +918,10 @@ int32_t RoboClaw::cache_getM2Encoder() {
}
std::string RoboClaw::getVersion() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, kGETVERSION);
writeN(false, 2, portAddress_, kGETVERSION);
uint8_t i;
uint8_t datum;
std::stringstream version;
for (i = 0; i < 48; i++) {
datum = readByteWithTimeout();
updateCrc(crc, datum);
if (datum == 0) {
uint16_t responseCrc = 0;
datum = readByteWithTimeout();
responseCrc = datum << 8;
datum = readByteWithTimeout();
responseCrc |= datum;
if (responseCrc == crc) {
return version.str();
} else {
RCUTILS_LOG_ERROR(
"[RoboClaw::getVersion] invalid CRC expected: 0x%02X, "
"got: 0x%02X",
crc, responseCrc);
}
} else {
version << (char)datum;
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getVersion] unexpected long string");
throw new TRoboClawException(
"[RoboClaw::getVersion] unexpected long string");
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::getVersion] Exception: %s, retry number: %d", e->what(),
retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::getVersion] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::getVersion] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::getVersion] RETRY COUNT EXCEEDED");
std::string version;
CmdReadFirmwareVersion command(*this, version);
command.execute();
return version;
}
void RoboClaw::openPort() {
@@ -1100,11 +1056,70 @@ uint8_t RoboClaw::readByteWithTimeout() {
static const bool kDEBUG_READBYTE = true;
if (kDEBUG_READBYTE) {
if ((buffer[0] < 0x21) || (buffer[0] > 0x7F)) {
RCUTILS_LOG_INFO("read ..> char: ?? 0x%02X <--", buffer[0]);
// 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]);
// RCUTILS_LOG_INFO("read ..> char: %c 0x%02X <--", buffer[0],
// buffer[0]); fprintf(stderr, "..> char: %c 0x%02X <--", buffer[0],
// buffer[0]);
}
}
return buffer[0];
} else {
RCUTILS_LOG_ERROR("[RoboClaw::readByteWithTimeout] Unhandled case");
throw new TRoboClawException(
"[RoboClaw::readByteWithTimeout] Unhandled case");
}
return 0;
}
uint8_t RoboClaw::readByteWithTimeout2() {
struct pollfd ufd[1];
ufd[0].fd = device_port_;
ufd[0].events = POLLIN;
int retval = poll(ufd, 1, 11);
if (retval < 0) {
RCUTILS_LOG_ERROR("[RoboClaw::readByteWithTimeout] Poll failed (%d) %s",
errno, strerror(errno));
throw new TRoboClawException("[RoboClaw::readByteWithTimeout] Read error");
} else if (retval == 0) {
std::stringstream ev;
ev << "[RoboClaw::readByteWithTimeout] TIMEOUT revents: " << std::hex
<< ufd[0].revents;
RCUTILS_LOG_ERROR(ev.str().c_str());
throw new TRoboClawException("[RoboClaw::readByteWithTimeout] TIMEOUT");
} else if (ufd[0].revents & POLLERR) {
RCUTILS_LOG_ERROR("[RoboClaw::readByteWithTimeout] Error on socket");
restartPort();
throw new TRoboClawException(
"[RoboClaw::readByteWithTimeout] Error on socket");
} else if (ufd[0].revents & POLLIN) {
unsigned char buffer[1];
ssize_t bytesRead = ::read(device_port_, buffer, sizeof(buffer));
if (bytesRead != 1) {
RCUTILS_LOG_ERROR(
"[RoboClaw::readByteWithTimeout] Failed to read 1 byte, read: "
"%d",
(int)bytesRead);
throw TRoboClawException(
"[RoboClaw::readByteWithTimeout] Failed to read 1 byte");
}
static const bool kDEBUG_READBYTE = true;
if (doLowLevelDebug_) {
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]);
}
}
@@ -1154,8 +1169,12 @@ bool RoboClaw::resetEncoders(
ros2_roboclaw_driver::srv::ResetEncoders::Request &request,
ros2_roboclaw_driver::srv::ResetEncoders::Response &response) {
try {
SetEncoder(kSETM1ENCODER, request.left);
SetEncoder(kSETM2ENCODER, request.right);
CmdSetEncoderValue m1(*this, kM1, request.left);
CmdSetEncoderValue m2(*this, kM2, request.right);
m1.execute();
m2.execute();
// SetEncoder(kSETM1ENCODER, request.left);
// SetEncoder(kSETM2ENCODER, request.right);
response.ok = true;
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::resetEncoders] uncaught exception");
@@ -1198,51 +1217,11 @@ void RoboClaw::SetEncoder(ROBOCLAW_COMMAND command, long value) {
void RoboClaw::setM1PID(float p, float i, float d, uint32_t qpps) {
CmdSetPid command(*this, kM1, p, i, d, qpps);
command.execute();
// int retry;
// for (retry = 0; retry < maxCommandRetries_; retry++) {
// try {
// uint32_t kp = int(p * 65536.0);
// uint32_t ki = int(i * 65536.0);
// uint32_t kd = int(d * 65536.0);
// writeN(true, 18, portAddress_, kSETM1PID, SetDWORDval(kd),
// SetDWORDval(kp), SetDWORDval(ki), SetDWORDval(qpps));
// return;
// } catch (TRoboClawException *e) {
// RCUTILS_LOG_ERROR("[RoboClaw::setM1PID] Exception: %s, retry number:
// %d",
// e->what(), retry);
// } catch (...) {
// RCUTILS_LOG_ERROR("[RoboClaw::setM1PID] Uncaught exception !!!");
// }
// }
// RCUTILS_LOG_ERROR("[RoboClaw::setM1PID] RETRY COUNT EXCEEDED");
// throw new TRoboClawException("[RoboClaw::setM1PID] RETRY COUNT EXCEEDED");
}
void RoboClaw::setM2PID(float p, float i, float d, uint32_t qpps) {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint32_t kp = int(p * 65536.0);
uint32_t ki = int(i * 65536.0);
uint32_t kd = int(d * 65536.0);
writeN(true, 18, portAddress_, kSETM2PID, SetDWORDval(kd),
SetDWORDval(kp), SetDWORDval(ki), SetDWORDval(qpps));
return;
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR("[RoboClaw::setM2PID] Exception: %s, retry number: %d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR("[RoboClaw::setM2PID] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::setM2PID] RETRY COUNT EXCEEDED");
throw new TRoboClawException("[RoboClaw::setM2PID] RETRY COUNT EXCEEDED");
CmdSetPid command(*this, kM2, p, i, d, qpps);
command.execute();
}
void RoboClaw::stop() {
@@ -1298,14 +1277,15 @@ void RoboClaw::writeByte2(uint8_t byte) {
ssize_t result;
do {
result = ::write(device_port_, &byte, 1);
// RCUTILS_LOG_INFO("--> wrote: 0x%02X, result: %ld", byte, result); // ####
#if kDebugIO
if (result == 1) {
appendToLog("%02X ", byte);
} else {
appendToLog(">%02X< ", byte);
// RCUTILS_LOG_INFO("--> wrote: 0x%02X, result: %ld", byte, result); //
// ####
if (doLowLevelDebug_) {
if (result == 1) {
appendToWriteLog("%02X ", byte);
} else {
appendToWriteLog(">%02X< ", byte);
}
}
#endif
} while (result == -1 && errno == EAGAIN);
@@ -1331,15 +1311,17 @@ void RoboClaw::writeN(bool sendCRC, uint8_t cnt, ...) {
for (uint8_t i = 0; i < cnt; i++) {
uint8_t byte = va_arg(marker, int);
if (i <= 1) {
snprintf(msg, sizeof(msg),
"RoboClaw::writeN] sendCRC: %d, cnt: %d, byte[%d]: %d", sendCRC,
cnt, i, byte);
RCUTILS_LOG_INFO("%s", msg);
if (i == 1) {
snprintf(msg, sizeof(msg), "[RoboClaw::writeN] command: %s",
commandNames_[byte]);
if (doLowLevelDebug_) {
if (i <= 1) {
snprintf(msg, sizeof(msg),
"RoboClaw::writeN2] sendCRC: %d, cnt: %d, byte[%d]: %d",
sendCRC, cnt, i, byte);
RCUTILS_LOG_INFO("%s", msg);
if (i == 1) {
snprintf(msg, sizeof(msg), "[RoboClaw::writeN2] command: %s",
commandNames_[byte]);
RCUTILS_LOG_INFO("%s", msg);
}
}
}
@@ -1355,10 +1337,11 @@ void RoboClaw::writeN(bool sendCRC, uint8_t cnt, ...) {
uint8_t response = readByteWithTimeout();
if (response != 0xFF) {
snprintf(msg, sizeof(msg),
"[RoboClaw::writeN] Invalid ACK response, expected 0xFF but got "
"0x%02X",
response);
snprintf(
msg, sizeof(msg),
"[RoboClaw::writeN2] Invalid ACK response, expected 0xFF but got "
"0x%02X",
response);
RCUTILS_LOG_ERROR("%s", msg);
throw new TRoboClawException(msg);
}
@@ -1371,25 +1354,12 @@ void RoboClaw::writeN2(bool sendCRC, uint8_t cnt, ...) {
uint16_t crc = 0;
va_list marker;
va_start(marker, cnt);
char msg[128];
// int origFlags = fcntl(device_port_, F_GETFL, 0);
// fcntl(device_port_, F_SETFL, origFlags & ~O_NONBLOCK);
for (uint8_t i = 0; i < cnt; i++) {
uint8_t byte = va_arg(marker, int);
if (i <= 1) {
snprintf(msg, sizeof(msg),
"RoboClaw::writeN2] sendCRC: %d, cnt: %d, byte[%d]: %d", sendCRC,
cnt, i, byte);
RCUTILS_LOG_INFO("%s", msg);
if (i == 1) {
snprintf(msg, sizeof(msg), "[RoboClaw::writeN2] command: %s",
commandNames_[byte]);
RCUTILS_LOG_INFO("%s", msg);
}
}
updateCrc(crc, byte);
writeByte2(byte);
}
@@ -1400,7 +1370,7 @@ void RoboClaw::writeN2(bool sendCRC, uint8_t cnt, ...) {
writeByte2(crc >> 8);
writeByte2(crc);
uint8_t response = readByteWithTimeout();
uint8_t response = readByteWithTimeout2();
if (response != 0xFF) {
char msg[128];
snprintf(