brian2lee
brian2lee

Reputation: 411

teleop_twist_keyboard moving the bot in reverse

Tried to control my gazebo bot through teleop_twist_keyboard.

rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=forklift/cmd_vel

However the bot ends up going in reverse (for example pressing "." should go backward by default, but my bot go forward). Checked my wheel joints axis and it looks fine to me, have no idea what's wrong. Here's my bot urdf code down below, and the project's git just in case more information is needed for this issue.

forklift.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="forklift">
    
    <link name="base_footprint"/>

    <link name="base_link">
        <inertial>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <mass value="50.0"/>
            <!--
            <inertia ixx="0.05" ixy="0.05" ixz="0.05" iyy="1.3419385416666667" iyz="1.3419385416666667" izz="2.296875"/>
            -->
            <inertia ixx="1.3419385416666667"
                iyy="1.3419385416666667"
                izz="2.296875"
                ixy="0.0"
                ixz="0.0"
                iyz="0.0"/>
        </inertial>
        <visual name="">
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.525 0.525 0.2155"/>
            </geometry>
            <material name="blue">
                <color rgba="0.0 0.0 1.0 1.0"/>
                <texture filename=""/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.525 0.525 0.2155"/>
            </geometry>
            <!--
            <surface>
                <friction>
                    <ode>
                        <mu>100</mu>
                        <mu2>100</mu2>
                    </ode>
                </friction>
            </surface>
            -->
        </collision>
    </link>
    <gazebo reference="base_link">
        <material>Gazebo/Blue</material>
    </gazebo>
    <joint name="base_joint" type="fixed">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <parent link="base_footprint"/>
        <child link="base_link"/>
    </joint>

<!--    ***   wheels   ***   -->    

    <xacro:macro name="wheel" params="name x y">
        <link name="wheel_link_${name}">
            <inertial>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <mass value="1.0"/>
                <!--
                <inertia ixx="0.05" ixy="0.05" ixz="0.05" 
                    iyy="0.0009333333333333333" iyz="0.0009333333333333333" izz="0.0018"/>
                -->        
                <inertia ixx="0.0009333333333333333"  
                    iyy="0.0009333333333333333"  
                    izz="0.0018"
                    ixy="0.0"
                    ixz="0.0"
                    iyz="0.0"/>
                </inertial>
            <visual name="">
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.06" length="0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 1.0"/>
                    <texture filename=""/>
                </material>
            </visual>
            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.06" length="0.02"/>
                </geometry>
                <!--
                <surface>
                    <friction>
                        <ode>
                            <mu>100</mu>
                            <mu2>100</mu2>
                        </ode>
                    </friction>
                </surface>
                -->
            </collision>
        </link>
        <gazebo reference="wheel_link_${name}">
            <material>Gazebo/Black</material>
        </gazebo>

        <joint name="wheel_joint_${name}" type="continuous">
            <origin xyz="${x} ${y} -0.100250" rpy="1.57 0.0 0.0"/>
            <parent link="base_link"/>
            <child link="wheel_link_${name}"/>
            <axis xyz="0.0 0.0 1.0"/>
            <limit effort="10000" velocity="1000" />
        <dynamics damping="1.0" friction="1.0" />
        </joint>
    </xacro:macro>

<!--   ***   fork base   ***   -->
    <xacro:macro name="fork_base" params="name y">
        <link name="forkbase_link_${name}">
            <inertial>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <mass value="0.5"/>
                <inertia ixx="0.0005208333333333334"  
                    iyy="0.0004208333333333334"  
                    izz="0.00010833333333333334"
                    ixy="0.0"
                    ixz="0.0"
                    iyz="0.0"/>
            </inertial>
            <visual name="">
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.01 0.05 0.1"/>
                </geometry>
                <material name="green">
                    <color rgba="0.0 1.0 0.0 1.0"/>
                    <texture filename=""/>
                </material>
            </visual>
            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.01 0.05 0.1"/>
                </geometry>
            </collision>
        </link>
        <gazebo reference="forkbase_link_${name}">
            <material>Gazebo/Green</material>
        </gazebo>

        <joint name="forkbase_link_${name}" type="fixed">
            <origin xyz="0.2675 ${y} -0.10025" rpy="0.0 0.0 0.0"/>
            <parent link="base_link"/>
            <child link="forkbase_link_${name}"/>
        </joint>
    </xacro:macro>

<!--   ***   fork   ***   -->
    <xacro:macro name="fork_link" params="name">
        <link name="fork_link_${name}">
            <inertial>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <mass value="0.5"/>
                <inertia ixx="0.00010833333333333334"  
                    iyy="0.0026083333333333336"  
                    izz="0.0027083333333333334"
                    ixy="0.0"
                    ixz="0.0"
                    iyz="0.0"/>
            </inertial>
            <visual name="">
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.25 0.05 0.01"/>
                </geometry>
                <material name="green">
                    <color rgba="0.0 1.0 0.0 1.0"/>
                    <texture filename=""/>
                </material>
            </visual>
            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.25 0.05 0.01"/>
                </geometry>
            </collision>
        </link>
        <gazebo reference="fork_link_${name}">
            <material>Gazebo/Green</material>
        </gazebo>

        <joint name="fork_link_${name}" type="fixed">
            <origin xyz="0.13 0 -0.045" rpy="0.0 0.0 0.0"/>
            <parent link="forkbase_link_${name}"/>
            <child link="fork_link_${name}"/>
        </joint>
    </xacro:macro>

    <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
    <xacro:sensor_d435 name="camera" topics_ns="camera" parent="base_link" publish_pointcloud="true">
        <origin xyz="0.2625 0 -0.10775" rpy="0 0 0" />
    </xacro:sensor_d435>  

<!--   ***   combine component   ***   -->

    <xacro:wheel name="back_right" x="-0.150000" y ="-0.272513"/>
    <xacro:wheel name="back_left" x="-0.150000" y ="0.272513"/>
    <xacro:wheel name="front_right" x="0.150000" y ="-0.272513"/>
    <xacro:wheel name="front_left" x="0.150000" y ="0.272513"/>

    <xacro:fork_base name="right" y ="-0.055"/>
    <xacro:fork_base name="left" y ="0.055"/>

    <xacro:fork_link name="right"/>
    <xacro:fork_link name="left"/>
    
    <gazebo>
    
    <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
        <updateRate>10.0</updateRate>
        <robotNamespace>/forklift</robotNamespace>
        <leftFrontJoint>wheel_joint_front_left</leftFrontJoint>
        <rightFrontJoint>wheel_joint_front_right</rightFrontJoint>
        <leftRearJoint>wheel_joint_back_left</leftRearJoint>
        <rightRearJoint>wheel_joint_back_right</rightRearJoint>
        <wheelSeparation>0.4</wheelSeparation>
        <wheelDiameter>0.12</wheelDiameter>
        <robotBaseFrame>base_footprint</robotBaseFrame>
        <torque>10</torque>

        <topicName>cmd_vel</topicName>
        <odometryTopic>odom</odometryTopic>
        <odometryFrame>odom</odometryFrame>

        <commandTopic>cmd_vel</commandTopic>
        <topic_name_twist>cmd_vel</topic_name_twist>
        <topic_name_odometry>odom</topic_name_odometry>
        <topic_name_joint>joint</topic_name_joint>

        <broadcastTF>true</broadcastTF>

        <covariance_x>0.0001</covariance_x>
        <covariance_y>0.0001</covariance_y>
        <covariance_yaw>0.01</covariance_yaw>

    </plugin>

    </gazebo>

</robot>

Upvotes: 0

Views: 60

Answers (0)

Related Questions