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

Instructions that M1 is left motor.

Browse Source 
Add odom publishing.
Config param for sensor read/joint state pub/odom pub.
Refactoring motor_driver.cpp
This commit is contained in:
Michael Wimble
2025-03-23 11:27:57 -07:00
parent f325bfc6ec
commit 2bd7a00af4
4 changed files with 193 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@@ -2,7 +2,7 @@
A driver for the RoboClaw family of devices for use under ROS2.
This driver is hard coded to use a pair of motors, presumed to be in a
differential drive configuration. In various places, they are refered to
as ***m1*** and ***m2***
as ***m1*** and ***m2***. It is expected that ***m1*** is the left motor and ***m2*** is the right motor.
This code does not publish odometry information. Wheel odometry is often so
wildly wrong that it is expected that a robot builder will derive it in some way

3
config/motor_driver.yaml
View File

@@ -56,6 +56,9 @@ motor_driver_node:
# Topic name to be used to publish the RoboClaw status messages.
roboclaw_status_topic: "roboclaw_status"
# How often, in Hz, to sense the various RoboClaw internal values.
sensor_rate_hz: 20.0
# Debugging control.
do_debug: false # True => Log RoboClaw commands and responses.
do_low_level_debug: false # True => Log low-level serial and RoboClaw data.

59
src/motor_driver.h → include/motor_driver.h Executable file → Normal file
View File

@@ -1,62 +1,59 @@
#pragma once
#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"
#include <thread>
class MotorDriver : public rclcpp::Node {
public:
MotorDriver();
static MotorDriver &singleton();
void onInit(rclcpp::Node::SharedPtr node);
private:
MotorDriver();
void declareParameters();
void eulerToQuaternion(float roll, float pitch, float yaw, float *q);
void initializeParameters();
void logParameters() const;
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) const;
void publisherThread();
// For publishing odom, joint state, etc.
static void publisherThread();
uint32_t accel_quad_pulses_per_second_;
uint32_t baud_rate_;
std::string device_name_;
uint8_t device_port_;
bool do_debug_;
bool do_low_level_debug_;
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmdVelSub_;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr
joint_state_publisher_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
std::thread publisher_thread_;
int accel_quad_pulses_per_second_;
int baud_rate_;
std::string device_name_;
int device_port_;
bool do_debug_;
bool do_low_level_debug_;
float m1_p_;
float m1_i_;
float m1_d_;
uint32_t m1_qpps_;
int m1_qpps_;
float m1_max_current_;
float m2_p_;
float m2_i_;
float m2_d_;
uint32_t m2_qpps_;
int m2_qpps_;
float m2_max_current_;
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_;
float max_angular_velocity_;
float max_linear_velocity_;
float max_seconds_uncommanded_travel_;
bool publish_joint_states_;
std::thread publisher_thread_; // For publishing odom, joint state, etc.
uint32_t quad_pulses_per_meter_;
bool publish_odom_;
int quad_pulses_per_meter_;
float quad_pulses_per_revolution_;
double wheel_radius_;
double wheel_separation_;
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmdVelSub_;
float sensor_update_rate_; // Hz
float wheel_radius_;
float wheel_separation_;
static MotorDriver *g_singleton;
};

225
src/motor_driver.cpp
View File

