1
$\begingroup$

I am working using ros2 on the keyestudio ks0223 smart car. I am using a diff_drive control. At this point, my right rear wheel goes in the opposite direction to the right front wheel. (for some reason my left front wheel does not even move at all). I wrote a simple python script (outside of ros) and the wheels move properly (left front still does not move). Here are my codes:

URDF:

<xacro:include filename="common_properties.xacro"/>
<xacro:include filename="$(find my_robot)/description/ros_control/my_robot_control.xacro"/>
<!-- <xacro:include filename="gojo_gazebo_ref.xacro" /> -->
<!-- <xacro:include filename="gojo_camera.xacro"/> -->
<!-- <xacro:include filename="gojo_ros2_control.xacro"/> -->

<link name="base_footprint">
</link>
<link name="base_link">
    <visual>
        <geometry>
            <box size="${box_len} ${box_wid} ${box_hgt}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="green" />
    </visual>
    <collision>
        <geometry>
            <box size="${box_len} ${box_wid} ${box_hgt}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
    </collision>
    <inertial>
        <mass value="${box_mass}" />
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
        <inertia ixx="${(1.0/12.0)*(box_len_sq + box_hgt_sq)}" 
                 iyy="${(1.0/12.0)*(box_wid_sq + box_hgt_sq)}" 
                 izz="${(1.0/12.0)*(box_wid_sq + box_len_sq)}" 
                 ixy="0.0" ixz="0.0" iyz="0.0" 
                 />
    </inertial>
</link>


<joint name="base_joint" type="fixed">
    <parent link="base_footprint" />
    <child link="base_link" />
    <origin xyz="0 0 0.03" rpy="0 0 0" />
</joint>


<link name="left_front_wheel_link">
    <visual>
        <geometry>
            <cylinder radius="${wheel_rad}" length="${wheel_wid}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="black" />
    </visual>
    <collision>
        <geometry>
            <cylinder radius="${wheel_rad}" length="${wheel_wid}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
    </collision>
    <inertial>
        <mass value="${wheel_mass}"/>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <inertia 
                 ixx="${1./12. * wheel_mass * ((wheel_rad_sq * 3) + (wheel_wid_sq))}" 
                 iyy="${1./12. * wheel_mass * ((wheel_rad_sq * 3) + (wheel_wid_sq))}" 
                 izz="${1./12. * wheel_mass * (wheel_rad_sq)}" 
                 ixy="0.0" ixz="0.0" iyz="0.0" 
                 />
    </inertial>
</link> 
<joint name="base_left_front_wheel_joint" type="continuous">
    <parent link="base_link" />
    <child link="left_front_wheel_link" />
    <origin xyz="${-box_len/4} ${-box_wid/2 -wheel_wid/2} 0" rpy="1.57 0 0" />
    <axis xyz="0 0 1" />
    <limit effort="1000" velocity="1000"/>
    <dynamics damping="${bearing_damping}" friction="${bearing_friction}"/>
</joint>



<link name="right_front_wheel_link">
    <visual>
        <geometry>
            <cylinder radius="${wheel_rad}" length="${wheel_wid}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="black" />
    </visual>
    <collision>
        <geometry>
            <cylinder radius="${wheel_rad}" length="${wheel_wid}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
    </collision>
    <inertial>
        <mass value="${wheel_mass}"/>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <inertia 
                 ixx="${1./12. * wheel_mass * ((wheel_rad_sq * 3) + (wheel_wid_sq))}" 
                 iyy="${1./12. * wheel_mass * ((wheel_rad_sq * 3) + (wheel_wid_sq))}" 
                 izz="${1./12. * wheel_mass * (wheel_rad_sq)}" 
                 ixy="0.0" ixz="0.0" iyz="0.0" 
                 />
    </inertial>
</link> 
<joint name="base_right_front_wheel_joint" type="continuous">
    <parent link="base_link" />
    <child link="right_front_wheel_link" />
    <origin xyz="${-box_len/4} ${box_wid/2 + wheel_wid/2} 0" rpy="1.57 0 0" />
    <axis xyz="0 0 1" />
    <limit effort="1000" velocity="1000"/>
    <dynamics damping="${bearing_damping}" friction="${bearing_friction}"/>
