0

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

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Start asking to get answers

Find the answer to your question by asking.

Ask question

Explore related questions

See similar questions with these tags.