|
| 1 | +#include "arc_msgs/msg/diff_drive.hpp" |
| 2 | +#include "geometry_msgs/msg/twist.hpp" |
| 3 | +#include "rclcpp/rclcpp.hpp" |
| 4 | +#include <cmath> |
| 5 | +#include <iostream> |
| 6 | + |
| 7 | +template <typename T> using Publisher = rclcpp::Publisher<T>; |
| 8 | + |
| 9 | +template <typename T> using Subscriber = rclcpp::Subscription<T>; |
| 10 | + |
| 11 | +using TwistMsg = geometry_msgs::msg::Twist; |
| 12 | +using DiffDriveMsg = arc_msgs::msg::DiffDrive; |
| 13 | + |
| 14 | +class DifferentialDrive { |
| 15 | + private: |
| 16 | + double wheel_base_; |
| 17 | + double wheel_radius_; |
| 18 | + |
| 19 | + public: |
| 20 | + DifferentialDrive(double wheel_base, double wheel_radius) |
| 21 | + : wheel_base_{wheel_base}, wheel_radius_{wheel_radius} {} |
| 22 | + |
| 23 | + // Calculate the left and right wheel velocities based on linear and angular |
| 24 | + // velocity |
| 25 | + auto calculateWheelVelocities(double linear_vel, double angular_vel) const { |
| 26 | + return std::pair{ |
| 27 | + (linear_vel - (angular_vel * wheel_base_ / 2.0)) / wheel_radius_, |
| 28 | + (linear_vel + (angular_vel * wheel_base_ / 2.0)) / wheel_radius_}; |
| 29 | + } |
| 30 | +}; |
| 31 | + |
| 32 | +class TwistToDiffDriveNode : public rclcpp::Node { |
| 33 | + public: |
| 34 | + TwistToDiffDriveNode() : Node("twist_to_diff_drive_node") { |
| 35 | + // Create a DifferentialDrive object with a wheel base of 224mm and a |
| 36 | + // wheel radius of 150mm |
| 37 | + diff_drive_model_ = DifferentialDrive(0.224, 0.15); |
| 38 | + |
| 39 | + twist_sub_ = this->create_subscription<TwistMsg>( |
| 40 | + "cmd_vel", 10, |
| 41 | + std::bind(&TwistToDiffDriveNode::twist_cb, this, |
| 42 | + std::placeholders::_1)); |
| 43 | + diff_drive_pub_ = |
| 44 | + this->create_publisher<DiffDriveMsg>("diff_drive", 10); |
| 45 | + } |
| 46 | + |
| 47 | + private: |
| 48 | + Subscriber<TwistMsg>::SharedPtr twist_sub_; |
| 49 | + Publisher<DiffDriveMsg>::SharedPtr diff_drive_pub_; |
| 50 | + DifferentialDrive diff_drive_model_; |
| 51 | + void twist_cb(TwistMsg msg) { |
| 52 | + // Calculate the left and right wheel velocities |
| 53 | + auto [left_motor, right_motor] = |
| 54 | + diff_drive_model_.calculateWheelVelocities(msg.linear.x, |
| 55 | + msg.angular.z); |
| 56 | + |
| 57 | + // Publish the left and right motor velocities |
| 58 | + auto diff_drive_msg = DiffDriveMsg(); |
| 59 | + diff_drive_msg.left_motor = left_motor; |
| 60 | + diff_drive_msg.right_motor = right_motor; |
| 61 | + diff_drive_pub_->publish(diff_drive_msg); |
| 62 | + } |
| 63 | +}; |
| 64 | + |
| 65 | +int main() { |
| 66 | + rclcpp::init(argc, argv); |
| 67 | + |
| 68 | + auto node = std::make_shared<TwistToDiffDriveNode>(); |
| 69 | + |
| 70 | + rclcpp::spin(node); |
| 71 | + rclcpp::shutdown(); |
| 72 | + |
| 73 | + return 0; |
| 74 | +} |
0 commit comments