</joint>


<link name="left_back_wheel_link">
    <visual>
        <geometry>
            <cylinder radius="${wheel_rad}" length="${wheel_wid}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="black" />
    </visual>
    <collision>
        <geometry>
            <cylinder radius="${wheel_rad}" length="${wheel_wid}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
    </collision>
    <inertial>
        <mass value="${wheel_mass}"/>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <inertia 
                 ixx="${1./12. * wheel_mass * ((wheel_rad_sq * 3) + (wheel_wid_sq))}" 
                 iyy="${1./12. * wheel_mass * ((wheel_rad_sq * 3) + (wheel_wid_sq))}" 
                 izz="${1./12. * wheel_mass * (wheel_rad_sq)}" 
                 ixy="0.0" ixz="0.0" iyz="0.0" 
                 />
    </inertial>
</link> 

<joint name="base_left_back_wheel_joint" type="continuous">
    <parent link="base_link" />
    <child link="left_back_wheel_link" />
    <origin xyz="${box_len/4} ${-box_wid/2 -wheel_wid/2} 0" rpy="1.57 0 0" />
    <axis xyz="0 0 1" />
    <limit effort="1000" velocity="1000"/>
    <dynamics damping="${bearing_damping}" friction="${bearing_friction}"/>
</joint>



<link name="right_back_wheel_link">
    <visual>
        <geometry>
            <cylinder radius="${wheel_rad}" length="${wheel_wid}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="black" />
    </visual>
    <collision>
        <geometry>
            <cylinder radius="${wheel_rad}" length="${wheel_wid}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
    </collision>
    <inertial>
        <mass value="${wheel_mass}"/>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <inertia 
                 ixx="${1./12. * wheel_mass * ((wheel_rad_sq * 3) + (wheel_wid_sq))}" 
                 iyy="${1./12. * wheel_mass * ((wheel_rad_sq * 3) + (wheel_wid_sq))}" 
                 izz="${1./12. * wheel_mass * (wheel_rad_sq)}" 
                 ixy="0.0" ixz="0.0" iyz="0.0" 
                 />
    </inertial>
</link> 
<joint name="base_right_back_wheel_joint" type="continuous">
    <parent link="base_link" />
    <child link="right_back_wheel_link" />
    <origin xyz="${box_len/4} ${box_wid/2 + wheel_wid/2} 0" rpy="1.57 0 0" />
    <axis xyz="0 0 1" />
    <limit effort="1000" velocity="1000"/>
    <dynamics damping="${bearing_damping}" friction="${bearing_friction}"/>
</joint>

my_robot_control.xacro:

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:property name="robot_namespace" value="my_robot"/>
    <xacro:property name="hardware_plugin" value="my_robot/MyRobotHardware"/>

    <ros2_control name="${robot_namespace}_controller" type="system">
    <hardware>
        <plugin>${hardware_plugin}</plugin>
                <param name="gpio_pins">
                    <!-- Example GPIO pin configuration -->
                    <param name="L_IN1" type="int">20</param>
                    <param name="L_IN2" type="int">21</param>
                    <param name="L_PWM1" type="int">0</param>
                    <param name="L_IN3" type="int">22</param>
                    <param name="L_IN4" type="int">23</param>
                    <param name="L_PWM2" type="int">1</param>
                    <param name="R_IN1" type="int">24</param>
                    <param name="R_IN2" type="int">25</param>
                    <param name="R_PWM1" type="int">12</param>
                    <param name="R_IN3" type="int">26</param>
                    <param name="R_IN4" type="int">27</param>
                    <param name="R_PWM2" type="int">13</param>
                </param>

        </hardware>
        <joint name="base_left_front_wheel_joint">
            <command_interface name="velocity"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>
        <joint name="base_left_back_wheel_joint">
            <command_interface name="velocity"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>
        <joint name="base_right_front_wheel_joint">
            <command_interface name="velocity"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>
        <joint name="base_right_back_wheel_joint">
            <command_interface name="velocity"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>
    </ros2_control>
</robot>

my_robot_controllers.yaml:

controller_manager:
  ros__parameters:
    update_rate: 50


    diff_cont:
      type: diff_drive_controller/DiffDriveController

    joint_broad:
      type: joint_state_broadcaster/JointStateBroadcaster