@@ -1,4 +1,3 @@
#include "motor_driver.h"
#include <math.h>
@@ -20,6 +19,11 @@ MotorDriver::MotorDriver()
device_name_("foo_bar"),
wheel_radius_(0.10169),
wheel_separation_(0.345) {
declareParameters();
initializeParameters();
}
void MotorDriver::declareParameters() {
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");
@@ -43,10 +47,79 @@ MotorDriver::MotorDriver()
this->declare_parameter<bool>("publish_odom", true);
this->declare_parameter<int>("quad_pulses_per_meter", 0);
this->declare_parameter<float>("quad_pulses_per_revolution", 0);
this->declare_parameter<float>("sensor_update_rate", 20.0); // Hz
this->declare_parameter<float>("wheel_radius", 0.0);
this->declare_parameter<float>("wheel_separation", 0.0);
}
void MotorDriver::initializeParameters() {
this->get_parameter("accel_quad_pulses_per_second",
accel_quad_pulses_per_second_);
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_);
this->get_parameter("m1_qpps", m1_qpps_);
this->get_parameter("m1_max_current", m1_max_current_);
this->get_parameter("m2_p", m2_p_);
this->get_parameter("m2_i", m2_i_);
this->get_parameter("m2_d", m2_d_);
this->get_parameter("m2_qpps", m2_qpps_);
this->get_parameter("m2_max_current", m2_max_current_);
this->get_parameter("max_angular_velocity", max_angular_velocity_);
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_odom", publish_odom_);
this->get_parameter("quad_pulses_per_meter", quad_pulses_per_meter_);
this->get_parameter("quad_pulses_per_revolution",
quad_pulses_per_revolution_);
this->get_parameter("sensor_update_rate", sensor_update_rate_);
this->get_parameter("wheel_radius", wheel_radius_);
this->get_parameter("wheel_separation", wheel_separation_);
logParameters();
}
void MotorDriver::logParameters() const {
RCUTILS_LOG_INFO("accel_quad_pulses_per_second: %d",
accel_quad_pulses_per_second_);
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_);
RCUTILS_LOG_INFO("m1_qpps: %d", m1_qpps_);
RCUTILS_LOG_INFO("m1_max_current: %f", m1_max_current_);
RCUTILS_LOG_INFO("m2_p: %f", m2_p_);
RCUTILS_LOG_INFO("m2_i: %f", m2_i_);
RCUTILS_LOG_INFO("m2_d: %f", m2_d_);
RCUTILS_LOG_INFO("m2_qpps: %d", m2_qpps_);
RCUTILS_LOG_INFO("m2_max_current: %f", m2_max_current_);
RCUTILS_LOG_INFO("max_angular_velocity: %f", max_angular_velocity_);
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("publish_odom: %s", publish_odom_ ? "True" : "False");
RCUTILS_LOG_INFO("quad_pulses_per_meter: %d", quad_pulses_per_meter_);
RCUTILS_LOG_INFO("quad_pulses_per_revolution: %3.4f",
quad_pulses_per_revolution_);
RCUTILS_LOG_INFO("sensor_update_rate: %f", sensor_update_rate_);
RCUTILS_LOG_INFO("wheel_radius: %f", wheel_radius_);
RCUTILS_LOG_INFO("wheel_separation: %f", wheel_separation_);
}
void MotorDriver::cmdVelCallback(
const geometry_msgs::msg::Twist::SharedPtr msg) const {
if (RoboClaw::singleton() != nullptr) {
@@ -81,68 +154,12 @@ void MotorDriver::cmdVelCallback(
void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
node_ = node;
this->get_parameter("accel_quad_pulses_per_second",
accel_quad_pulses_per_second_);
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_);
this->get_parameter("m1_qpps", m1_qpps_);
this->get_parameter("m1_max_current", m1_max_current_);
this->get_parameter("m2_p", m2_p_);
this->get_parameter("m2_i", m2_i_);
this->get_parameter("m2_d", m2_d_);
this->get_parameter("m2_qpps", m2_qpps_);
this->get_parameter("m2_max_current", m2_max_current_);
max_angular_velocity_ = -12.34;
this->get_parameter("max_angular_velocity", max_angular_velocity_);
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_);
this->get_parameter("wheel_radius", wheel_radius_);
this->get_parameter("wheel_separation", wheel_separation_);
initializeParameters();
RCUTILS_LOG_INFO("accel_quad_pulses_per_second: %d",
accel_quad_pulses_per_second_);
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_);
RCUTILS_LOG_INFO("m1_qpps: %d", m1_qpps_);
RCUTILS_LOG_INFO("m1_max_current: %f", m1_max_current_);
RCUTILS_LOG_INFO("m2_p: %f", m2_p_);
RCUTILS_LOG_INFO("m2_i: %f", m2_i_);
RCUTILS_LOG_INFO("m2_d: %f", m2_d_);
RCUTILS_LOG_INFO("m2_qpps: %d", m2_qpps_);
RCUTILS_LOG_INFO("m2_max_current: %f", m2_max_current_);
RCUTILS_LOG_INFO("max_angular_velocity: %f", max_angular_velocity_);
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: %3.4f",
quad_pulses_per_revolution_);
RCUTILS_LOG_INFO("wheel_radius: %f", wheel_radius_);
RCUTILS_LOG_INFO("wheel_separation: %f", wheel_separation_);
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::TPIDQ m1Pid = {m1_p_, m1_i_, m1_d_, (uint32_t)m1_qpps_,
m1_max_current_};
RoboClaw::TPIDQ m2Pid = {m2_p_, m2_i_, m2_d_, (uint32_t)m2_qpps_,
m2_max_current_};
new RoboClaw(m1Pid, m2Pid, m1_max_current_, m2_max_current_,
device_name_.c_str(), device_port_, baud_rate_, do_debug_,
@@ -171,15 +188,34 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
this->create_publisher<nav_msgs::msg::Odometry>("odom", qos);
}
// Start the publisher thread if we are publishing joint states or odometry
// messages. This thread will read the RoboClaw sensors and publish the
// corresponding messages at the specified sensor update rate.
// The thread will run until the node is shut down.
if (publish_joint_states_ || publish_odom_) {
this->publisher_thread_ = std::thread(&MotorDriver::publisherThread);
this->publisher_thread_ = std::thread(&MotorDriver::publisherThread, this);
}
}
void MotorDriver::eulerToQuaternion(float roll, float pitch, float yaw,
float *q) {
float cy = cos(yaw * 0.5);
float sy = sin(yaw * 0.5);
float cp = cos(pitch * 0.5);
float sp = sin(pitch * 0.5);
float cr = cos(roll * 0.5);
float sr = sin(roll * 0.5);
q[0] = cy * cp * cr + sy * sp * sr;
q[1] = cy * cp * sr - sy * sp * cr;
q[2] = sy * cp * sr + cy * sp * cr;
q[3] = sy * cp * cr - cy * sp * sr;
}
void MotorDriver::publisherThread() {
static rclcpp::Clock::SharedPtr clock =
std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
rclcpp::WallRate loop_rate(20);
rclcpp::WallRate loop_rate(sensor_update_rate_);
rclcpp::Time now = clock->now();
rclcpp::Time last_time = now;
@@ -212,6 +248,67 @@ void MotorDriver::publisherThread() {
joint_state_msg.position.push_back(radians_right);
g_singleton->joint_state_publisher_->publish(joint_state_msg);
}
if (g_singleton->publish_odom_) {
now = clock->now();
double dt = (now - last_time).seconds();
last_time = now;
float linear_velocity_x =
RoboClaw::singleton()->getVelocity(RoboClaw::kM1) * 1.0;
float linear_velocity_y =
RoboClaw::singleton()->getVelocity(RoboClaw::kM2) * 1.0;
float angular_velocity_z = (linear_velocity_x - linear_velocity_y) /
g_singleton->wheel_separation_;
// Calculate the robot's position and orientation
static float x_pos_(0.0);
static float y_pos_(0.0);
static float heading_(0.0);
static bool first_time = true;
if (first_time) {
first_time = false;
x_pos_ = 0.0;
y_pos_ = 0.0;
heading_ = 0.0;
}
float delta_heading = angular_velocity_z * dt; // radians
float cos_h = cos(heading_);
float sin_h = sin(heading_);
float delta_x =
(linear_velocity_x * cos_h - linear_velocity_y * sin_h) * dt; // m
float delta_y =
(linear_velocity_x * sin_h + linear_velocity_y * cos_h) * dt; // m
// calculate current position of the robot
x_pos_ += delta_x;
y_pos_ += delta_y;
heading_ += delta_heading;
// calculate robot's heading in quaternion angle
// ROS has a function to calculate yaw in quaternion angle
float q[4];
eulerToQuaternion(0, 0, heading_, q);
// robot's position in x,y, and z
odometry_msg.pose.pose.position.x = x_pos_;
odometry_msg.pose.pose.position.y = y_pos_;
odometry_msg.pose.pose.position.z = 0.0;
// robot's heading in quaternion
odometry_msg.pose.pose.orientation.x = (double)q[1];
odometry_msg.pose.pose.orientation.y = (double)q[2];
odometry_msg.pose.pose.orientation.z = (double)q[3];
odometry_msg.pose.pose.orientation.w = (double)q[0];
odometry_msg.pose.covariance[0] = 0.001;
odometry_msg.pose.covariance[7] = 0.001;
odometry_msg.pose.covariance[35] = 0.001;
odometry_msg.twist.twist.linear.x = linear_velocity_x;
odometry_msg.twist.twist.linear.y = linear_velocity_y;
odometry_msg.twist.twist.linear.z = 0.0;
odometry_msg.twist.twist.angular.x = 0.0;
odometry_msg.twist.twist.angular.y = 0.0;
odometry_msg.twist.twist.angular.z = angular_velocity_z;
g_singleton->odom_publisher_->publish(odometry_msg);
}
}
}
}