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

Added joint state publisher. Still haven't added all the code for odom publisher

Browse Source
This commit is contained in:
Michael Wimble
2022-06-24 17:47:40 -07:00
parent 76a84b033a
commit cee9d23139
7 changed files with 492 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@@ -24,6 +24,7 @@ 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)
set(msg_files
@@ -37,7 +38,6 @@ set(srv_files
rosidl_generate_interfaces(${PROJECT_NAME}
${msg_files}
${srv_files}
# DEPENDENCIES builtin_interfaces std_msgs
)
add_executable(
@@ -52,6 +52,7 @@ ament_target_dependencies(
rclcpp
geometry_msgs
nav_msgs
sensor_msgs
std_msgs
)

3
config/motor_driver.yaml
View File

@@ -39,6 +39,9 @@ motor_driver_node:
# command in this number of seconds, stop the motors.
max_seconds_uncommanded_travel: 0.25
publish_joint_states: true
publish_odom: true
# Based upon your particular motor gearing and encoders.
# These values are used to scale cmd_vel commands
# and encoder values to motor commands and odometry, respectfully.

130
src/motor_driver.cpp
View File

@@ -6,6 +6,7 @@
#include <stdint.h>
#include <algorithm>
#include <chrono>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/rclcpp.hpp>
@@ -34,6 +35,8 @@ MotorDriver::MotorDriver()
this->declare_parameter<float>("max_linear_velocity", 0.0);
this->declare_parameter<float>("m2_max_current", 0.0);
this->declare_parameter<float>("max_seconds_uncommanded_travel", 0.0);
this->declare_parameter<bool>("publish_joint_states", true);
this->declare_parameter<bool>("publish_odom", true);
this->declare_parameter<int>("quad_pulses_per_meter", 0);
this->declare_parameter<int>("quad_pulses_per_revolution", 0);
this->declare_parameter<int>("vmin", 1);
@@ -44,31 +47,33 @@ MotorDriver::MotorDriver()
void MotorDriver::cmdVelCallback(
const geometry_msgs::msg::Twist::SharedPtr msg) const {
double x_velocity =
std::min(std::max((float)msg->linear.x, -max_linear_velocity_),
max_linear_velocity_);
double yaw_velocity =
std::min(std::max((float)msg->angular.z, -max_angular_velocity_),
max_angular_velocity_);
if ((msg->linear.x == 0) && (msg->angular.z == 0)) {
roboclaw_->doMixedSpeedDist(0, 0, 0, 0);
} else if ((fabs(x_velocity) > 0.01) || (fabs(yaw_velocity) > 0.01)) {
const double m1_desired_velocity =
x_velocity - (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
const double m2_desired_velocity =
x_velocity + (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
if (RoboClaw::singleton() != nullptr) {
double x_velocity =
std::min(std::max((float)msg->linear.x, -max_linear_velocity_),
max_linear_velocity_);
double yaw_velocity =
std::min(std::max((float)msg->angular.z, -max_angular_velocity_),
max_angular_velocity_);
if ((msg->linear.x == 0) && (msg->angular.z == 0)) {
RoboClaw::singleton()->doMixedSpeedDist(0, 0, 0, 0);
} else if ((fabs(x_velocity) > 0.01) || (fabs(yaw_velocity) > 0.01)) {
const double m1_desired_velocity =
x_velocity - (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
const double m2_desired_velocity =
x_velocity + (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
const int32_t m1_quad_pulses_per_second =
m1_desired_velocity * quad_pulses_per_meter_;
const int32_t m2_quad_pulses_per_second =
m2_desired_velocity * quad_pulses_per_meter_;
const int32_t m1_max_distance =
fabs(m1_quad_pulses_per_second * max_seconds_uncommanded_travel_);
const int32_t m2_max_distance =
fabs(m2_quad_pulses_per_second * max_seconds_uncommanded_travel_);
roboclaw_->doMixedSpeedAccelDist(
accel_quad_pulses_per_second_, m1_quad_pulses_per_second,
m1_max_distance, m2_quad_pulses_per_second, m2_max_distance);
const int32_t m1_quad_pulses_per_second =
m1_desired_velocity * quad_pulses_per_meter_;
const int32_t m2_quad_pulses_per_second =
m2_desired_velocity * quad_pulses_per_meter_;
const int32_t m1_max_distance =
fabs(m1_quad_pulses_per_second * max_seconds_uncommanded_travel_);
const int32_t m2_max_distance =
fabs(m2_quad_pulses_per_second * max_seconds_uncommanded_travel_);
RoboClaw::singleton()->doMixedSpeedAccelDist(
accel_quad_pulses_per_second_, m1_quad_pulses_per_second,
m1_max_distance, m2_quad_pulses_per_second, m2_max_distance);
}
}
}
@@ -93,6 +98,8 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
this->get_parameter("max_linear_velocity", max_linear_velocity_);
this->get_parameter("max_seconds_uncommanded_travel",
max_seconds_uncommanded_travel_);
this->get_parameter("publish_joint_states", publish_joint_states_);
this->get_parameter("publish_dom", publish_odom_);
this->get_parameter("quad_pulses_per_meter", quad_pulses_per_meter_);
this->get_parameter("quad_pulses_per_revolution",
quad_pulses_per_revolution_);
@@ -119,6 +126,8 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
RCUTILS_LOG_INFO("max_linear_velocity: %f", max_linear_velocity_);
RCUTILS_LOG_INFO("max_seconds_uncommanded_travel: %f",
max_seconds_uncommanded_travel_);
RCUTILS_LOG_INFO("publish_joint_states: %s",
publish_joint_states_ ? "True" : "False");
RCUTILS_LOG_INFO("quad_pulses_per_meter: %d", quad_pulses_per_meter_);
RCUTILS_LOG_INFO("quad_pulses_per_revolution: %d",
quad_pulses_per_revolution_);
@@ -130,18 +139,75 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
RoboClaw::TPIDQ m1Pid = {m1_p_, m1_i_, m1_d_, m1_qpps_, m1_max_current_};
RoboClaw::TPIDQ m2Pid = {m2_p_, m2_i_, m2_d_, m2_qpps_, m2_max_current_};
roboclaw_ = new RoboClaw(m1Pid, m2Pid, m1_max_current_, m2_max_current_,
device_name_.c_str(), device_port_, vmin_, vtime_);
RCUTILS_LOG_INFO("Main battery: %f", roboclaw_->getMainBatteryLevel());
new RoboClaw(m1Pid, m2Pid, m1_max_current_, m2_max_current_,
device_name_.c_str(), device_port_, vmin_, vtime_);
RCUTILS_LOG_INFO("Main battery: %f",
RoboClaw::singleton()->getMainBatteryLevel());
rclcpp::QoS bestEffortQos(10);
bestEffortQos.keep_last(10);
bestEffortQos.best_effort();
bestEffortQos.durability_volatile();
auto qos = rclcpp::QoS(
rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10));
qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
qos.avoid_ros_namespace_conventions(false);
cmdVelSub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", bestEffortQos,
"/cmd_vel", qos,
std::bind(&MotorDriver::cmdVelCallback, this, std::placeholders::_1));
if (publish_joint_states_) {
joint_state_publisher_ =
this->create_publisher<sensor_msgs::msg::JointState>("JointStates",
qos);
}
if (publish_odom_) {
odom_publisher_ =
this->create_publisher<nav_msgs::msg::Odometry>("odom", qos);
}
if (publish_joint_states_ || publish_odom_) {
this->publisher_thread_ = std::thread(&MotorDriver::publisherThread);
}
}
void MotorDriver::publisherThread() {
static rclcpp::Clock::SharedPtr clock =
std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
rclcpp::WallRate loop_rate(20);
rclcpp::Time now = clock->now();
rclcpp::Time last_time = now;
while (rclcpp::ok()) {
loop_rate.sleep();
if (RoboClaw::singleton() != nullptr) {
RoboClaw::singleton()->readSensorGroup();
nav_msgs::msg::Odometry odometry_msg;
sensor_msgs::msg::JointState joint_state_msg;
odometry_msg.header.stamp = clock->now();
odometry_msg.header.frame_id = "base_link";
joint_state_msg.header.stamp = clock->now();
joint_state_msg.header.frame_id = "base_link";
if (g_singleton->publish_joint_states_) {
float encoder_left = RoboClaw::singleton()->getM1Encoder() * 1.0;
float encoder_right = RoboClaw::singleton()->getM2Encoder() * 1.0;
double radians_left =
(encoder_left / g_singleton->quad_pulses_per_revolution_) * 2.0 *
M_PI;
double radians_right =
(encoder_right / g_singleton->quad_pulses_per_revolution_) * 2.0 *
M_PI;
joint_state_msg.name.push_back("front_left_wheel");
joint_state_msg.name.push_back("front_right_wheel");
joint_state_msg.position.push_back(radians_left);
joint_state_msg.position.push_back(radians_right);
g_singleton->joint_state_publisher_->publish(joint_state_msg);
}
}
}
}
MotorDriver &MotorDriver::singleton() {

13
src/motor_driver.h
View File

@@ -3,7 +3,9 @@
#include <stdint.h>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
#include "roboclaw.h"
@@ -14,16 +16,18 @@ class MotorDriver : public rclcpp::Node {
void onInit(rclcpp::Node::SharedPtr node);
RoboClaw &roboClaw() { return *roboclaw_; }
private:
MotorDriver();
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) const;
// For publishing odom, joint state, etc.
static void publisherThread();
uint32_t accel_quad_pulses_per_second_;
std::string device_name_;
uint8_t device_port_;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
float m1_p_;
float m1_i_;
float m1_d_;
@@ -37,9 +41,12 @@ class MotorDriver : public rclcpp::Node {
float max_angular_velocity_; // Maximum allowed angular velocity.
float max_linear_velocity_; // Maximum allowed linear velocity.
double max_seconds_uncommanded_travel_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
bool publish_odom_;
bool publish_joint_states_;
std::thread publisher_thread_; // For publishing odom, joint state, etc.
uint32_t quad_pulses_per_meter_;
uint32_t quad_pulses_per_revolution_;
RoboClaw *roboclaw_;
uint8_t vmin_;
uint8_t vtime_;
double wheel_radius_;

46
src/motor_driver_node.cpp
View File

@@ -33,68 +33,44 @@ int main(int argc, char *argv[])
{
try
{
roboClawStatus.logic_battery_voltage = motorDriver.roboClaw().getLogicBatteryLevel();
roboClawStatus.main_battery_voltage = motorDriver.roboClaw().getMainBatteryLevel();
RoboClaw::TMotorCurrents motorCurrents = motorDriver.roboClaw().getMotorCurrents();
roboClawStatus.logic_battery_voltage = RoboClaw::singleton()->getLogicBatteryLevel();
roboClawStatus.main_battery_voltage = RoboClaw::singleton()->getMainBatteryLevel();
RoboClaw::TMotorCurrents motorCurrents = RoboClaw::singleton()->getMotorCurrents();
roboClawStatus.m1_motor_current = motorCurrents.m1Current;
roboClawStatus.m2_motor_current = motorCurrents.m2Current;
RoboClaw::TPIDQ pidq = motorDriver.roboClaw().getPIDQ(RoboClaw::kGETM1PID);
RoboClaw::TPIDQ pidq = RoboClaw::singleton()->getPIDQ(RoboClaw::kGETM1PID);
roboClawStatus.m1_p = pidq.p / 65536.0;
roboClawStatus.m1_i = pidq.i / 65536.0;
roboClawStatus.m1_d = pidq.d / 65536.0;
roboClawStatus.m1_qpps = pidq.qpps;
pidq = motorDriver.roboClaw().getPIDQ(RoboClaw::kGETM2PID);
pidq = RoboClaw::singleton()->getPIDQ(RoboClaw::kGETM2PID);
roboClawStatus.m2_p = pidq.p / 65536.0;
roboClawStatus.m2_i = pidq.i / 65536.0;
roboClawStatus.m2_d = pidq.d / 65536.0;
roboClawStatus.m2_qpps = pidq.qpps;
roboClawStatus.temperature = motorDriver.roboClaw().getTemperature();
roboClawStatus.temperature = RoboClaw::singleton()->getTemperature();
{
RoboClaw::EncodeResult encoder = motorDriver.roboClaw().getEncoderCommandResult(RoboClaw::kGETM1ENC);
RoboClaw::EncodeResult encoder = RoboClaw::singleton()->getEncoderCommandResult(RoboClaw::kGETM1ENC);
roboClawStatus.m1_encoder_value = encoder.value;
roboClawStatus.m1_encoder_status = encoder.status;
}
{
RoboClaw::EncodeResult encoder = motorDriver.roboClaw().getEncoderCommandResult(RoboClaw::kGETM2ENC);
RoboClaw::EncodeResult encoder = RoboClaw::singleton()->getEncoderCommandResult(RoboClaw::kGETM2ENC);
roboClawStatus.m2_encoder_value = encoder.value;
roboClawStatus.m2_encoder_status = encoder.status;
}
roboClawStatus.m1_current_speed = motorDriver.roboClaw().getVelocity(RoboClaw::kGETM1SPEED);
roboClawStatus.m2_current_speed = motorDriver.roboClaw().getVelocity(RoboClaw::kGETM2SPEED);
roboClawStatus.m1_current_speed = RoboClaw::singleton()->getVelocity(RoboClaw::kGETM1SPEED);
roboClawStatus.m2_current_speed = RoboClaw::singleton()->getVelocity(RoboClaw::kGETM2SPEED);
roboClawStatus.error_string = motorDriver.roboClaw().getErrorString();
roboClawStatus.error_string = RoboClaw::singleton()->getErrorString();
statusPublisher->publish(roboClawStatus);
if (motorDriver.roboClaw().motorAlarms() != 0)
{
if (motorDriver.roboClaw().motorAlarms() & RoboClaw::kM1_OVER_CURRENT)
{
RCUTILS_LOG_ERROR("[motor_driver_node] M1_OVER_CURRENT");
}
if (motorDriver.roboClaw().motorAlarms() & RoboClaw::kM2_OVER_CURRENT)
{
RCUTILS_LOG_ERROR("[motor_driver_node] M2_OVER_CURRENT");
}
if (motorDriver.roboClaw().motorAlarms() & RoboClaw::kM1_OVER_CURRENT_ALARM)
{
RCUTILS_LOG_ERROR("[motor_driver_node] M1_OVER_CURRENT_ALARM");
}
if (motorDriver.roboClaw().motorAlarms() & RoboClaw::kM2_OVER_CURRENT_ALARM)
{
RCUTILS_LOG_ERROR("[motor_driver_node] M2_OVER_CURRENT_ALARM");
}
}
}
catch (RoboClaw::TRoboClawException *e)
{

95
src/roboclaw.cpp
View File

@@ -41,6 +41,8 @@ RoboClaw::RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent,
resetRequest.right = 0;
ros2_roboclaw_driver::srv::ResetEncoders::Response response;
resetEncoders(resetRequest, response);
g_singleton = this;
readSensorGroup();
}
RoboClaw::~RoboClaw() {}
@@ -70,6 +72,15 @@ void RoboClaw::doMixedSpeedAccelDist(uint32_t accel_quad_pulses_per_second,
}
RoboClaw::EncodeResult RoboClaw::getEncoderCommandResult(WHICH_ENC command) {
if (command == kGETM1ENC) {
return g_sensor_value_group_.m1_encoder_command_result;
} else {
return g_sensor_value_group_.m2_encoder_command_result;
}
}
RoboClaw::EncodeResult RoboClaw::cache_getEncoderCommandResult(
WHICH_ENC command) {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, command);
@@ -114,6 +125,10 @@ RoboClaw::EncodeResult RoboClaw::getEncoderCommandResult(WHICH_ENC command) {
}
uint16_t RoboClaw::getErrorStatus() {
return g_sensor_value_group_.error_status;
}
uint16_t RoboClaw::cache_getErrorStatus() {
for (int retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint16_t crc = 0;
@@ -150,6 +165,10 @@ uint16_t RoboClaw::getErrorStatus() {
}
std::string RoboClaw::getErrorString() {
return g_sensor_value_group_.error_string;
}
std::string RoboClaw::cache_getErrorString() {
uint16_t errorStatus = getErrorStatus();
if (errorStatus == 0)
return "normal";
@@ -230,6 +249,10 @@ std::string RoboClaw::getErrorString() {
}
float RoboClaw::getLogicBatteryLevel() {
return g_sensor_value_group_.logic_battery_level;
}
float RoboClaw::cache_getLogicBatteryLevel() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
@@ -252,7 +275,9 @@ float RoboClaw::getLogicBatteryLevel() {
"[RoboClaw::getLogicBatteryLevel] RETRY COUNT EXCEEDED");
}
int32_t RoboClaw::getM1Encoder() {
int32_t RoboClaw::getM1Encoder() { return g_sensor_value_group_.m1_encoder; }
int32_t RoboClaw::cache_getM1Encoder() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
@@ -273,6 +298,10 @@ int32_t RoboClaw::getM1Encoder() {
}
float RoboClaw::getMainBatteryLevel() {
return g_sensor_value_group_.main_battery_level;
}
float RoboClaw::cache_getMainBatteryLevel() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
@@ -329,6 +358,10 @@ unsigned short RoboClaw::get2ByteCommandResult(uint8_t command) {
}
RoboClaw::TMotorCurrents RoboClaw::getMotorCurrents() {
return g_sensor_value_group_.motor_currents;
}
RoboClaw::TMotorCurrents RoboClaw::cache_getMotorCurrents() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
@@ -375,6 +408,14 @@ RoboClaw::TMotorCurrents RoboClaw::getMotorCurrents() {
}
RoboClaw::TPIDQ RoboClaw::getPIDQ(WHICH_MOTOR whichMotor) {
if (whichMotor == kGETM1PID) {
return g_sensor_value_group_.m1_pidq;
} else {
return g_sensor_value_group_.m2_pidq;
}
}
RoboClaw::TPIDQ RoboClaw::cache_getPIDQ(WHICH_MOTOR whichMotor) {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
@@ -415,7 +456,9 @@ RoboClaw::TPIDQ RoboClaw::getPIDQ(WHICH_MOTOR whichMotor) {
throw new TRoboClawException("[RoboClaw::getPIDQ] RETRY COUNT EXCEEDED");
}
float RoboClaw::getTemperature() {
float RoboClaw::getTemperature() { return g_sensor_value_group_.temperature; }
float RoboClaw::cache_getTemperature() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
@@ -514,6 +557,14 @@ uint32_t RoboClaw::getULongCont(uint16_t &crc) {
}
int32_t RoboClaw::getVelocity(WHICH_VELOCITY whichVelocity) {
if (whichVelocity == kGETM1SPEED) {
return g_sensor_value_group_.m1_velocity;
} else {
return g_sensor_value_group_.m2_speed;
}
}
int32_t RoboClaw::cache_getVelocity(WHICH_VELOCITY whichVelocity) {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
@@ -577,7 +628,9 @@ int32_t RoboClaw::getVelocityResult(uint8_t command) {
return 0;
}
int32_t RoboClaw::getM2Encoder() {
int32_t RoboClaw::getM2Encoder() { return g_sensor_value_group_.m2_encoder; }
int32_t RoboClaw::cache_getM2Encoder() {
int retry;
for (retry = 0; retry < maxCommandRetries_; retry++) {
@@ -798,6 +851,37 @@ uint8_t RoboClaw::readByteWithTimeout() {
return 0;
}
void RoboClaw::readSensorGroup() {
if (singleton() != nullptr) {
g_sensor_value_group_.error_status = singleton()->cache_getErrorStatus();
g_sensor_value_group_.error_string = singleton()->cache_getErrorString();
g_sensor_value_group_.logic_battery_level =
singleton()->cache_getLogicBatteryLevel();
g_sensor_value_group_.m1_encoder = singleton()->cache_getM1Encoder();
g_sensor_value_group_.m1_encoder_command_result =
singleton()->cache_getEncoderCommandResult(kGETM1ENC);
g_sensor_value_group_.m1_pidq = singleton()->cache_getPIDQ(kGETM1PID);
g_sensor_value_group_.m1_velocity =
singleton()->cache_getVelocity(RoboClaw::kGETM1SPEED);
g_sensor_value_group_.m2_encoder = singleton()->cache_getM2Encoder();
g_sensor_value_group_.m2_encoder_command_result =
singleton()->cache_getEncoderCommandResult(kGETM2ENC);
g_sensor_value_group_.m1_pidq = singleton()->cache_getPIDQ(kGETM2PID);
g_sensor_value_group_.m2_speed =
singleton()->cache_getVelocity(RoboClaw::kGETM2SPEED);
g_sensor_value_group_.main_battery_level =
singleton()->cache_getMainBatteryLevel();
// Call getMotorCurrents before getMotorAlarms;
g_sensor_value_group_.motor_currents = singleton()->cache_getMotorCurrents();
g_sensor_value_group_.motor_alarms = singleton()->getMotorAlarms();
g_sensor_value_group_.temperature = singleton()->cache_getTemperature();
g_sensor_value_group_.last_sensor_read_time_ =
std::chrono::system_clock::now();
}
}
bool RoboClaw::resetEncoders(
ros2_roboclaw_driver::srv::ResetEncoders::Request &request,
ros2_roboclaw_driver::srv::ResetEncoders::Response &response) {
@@ -964,3 +1048,8 @@ void RoboClaw::writeN(bool sendCRC, uint8_t cnt, ...) {
fcntl(device_port_, F_SETFL, origFlags);
}
RoboClaw *RoboClaw::singleton() { return g_singleton; }
RoboClaw::SensorValueGroup RoboClaw::g_sensor_value_group_;
RoboClaw *RoboClaw::g_singleton = nullptr;

481
src/roboclaw.h
View File

@@ -1,258 +1,329 @@
#pragma once
#include "ros2_roboclaw_driver/srv/reset_encoders.hpp"
#include <chrono>
#include <rclcpp/logger.hpp>
#include <string>
class RoboClaw
{
public:
// Bit positions used to build alarms.
enum
{
kM1_OVER_CURRENT = 0x01, // Motor 1 current sense is too high.
kM2_OVER_CURRENT = 0x02, // Motor 2 current sense is too high.
kM1_OVER_CURRENT_ALARM = 0x04, // Motor 1 controller over current alarm.
kM2_OVER_CURRENT_ALARM = 0x08, // Motor 2 controller over current alarm.
};
#include "ros2_roboclaw_driver/srv/reset_encoders.hpp"
// Referencing which encoder in the RoboClaw
typedef enum WHICH_ENC
{
kGETM1ENC = 16,
kGETM2ENC = 17,
/* There are possibly several clients that want to read sensor data,
such as encoder values, motor speeds, etc. Each time an actual read
from the RoboClaw device is attempted, it potentially conflicts with
motor commands. And it's possible that clients want sensor data more
often than reasonably expected.
} WHICH_ENC;
As a result, GetXxx methods below that provide sensor data values are
broken up into a GetXxx and cache_GetXxx methods. The GetXxx methods
just return that last reading fetched from a periodic loop. The
cache_GetXxx methods do the real command request to the RoboClaw and
are only called during the class constructor and a periodic, background
thread.
*/
// Referencing which motor in the RoboClaw
typedef enum WHICH_MOTOR
{
kGETM1PID = 55,
kGETM2PID = 56,
} WHICH_MOTOR;
class RoboClaw {
public:
// Bit positions used to build alarms.
enum {
kM1_OVER_CURRENT = 0x01, // Motor 1 current sense is too high.
kM2_OVER_CURRENT = 0x02, // Motor 2 current sense is too high.
kM1_OVER_CURRENT_ALARM = 0x04, // Motor 1 controller over current alarm.
kM2_OVER_CURRENT_ALARM = 0x08, // Motor 2 controller over current alarm.
};
// Referencing which velocity in the RoboClaw
typedef enum WHICH_VELOCITY
{
kGETM1SPEED = 18,
kGETM2SPEED = 19,
} WHICH_VELOCITY;
// Referencing which encoder in the RoboClaw
typedef enum WHICH_ENC {
kGETM1ENC = 16,
kGETM2ENC = 17,
// A convenience struction to pass around configuration information.
typedef struct
{
float p;
float i;
float d;
uint32_t qpps;
float max_current;
} TPIDQ;
} WHICH_ENC;
// For a custom exception message.
struct TRoboClawException : public std::exception
{
std::string s;
TRoboClawException(std::string ss) : s(ss) {}
~TRoboClawException() throw() {}
const char *what() const throw() { return s.c_str(); }
};
// Referencing which motor in the RoboClaw
typedef enum WHICH_MOTOR {
kGETM1PID = 55,
kGETM2PID = 56,
} WHICH_MOTOR;
// Holds RoboClaw encoder result.
typedef struct
{
int32_t value;
uint8_t status;
} EncodeResult;
// Referencing which velocity in the RoboClaw
typedef enum WHICH_VELOCITY {
kGETM1SPEED = 18,
kGETM2SPEED = 19,
} WHICH_VELOCITY;
// Constructor.
RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent, float m2MaxCurrent, std::string device_name, uint8_t device_port, uint8_t vmin, uint8_t vtime);
// A convenience struction to pass around configuration information.
typedef struct {
float p;
float i;
float d;
uint32_t qpps;
float max_current;
} TPIDQ;
~RoboClaw();
// For a custom exception message.
struct TRoboClawException : public std::exception {
std::string s;
TRoboClawException(std::string ss) : s(ss) {}
~TRoboClawException() throw() {}
const char *what() const throw() { return s.c_str(); }
};
void doMixedSpeedDist(int32_t m1_quad_pulses_per_second, int32_t m1_max_distance, int32_t m2_quad_pulses_per_second, int32_t m2_max_distance);
// Holds RoboClaw encoder result.
typedef struct {
int32_t value;
uint8_t status;
} EncodeResult;
void doMixedSpeedAccelDist(uint32_t accel_quad_pulses_per_second, int32_t m1_quad_pulses_per_second, uint32_t m1_max_distance, int32_t m2_quad_pulses_per_second, uint32_t m2_max_distance);
EncodeResult getEncoderCommandResult(WHICH_ENC command);
// Constructor.
RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent,
float m2MaxCurrent, std::string device_name, uint8_t device_port,
uint8_t vmin, uint8_t vtime);
// Get RoboClaw error status bits.
uint16_t getErrorStatus();
~RoboClaw();
// Get RoboClaw error status as a string.
std::string getErrorString();
void doMixedSpeedDist(int32_t m1_quad_pulses_per_second,
int32_t m1_max_distance,
int32_t m2_quad_pulses_per_second,
int32_t m2_max_distance);
float getLogicBatteryLevel();
void doMixedSpeedAccelDist(uint32_t accel_quad_pulses_per_second,
int32_t m1_quad_pulses_per_second,
uint32_t m1_max_distance,
int32_t m2_quad_pulses_per_second,
uint32_t m2_max_distance);
float getMainBatteryLevel();
EncodeResult getEncoderCommandResult(WHICH_ENC command);
// Get the encoder value for motor 1.
int32_t getM1Encoder();
// Get RoboClaw error status bits.
uint16_t getErrorStatus();
// Get the encoder value for motor 2.
int32_t getM2Encoder();
// Get RoboClaw error status as a string.
std::string getErrorString();
// Convenience structure to hold a pair of current values.
typedef struct
{
float m1Current;
float m2Current;
} TMotorCurrents;
float getLogicBatteryLevel();
TMotorCurrents getMotorCurrents();
float getMainBatteryLevel();
TPIDQ getPIDQ(WHICH_MOTOR whichMotor);
// Get the encoder value for motor 1.
int32_t getM1Encoder();
int32_t getM1Speed();
// Get the encoder value for motor 2.
int32_t getM2Encoder();
TPIDQ getM2PIDQ();
// Convenience structure to hold a pair of current values.
typedef struct {
float m1Current;
float m2Current;
} TMotorCurrents;
int32_t getM2Speed();
// Make sure you call getMotorCurrents before getMotorAlarms.
int getMotorAlarms() { return motorAlarms_; }
float getTemperature();
TMotorCurrents getMotorCurrents();
// Get velocity (speed) of a motor.
int32_t getVelocity(WHICH_VELOCITY whichVelocity);
TPIDQ getPIDQ(WHICH_MOTOR whichMotor);
// Get RoboClaw software versions.
float getTemperature();
// Get velocity (speed) of a motor.
int32_t getVelocity(WHICH_VELOCITY whichVelocity);
// Get RoboClaw software versions.
std::string getVersion();
// Stop motion.
void stop();
int motorAlarms() { return motorAlarms_; }
// Get singleton instance of class.
static RoboClaw *singleton();
// Stop motion.
void stop();
static void readSensorGroup();
private:
EncodeResult cache_getEncoderCommandResult(WHICH_ENC command);
private:
typedef struct
{
unsigned long p1;
unsigned long p2;
} ULongPair;
// Get RoboClaw error status bits.
uint16_t cache_getErrorStatus();
enum
{
kERROR_NORMAL = 0x00,
kM1OVERCURRENT = 0x01,
kM2OVERCURRENT = 0x02,
kESTOP = 0x04,
kTEMPERATURE = 0x08,
kMAINBATTERYHIGH = 0x10,
kMAINBATTERYLOW = 0x20,
kLOGICBATTERYHIGH = 0x40,
kLOGICBATTERYLOW = 0x80
};
// Get RoboClaw error status as a string.
std::string cache_getErrorString();
// Enum values without a 'k' prefix have not been used in code.
typedef enum ROBOCLAW_COMMAND
{
M1FORWARD = 0,
M1BACKWARD = 1,
SETMINMB = 2,
SETMAXMB = 3,
M2FORWARD = 4,
M2BACKWARD = 5,
M17BIT = 6,
M27BIT = 7,
MIXEDFORWARD = 8,
MIXEDBACKWARD = 9,
MIXEDRIGHT = 10,
MIXEDLEFT = 11,
MIXEDFB = 12,
MIXEDLR = 13,
RESETENC = 20,
kGETVERSION = 21,
kSETM1ENCODER = 22,
kSETM2ENCODER = 23,
kGETMBATT = 24,
kGETLBATT = 25,
SETMINLB = 26,
SETMAXLB = 27,
kSETM1PID = 28,
kSETM2PID = 29,
GETM1ISPEED = 30,
GETM2ISPEED = 31,
M1DUTY = 32,
M2DUTY = 33,
kMIXEDDUTY = 34,
M1SPEED = 35,
M2SPEED = 36,
kMIXEDSPEED = 37,
M1SPEEDACCEL = 38,
M2SPEEDACCEL = 39,
MIXEDSPEEDACCEL = 40,
M1SPEEDDIST = 41,
M2SPEEDDIST = 42,
kMIXEDSPEEDDIST = 43,
M1SPEEDACCELDIST = 44,
M2SPEEDACCELDIST = 45,
kMIXEDSPEEDACCELDIST = 46,
GETBUFFERS = 47,
SETPWM = 48,
kGETCURRENTS = 49,
MIXEDSPEED2ACCEL = 50,
MIXEDSPEED2ACCELDIST = 51,
M1DUTYACCEL = 52,
M2DUTYACCEL = 53,
MIXEDDUTYACCEL = 54,
kGETTEMPERATURE = 82,
kGETERROR = 90,
WRITENVM = 94,
GETM1MAXCURRENT = 135
} ROBOCLAW_COMMAND;
float cache_getLogicBatteryLevel();
int device_port_; // Unix file descriptor for RoboClaw connection.
float m1p_;
float m1i_;
float m1d_;
int m1qpps_;
float m2p_;
float m2i_;
float m2d_;
int m2qpps_;
int maxCommandRetries_; // Maximum number of times to retry a RoboClaw command.
float maxM1Current_; // Maximum allowed M1 current.
float maxM2Current_; // Maximum allowed M2 current.
int motorAlarms_; // Motors alarms. Bit-wise OR of contributors.
std::string device_name_; // Device name of RoboClaw device.
int portAddress_; // Port number of RoboClaw device under control
int vmin_; // Terminal control value.
int vtime_; // Terminal control value.
// Get the encoder value for motor 1.
int32_t cache_getM1Encoder();
// Get velocity (speed) result from the RoboClaw controller.
int32_t getVelocityResult(uint8_t command);
int32_t cache_getM1Speed();
unsigned long getUlongCommandResult(uint8_t command);
// Get the encoder value for motor 2.
int32_t cache_getM2Encoder();
uint32_t getULongCont(uint16_t &crc);
int32_t cache_getM2Speed();
unsigned short get2ByteCommandResult(uint8_t command);
float cache_getMainBatteryLevel();
// Open the RoboClaw USB port.
void openPort();
TMotorCurrents cache_getMotorCurrents();
// Read one byte from device with timeout.
uint8_t readByteWithTimeout();
TPIDQ cache_getPIDQ(WHICH_MOTOR whichMotor);
// Perform error recovery to re-open a failed device port.
void restartPort();
float cache_getTemperature();
// Reset the encoders.
bool resetEncoders(ros2_roboclaw_driver::srv::ResetEncoders::Request &request,
ros2_roboclaw_driver::srv::ResetEncoders::Response &response);
// Get velocity (speed) of a motor.
int32_t cache_getVelocity(WHICH_VELOCITY whichVelocity);
// Set the PID for motor M1.
void setM1PID(float p, float i, float d, uint32_t qpps);
static void sensorReadThread();
// Set the PID for motor M1.
void setM2PID(float p, float i, float d, uint32_t qpps);
typedef struct {
uint16_t error_status;
std::string error_string;
float logic_battery_level;
int32_t m1_encoder;
EncodeResult m1_encoder_command_result;
TPIDQ m1_pidq;
int32_t m1_velocity;
int32_t m2_encoder;
EncodeResult m2_encoder_command_result;
TPIDQ m2_pidq;
int32_t m2_speed;
float main_battery_level;
int motor_alarms;
TMotorCurrents motor_currents;
float temperature;
std::chrono::system_clock::time_point last_sensor_read_time_;
} SensorValueGroup;
// Update the running CRC result.
void updateCrc(uint16_t &crc, uint8_t data);
static SensorValueGroup g_sensor_value_group_;
// Write one byte to the device.
void writeByte(uint8_t byte);
typedef struct {
unsigned long p1;
unsigned long p2;
} ULongPair;
// Write a stream of bytes to the device.
void writeN(bool sendCRC, uint8_t cnt, ...);
enum {
kERROR_NORMAL = 0x00,
kM1OVERCURRENT = 0x01,
kM2OVERCURRENT = 0x02,
kESTOP = 0x04,
kTEMPERATURE = 0x08,
kMAINBATTERYHIGH = 0x10,
kMAINBATTERYLOW = 0x20,
kLOGICBATTERYHIGH = 0x40,
kLOGICBATTERYLOW = 0x80
};
void SetEncoder(ROBOCLAW_COMMAND command, long value);
// Enum values without a 'k' prefix have not been used in code.
typedef enum ROBOCLAW_COMMAND {
M1FORWARD = 0,
M1BACKWARD = 1,
SETMINMB = 2,
SETMAXMB = 3,
M2FORWARD = 4,
M2BACKWARD = 5,
M17BIT = 6,
M27BIT = 7,
MIXEDFORWARD = 8,
MIXEDBACKWARD = 9,
MIXEDRIGHT = 10,
MIXEDLEFT = 11,
MIXEDFB = 12,
MIXEDLR = 13,
RESETENC = 20,
kGETVERSION = 21,
kSETM1ENCODER = 22,
kSETM2ENCODER = 23,
kGETMBATT = 24,
kGETLBATT = 25,
SETMINLB = 26,
SETMAXLB = 27,
kSETM1PID = 28,
kSETM2PID = 29,
GETM1ISPEED = 30,
GETM2ISPEED = 31,
M1DUTY = 32,
M2DUTY = 33,
kMIXEDDUTY = 34,
M1SPEED = 35,
M2SPEED = 36,
kMIXEDSPEED = 37,
M1SPEEDACCEL = 38,
M2SPEEDACCEL = 39,
MIXEDSPEEDACCEL = 40,
M1SPEEDDIST = 41,
M2SPEEDDIST = 42,
kMIXEDSPEEDDIST = 43,
M1SPEEDACCELDIST = 44,
M2SPEEDACCELDIST = 45,
kMIXEDSPEEDACCELDIST = 46,
GETBUFFERS = 47,
SETPWM = 48,
kGETCURRENTS = 49,
MIXEDSPEED2ACCEL = 50,
MIXEDSPEED2ACCELDIST = 51,
M1DUTYACCEL = 52,
M2DUTYACCEL = 53,
MIXEDDUTYACCEL = 54,
kGETTEMPERATURE = 82,
kGETERROR = 90,
WRITENVM = 94,
GETM1MAXCURRENT = 135
} ROBOCLAW_COMMAND;
int device_port_; // Unix file descriptor for RoboClaw connection.
float m1p_;
float m1i_;
float m1d_;
int m1qpps_;
float m2p_;
float m2i_;
float m2d_;
int m2qpps_;
int maxCommandRetries_; // Maximum number of times to retry a RoboClaw
// command.
float maxM1Current_; // Maximum allowed M1 current.
float maxM2Current_; // Maximum allowed M2 current.
int motorAlarms_; // Motors alarms. Bit-wise OR of contributors.
std::string device_name_; // Device name of RoboClaw device.
int portAddress_; // Port number of RoboClaw device under control
int vmin_; // Terminal control value.
int vtime_; // Terminal control value.
// Get velocity (speed) result from the RoboClaw controller.
int32_t getVelocityResult(uint8_t command);
unsigned long getUlongCommandResult(uint8_t command);
uint32_t getULongCont(uint16_t &crc);
unsigned short get2ByteCommandResult(uint8_t command);
// Open the RoboClaw USB port.
void openPort();
// Read one byte from device with timeout.
uint8_t readByteWithTimeout();
// Perform error recovery to re-open a failed device port.
void restartPort();
// Reset the encoders.
bool resetEncoders(
ros2_roboclaw_driver::srv::ResetEncoders::Request &request,
ros2_roboclaw_driver::srv::ResetEncoders::Response &response);
// Set the PID for motor M1.
void setM1PID(float p, float i, float d, uint32_t qpps);
// Set the PID for motor M1.
void setM2PID(float p, float i, float d, uint32_t qpps);
// Update the running CRC result.
void updateCrc(uint16_t &crc, uint8_t data);
// Write one byte to the device.
void writeByte(uint8_t byte);
// Write a stream of bytes to the device.
void writeN(bool sendCRC, uint8_t cnt, ...);
void SetEncoder(ROBOCLAW_COMMAND command, long value);
static RoboClaw *g_singleton;
};