diff_cont:
  ros__parameters:

    publish_rate: 50.0

    base_frame_id: base_footprint
    wheels_per_side: 2

    left_wheel_names: [ "base_left_back_wheel_joint", "base_left_front_wheel_joint"]
    right_wheel_names: ["base_right_back_wheel_joint", "base_right_front_wheel_joint"]
    odom_frame_id: odom

    wheel_separation: 0.145
    wheel_radius: 0.03


    open_loop: true
    enable_odom_tf: true

    cmd_vel_timeout: 0.5
    publish_limited_velocity: true
    velocity_rolling_window_size: 10

    # Velocity and acceleration limits
    # Whenever a min_* is unspecified, default to -max_*
    linear.x.has_velocity_limits: true
    linear.x.has_acceleration_limits: true
    linear.x.has_jerk_limits: false
    linear.x.max_velocity: 1.0
    linear.x.min_velocity: -1.0
    linear.x.max_acceleration: 1.0
    linear.x.max_jerk: 0.0
    linear.x.min_jerk: 0.0

    angular.z.has_velocity_limits: true
    angular.z.has_acceleration_limits: true
    angular.z.has_jerk_limits: false
    angular.z.max_velocity: 1.0
    angular.z.min_velocity: -1.0
    angular.z.max_acceleration: 1.0
    angular.z.min_acceleration: -1.0
    angular.z.max_jerk: 0.0
    angular.z.min_jerk: 0.0

    use_stamped_vel: false

my_robot.launch.py:

def generate_launch_description():
    # Include rsp.launch.py
    rsp = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('my_robot'), 'bringup', 'launch', 'rsp.launch.py'
        )]), launch_arguments={'use_sim_time': 'false', 'use_ros2_control': 'true'}.items()
    )

    
    robot_controllers = os.path.join(get_package_share_directory('my_robot'), 'bringup', 'config', 'my_robot_controllers.yaml')
    robot_description = Command(['ros2 param get --hide-type /robot_state_publisher robot_description'])


    # Control node
    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[{'robot_description': robot_description},
                    robot_controllers]
    )

    # TimerAction to delay the controller manager
    delayed_controller_manager = TimerAction(period=5.0, actions=[control_node])

    # Spawner nodes
    diff_drive_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['diff_cont', '--controller-manager', '/controller_manager'],
    )

    joint_broad_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['joint_broad', '--controller-manager', '/controller_manager'],
    )

    # Delayed spawner handlers
    delayed_diff_drive_spawner = RegisterEventHandler(
        event_handler=OnProcessStart(
            target_action=control_node,
            on_start=[diff_drive_spawner],
        )
    )

    delayed_joint_broad_spawner = RegisterEventHandler(
        event_handler=OnProcessStart(
            target_action=control_node,
            on_start=[joint_broad_spawner],
        )
    )

    return LaunchDescription([
        rsp,
        delayed_controller_manager,
        delayed_diff_drive_spawner,
        delayed_joint_broad_spawner
    ])

my_robot_hardware.hpp:

namespace my_robot
{
  class MyRobotHardware : public hardware_interface::SystemInterface
  {
  public:
    RCLCPP_SHARED_PTR_DEFINITIONS(MyRobotHardware);

    hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
    hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
    hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;

    hardware_interface::return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override;
    hardware_interface::return_type write(const rclcpp::Time &time, const rclcpp::Duration &period) override;

    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

  private:
    // Member variables to hold the state and command values
    std::vector<double> hw_positions_;
    std::vector<double> hw_velocities_;
    std::vector<double> hw_commands_;
    std::vector<std::string> joint_names_;
    size_t number_of_joints_;

    // Motor control pins
    int pi_;
    int L_IN1, L_IN2, L_PWM1, L_IN3, L_IN4, L_PWM2;
    int R_IN1, R_IN2, R_PWM1, R_IN3, R_IN4, R_PWM2;
  };
} // namespace my_robot

#endif // MY_ROBOT_HARDWARE_HPP_

my_robot_hardware.cpp:

namespace my_robot
{

