ros - How to move turtlebot3 without diff drive plugin? (ROS2) - Stack Overflow

admin管理员组

文章数量:1294382

I'm trying to move my turtlebot burger model on gazebo without diff_drive_controller. I commented diff drive plugin from the sdf file. I wrote a code but my robot still not moving.

Here's my code:

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "std_msgs/msg/float64.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

class RobotController : public rclcpp::Node
{
public:
    RobotController() : Node("robot_controller")
    {
        // cmd_vel subscriber
        cmd_vel_subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "/cmd_vel", 10, std::bind(&RobotController::cmd_vel_callback, this, std::placeholders::_1));
            
        // joint_states publisher for wheels
        joint_states_publisher_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);

        // Get joint_state messages
        joint_state_subscriber_ = this->create_subscription<sensor_msgs::msg::JointState>(
            "/joint_states", 10, std::bind(&RobotController::joint_states_callback, this, std::placeholders::_1));
    }

private:
    void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
        double wheel_separation = 0.16;  // Distance between the wheels (m)
        double wheel_radius = 0.03;      // Wheel radius (m)

        double linear_force = msg->linear.x * 5.0;  // Converting to force with basic coef.
        double angular_force = msg->angular.z * 2.0; // Angular force

        // Calculate torques for both wheels 
        double left_wheel_effort = (linear_force - (wheel_separation / 2.0) * angular_force);
        double right_wheel_effort = (linear_force + (wheel_separation / 2.0) * angular_force);

        // Create JointState message
        auto joint_state_msg = sensor_msgs::msg::JointState();
        joint_state_msg.header.stamp = this->get_clock()->now();  // Zaman damgası
        joint_state_msg.name.push_back("wheel_left_joint");
        joint_state_msg.name.push_back("wheel_right_joint");

        // Add position values as 0
        joint_state_msg.position.push_back(0.0);  // Left wheel position (temp,0) 
        joint_state_msg.position.push_back(0.0);  // Right wheel positin (temp,0)

        // Set the torque values of left and right wheels
        joint_state_msg.effort.push_back(left_wheel_effort);
        joint_state_msg.effort.push_back(right_wheel_effort);

        // Publish JointState to joint_states topic
        joint_states_publisher_->publish(joint_state_msg);

        RCLCPP_INFO(this->get_logger(), "Set Effort -> Left: %f, Right: %f", left_wheel_effort, right_wheel_effort);
    }

    void joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
    {
        double left_wheel_effort = 0.0;
        double right_wheel_effort = 0.0;

        // Control the torque values that come in the joint_state mesage 
        for (size_t i = 0; i < msg->name.size(); i++) {
            if (msg->name[i] == "wheel_left_joint") {
                left_wheel_effort = msg->effort[i];
            }
            if (msg->name[i] == "wheel_right_joint") {
                right_wheel_effort = msg->effort[i];
            }
        }

        // Log the incoming torques 
        RCLCPP_INFO(this->get_logger(), "Effort Feedback -> Left: %f, Right: %f", left_wheel_effort, right_wheel_effort);
    }

    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber_;
    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_states_publisher_;
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RobotController>());
    rclcpp::shutdown();
    return 0;
}

I wrote a code i used publishers and subscribers. In terminal i see joint states sub count 2 pub count 2. cmd vel sub count 1 pub count 1 but as i said my robot not moving

本文标签： rosHow to move turtlebot3 without diff drive plugin (ROS2)Stack Overflow