I'm using ros2 humble. I simulate gazebo empty world, turtlebot3 burger model. I deleted diff drive plugin part from sdf file. I want my turtlebot to move without using diff_drive_controller. I tried to create my own plugin but i failed. My mentor said i don't need to write a plugin. It's possible to write a code using cmd and robot's physicial propereties that can move the robot (without using diff drive). It's been a week but i couldn't find a solution.
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
class WheelEffortController : public rclcpp::Node
{
public:
WheelEffortController() : Node("wheel_effort_controller")
{
// /cmd_vel Subscriber
cmd_vel_subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>(
"cmd_vel", 10, std::bind(&WheelEffortController::cmd_vel_callback, this, std::placeholders::_1));
// /joint_states Subscriber (To read real wheel velocities)
joint_states_subscriber_ = this->create_subscription<sensor_msgs::msg::JointState>(
"joint_states", 10, std::bind(&WheelEffortController::joint_states_callback, this, std::placeholders::_1));
// Publisher that publish wheel velocities
wheel_velocity_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
"/wheel_velocity_controller/commands", 10);
// Wheel properties
wheel_radius_ = 0.033; // m
wheel_base_ = 0.160; // m
}
private:
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
// Take linear and angular velocities
double linear_velocity = msg->linear.x; // m/s
double angular_velocity = msg->angular.z; // rad/s
// Calculate wheel velocities
double wheel_left_velocity = (linear_velocity - (angular_velocity * wheel_base_ / 2)) / wheel_radius_;
double wheel_right_velocity = (linear_velocity + (angular_velocity * wheel_base_ / 2)) / wheel_radius_;
std_msgs::msg::Float64MultiArray wheel_msg;
wheel_msg.data = {wheel_left_velocity, wheel_right_velocity};
wheel_velocity_publisher_->publish(wheel_msg);
RCLCPP_INFO(this->get_logger(), "CMD_VEL -> Publishing velocities: Left = %.2f, Right = %.2f",
wheel_left_velocity, wheel_right_velocity);
}
void joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
if (msg->name.size() < 2 || msg->velocity.size() < 2)
return;
double wheel_left_current_velocity = msg->velocity[0];
double wheel_right_current_velocity = msg->velocity[1];
RCLCPP_INFO(this->get_logger(), "JOINT_STATES -> Left Wheel: %.2f, Right Wheel: %.2f",
wheel_left_current_velocity, wheel_right_current_velocity);
}
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_subscriber_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr wheel_velocity_publisher_;
double wheel_radius_;
double wheel_base_;
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<WheelEffortController>());
rclcpp::shutdown();
return 0;
}