    hardware_interface::CallbackReturn MyRobotHardware::on_init(const hardware_interface::HardwareInfo & info)
    {
        if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS)
        {
            return hardware_interface::CallbackReturn::ERROR;
        }
        for (const hardware_interface::ComponentInfo &joint : info_.joints)
        {
            // DiffBotSystem has exactly two states and one command interface on each joint
            if (joint.command_interfaces.size() != 1)
            {
                RCLCPP_FATAL(
                    rclcpp::get_logger("DiffBotSystemHardware"),
                    "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
                    joint.command_interfaces.size());
                return hardware_interface::CallbackReturn::ERROR;
            }

            if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY)
            {
                RCLCPP_FATAL(
                    rclcpp::get_logger("DiffBotSystemHardware"),
                    "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
                    joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
                return hardware_interface::CallbackReturn::ERROR;
            }

            if (joint.state_interfaces.size() != 2)
            {
                RCLCPP_FATAL(
                    rclcpp::get_logger("DiffBotSystemHardware"),
                    "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
                    joint.state_interfaces.size());
                return hardware_interface::CallbackReturn::ERROR;
            }

            if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
            {
                RCLCPP_FATAL(
                    rclcpp::get_logger("DiffBotSystemHardware"),
                    "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(),
                    joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
                return hardware_interface::CallbackReturn::ERROR;
            }

            if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY)
            {
                RCLCPP_FATAL(
                    rclcpp::get_logger("DiffBotSystemHardware"),
                    "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),
                    joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
                return hardware_interface::CallbackReturn::ERROR;
            }
        }

        number_of_joints_ = info.joints.size();
        hw_positions_.resize(number_of_joints_, 0.0);
        hw_velocities_.resize(number_of_joints_, 0.0);
        hw_commands_.resize(number_of_joints_, 0.0);
        joint_names_.resize(number_of_joints_);
        


        for (size_t i = 0; i < number_of_joints_; ++i)
        {
            joint_names_[i] = info.joints[i].name;
        }

        // Initialize pigpio
        pi_ = pigpio_start(nullptr, nullptr);
        if (pi_ < 0)
        {
            RCLCPP_ERROR(rclcpp::get_logger("MyRobotHardware"), "Failed to initialize pigpio");
            return hardware_interface::CallbackReturn::ERROR;
        }

        // Define motor control pins
        L_IN1 = 20;
        L_IN2 = 21;
        L_PWM1 = 0;
        L_IN3 = 22;
        L_IN4 = 23;
        L_PWM2 = 1;
        R_IN1 = 24;
        R_IN2 = 25;
        R_PWM1 = 12;
        R_IN3 = 26;
        R_IN4 = 27;
        R_PWM2 = 13;

        // Set up motor control pins
        set_mode(pi_, L_IN1, PI_OUTPUT);
        set_mode(pi_, L_IN2, PI_OUTPUT);
        set_mode(pi_, L_PWM1, PI_OUTPUT);
        set_mode(pi_, L_IN3, PI_OUTPUT);
        set_mode(pi_, L_IN4, PI_OUTPUT);
        set_mode(pi_, L_PWM2, PI_OUTPUT);
        set_mode(pi_, R_IN1, PI_OUTPUT);
        set_mode(pi_, R_IN2, PI_OUTPUT);
        set_mode(pi_, R_PWM1, PI_OUTPUT);
        set_mode(pi_, R_IN3, PI_OUTPUT);
        set_mode(pi_, R_IN4, PI_OUTPUT);
        set_mode(pi_, R_PWM2, PI_OUTPUT);

        RCLCPP_INFO(rclcpp::get_logger("MyRobotHardware"), "Hardware interface initialized successfully.");
        return hardware_interface::CallbackReturn::SUCCESS;
    }

