From 2bd7a00af439956027553b9f058f7d0816dfe34f Mon Sep 17 00:00:00 2001 From: Michael Wimble Date: Sun, 23 Mar 2025 11:27:57 -0700 Subject: [PATCH] Instructions that M1 is left motor. Add odom publishing. Config param for sensor read/joint state pub/odom pub. Refactoring motor_driver.cpp --- README.md | 2 +- config/motor_driver.yaml | 3 + {src => include}/motor_driver.h | 59 ++++----- src/motor_driver.cpp | 225 +++++++++++++++++++++++--------- 4 files changed, 193 insertions(+), 96 deletions(-) rename {src => include}/motor_driver.h (62%) mode change 100755 => 100644 diff --git a/README.md b/README.md index dbdc194..e86bd94 100644 --- a/README.md +++ b/README.md @@ -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 diff --git a/config/motor_driver.yaml b/config/motor_driver.yaml index 5eb2613..e5643f5 100755 --- a/config/motor_driver.yaml +++ b/config/motor_driver.yaml @@ -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. \ No newline at end of file diff --git a/src/motor_driver.h b/include/motor_driver.h old mode 100755 new mode 100644 similarity index 62% rename from src/motor_driver.h rename to include/motor_driver.h index 4ee9d64..6db73b0 --- a/src/motor_driver.h +++ b/include/motor_driver.h @@ -1,62 +1,59 @@ #pragma once -#include - #include #include #include #include -#include - -#include "roboclaw.h" +#include 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::SharedPtr cmdVelSub_; rclcpp::Publisher::SharedPtr joint_state_publisher_; + rclcpp::Publisher::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::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::SharedPtr cmdVelSub_; + float sensor_update_rate_; // Hz + float wheel_radius_; + float wheel_separation_; static MotorDriver *g_singleton; }; diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp index 7dd8f26..b84fa1c 100755 --- a/src/motor_driver.cpp +++ b/src/motor_driver.cpp @@ -1,4 +1,3 @@ - #include "motor_driver.h" #include @@ -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("accel_quad_pulses_per_second", 600); this->declare_parameter("baud_rate", 38400); this->declare_parameter("device_name", "roboclaw"); @@ -43,10 +47,79 @@ MotorDriver::MotorDriver() this->declare_parameter("publish_odom", true); this->declare_parameter("quad_pulses_per_meter", 0); this->declare_parameter("quad_pulses_per_revolution", 0); + this->declare_parameter("sensor_update_rate", 20.0); // Hz this->declare_parameter("wheel_radius", 0.0); this->declare_parameter("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("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(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); + } } } }