    std::vector<hardware_interface::StateInterface> MyRobotHardware::export_state_interfaces()
    {
        std::vector<hardware_interface::StateInterface> state_interfaces;

        state_interfaces.emplace_back(hardware_interface::StateInterface(
            joint_names_[0], hardware_interface::HW_IF_POSITION, &hw_positions_[0]));
        state_interfaces.emplace_back(hardware_interface::StateInterface(
            joint_names_[1], hardware_interface::HW_IF_POSITION, &hw_positions_[1]));
        state_interfaces.emplace_back(hardware_interface::StateInterface(
            joint_names_[2], hardware_interface::HW_IF_POSITION, &hw_positions_[2]));
        state_interfaces.emplace_back(hardware_interface::StateInterface(
            joint_names_[3], hardware_interface::HW_IF_POSITION, &hw_positions_[3]));

        state_interfaces.emplace_back(hardware_interface::StateInterface(
            joint_names_[0], hardware_interface::HW_IF_VELOCITY, &hw_velocities_[0]));
        state_interfaces.emplace_back(hardware_interface::StateInterface(
            joint_names_[1], hardware_interface::HW_IF_VELOCITY, &hw_velocities_[1]));
        state_interfaces.emplace_back(hardware_interface::StateInterface(
            joint_names_[2], hardware_interface::HW_IF_VELOCITY, &hw_velocities_[2]));
        state_interfaces.emplace_back(hardware_interface::StateInterface(
            joint_names_[3], hardware_interface::HW_IF_VELOCITY, &hw_velocities_[3]));

        return state_interfaces;
    }

    std::vector<hardware_interface::CommandInterface> MyRobotHardware::export_command_interfaces()
    {
        std::vector<hardware_interface::CommandInterface> command_interfaces;

        command_interfaces.emplace_back(hardware_interface::CommandInterface(
            joint_names_[0], hardware_interface::HW_IF_VELOCITY, &hw_commands_[0]));
        command_interfaces.emplace_back(hardware_interface::CommandInterface(
            joint_names_[1], hardware_interface::HW_IF_VELOCITY, &hw_commands_[1]));
        command_interfaces.emplace_back(hardware_interface::CommandInterface(
            joint_names_[2], hardware_interface::HW_IF_VELOCITY, &hw_commands_[2]));
        command_interfaces.emplace_back(hardware_interface::CommandInterface(
            joint_names_[3], hardware_interface::HW_IF_VELOCITY, &hw_commands_[3]));

        return command_interfaces;
    }

    hardware_interface::CallbackReturn MyRobotHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
    {
        std::fill(hw_commands_.begin(), hw_commands_.end(), 0.0);
        RCLCPP_INFO(rclcpp::get_logger("MyRobotHardware"), "Hardware interface activated successfully.");
        return hardware_interface::CallbackReturn::SUCCESS;
    }

    hardware_interface::CallbackReturn MyRobotHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
    {
        gpio_write(pi_, L_IN1, 0);
        gpio_write(pi_, L_IN2, 0);
        gpio_write(pi_, L_IN3, 0);
        gpio_write(pi_, L_IN4, 0);
        gpio_write(pi_, R_IN1, 0);
        gpio_write(pi_, R_IN2, 0);
        gpio_write(pi_, R_IN3, 0);
        gpio_write(pi_, R_IN4, 0);

        RCLCPP_INFO(rclcpp::get_logger("MyRobotHardware"), "Hardware interface deactivated successfully.");
        return hardware_interface::CallbackReturn::SUCCESS;
    }

    hardware_interface::return_type MyRobotHardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration &period)
    {
        for (std::size_t i = 0; i < hw_velocities_.size(); i++)
        {
            hw_positions_[i] = hw_positions_[i] + period.seconds() * hw_velocities_[i];

            RCLCPP_INFO(
                rclcpp::get_logger("MyRobotHardware"),
                "Got position state %.5f and velocity state %.5f for '%s'!", hw_positions_[i],
                hw_velocities_[i], joint_names_[i].c_str());
        }
        return hardware_interface::return_type::OK;
    }

    hardware_interface::return_type MyRobotHardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
    {

        // Write commands to hardware here using pigpio
        int left_front_speed = static_cast<int>(hw_commands_[0] * 255);  // Scale command to PWM range
        int left_rear_speed = static_cast<int>(hw_commands_[1] * 255);   // Scale command to PWM range
        int right_front_speed = static_cast<int>(hw_commands_[2] * 255); // Scale command to PWM range
        int right_rear_speed = static_cast<int>(hw_commands_[3] * 255);  // Scale command to PWM range

        hw_velocities_[0] = hw_commands_[0];
        hw_velocities_[1] = hw_commands_[1];
        hw_velocities_[2] = hw_commands_[2];
        hw_velocities_[3] = hw_commands_[3];

        // Left front motor control

        // Left front motor control
        if (left_front_speed >= 0)
        {
            gpio_write(pi_, L_IN1, 0);
            gpio_write(pi_, L_IN2, 1);
        }
        else
        {
            gpio_write(pi_, L_IN1, 1);
            gpio_write(pi_, L_IN2, 0);
        }
        set_PWM_dutycycle(pi_, L_PWM1, abs(left_front_speed));

        // Left rear motor control
        if (left_rear_speed >= 0)
        {
            gpio_write(pi_, L_IN3, 1);
            gpio_write(pi_, L_IN4, 0);
        }
        else
        {
            gpio_write(pi_, L_IN3, 0);
            gpio_write(pi_, L_IN4, 1);
        }
        set_PWM_dutycycle(pi_, L_PWM2, abs(left_rear_speed));

        // Right front motor control
        if (right_front_speed >= 0)
        {
            gpio_write(pi_, R_IN1, 1);
            gpio_write(pi_, R_IN2, 0);
        }
        else
        {
            gpio_write(pi_, R_IN1, 0);
            gpio_write(pi_, R_IN2, 1);
        }
        set_PWM_dutycycle(pi_, R_PWM1, abs(right_front_speed));

        // Right rear motor control
        if (right_rear_speed >= 0)
        {
            gpio_write(pi_, R_IN3, 1);
            gpio_write(pi_, R_IN4, 0);
        }
        else
        {
            gpio_write(pi_, R_IN3, 0);
            gpio_write(pi_, R_IN4, 1);
        }
        set_PWM_dutycycle(pi_, R_PWM2, abs(right_rear_speed));

        RCLCPP_INFO(
            rclcpp::get_logger("MyRobotHardware"),
            "Commands: LF: %d, LR: %d, RF: %d, RR: %d",
            left_front_speed, left_rear_speed, right_front_speed, right_rear_speed);

        return hardware_interface::return_type::OK;
    }

} // namespace my_robot

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(my_robot::MyRobotHardware, hardware_interface::SystemInterface)

Please, help me.

$\endgroup$
2
  • $\begingroup$ Welcome at RSE. Please format your question properly, and it seems that you have copied the xacro twice instead of the controller yaml. $\endgroup$ Commented Jun 26 at 5:48
  • $\begingroup$ I appreciate the feedback, I had no idea how. I have updated accordingly. $\endgroup$ Commented Jun 26 at 10:15

2 Answers 2

1
$\begingroup$

The issue was me not properly reading the documentation for the wheel movements and which GPIO pins should be set to HIGH for forward motion. I set things in reverse.

$\endgroup$
0
$\begingroup$

First, I'd suggest to use the MockHardware to check if your ros2-control settings and URDF are correct.

What do you mean by "moving opposite direction", what value are you observing?

Have you checked ros2 control list_hardware_interfaces, are all interfaces claimed as you would expect?

$\endgroup$
5
  • $\begingroup$ Thank you so much for your fast response. If the right front wheel is moving forward, the right rear moving backwards and vice versa. I am currently unable to run the command, but I will as soon as I get a hold of the hardware again. $\endgroup$ Commented Jun 26 at 19:40
  • $\begingroup$ Thanks. I will do that. Please, my robot is a 4wd with independent motors that use gpio interfaces. Please, is using a differential controller okay? $\endgroup$ Commented Jun 26 at 19:53
  • $\begingroup$ I don't know the car, but from picture it seems that diff_drive is the correct kinematics. If you don't have access to the hardware, use the mockhardware as I've suggested. $\endgroup$ Commented Jun 27 at 8:58
  • $\begingroup$ I have access now, and ros2 control list_hardware_interface is showing the correct state and command interfaces: command interfaces base_left_back_wheel_joint/velocity [available] [claimed] base_left_front_wheel_joint/velocity [available] [claimed] base_right_back_wheel_joint/velocity [available] [claimed] base_right_front_wheel_joint/velocity [available] [claimed] state interfaces $\endgroup$ Commented Jun 27 at 9:20
  • $\begingroup$ Have you tried the mock hardware? are you sure that your URDF and ros2_control config is correct? $\endgroup$ Commented Jun 29 at 6:26

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.