0
$\begingroup$

I am trying to simulate a 6 wheeled robot in Ignition Gazebo Fortress, running ROS2 Humble under Ubuntu 22.04.

I imagine there are multiple possible approaches to this, but I have chosen to use the DiffDrive plugin to control the middle wheels and leave the others free and without friction.

When merely displaying in RViz2 and Joint State Publisher Gui, everything is fine. When actually running the simulation and using "youship/base_link" as Fixed Frame in Rviz2, everything works fine. However, when the FixedFrame is "youship/odom", the wheels loose color and there are errors sying there is no transform between "youship/odom" and "youship/<wheel_name>". Simulation with youship/base_link as fixed frame Simulation with youship/odom as fixed frame

My youship.xacro is:

<?xml version="1.0"?>
<robot name="idmind_youship" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- idmind_6wd Model -->
<xacro:property name="robot_name"   value="youship" />
<xacro:property name="base_mass"    value="30.00" />
<xacro:property name="base_length"  value="0.660" />
<xacro:property name="base_width"   value="0.510" />
<xacro:property name="base_height"  value="0.170" />
<xacro:property name="body_mass"    value="20.00" />
<xacro:property name="body_length"  value="0.660" />
<xacro:property name="body_width"   value="0.510" />
<xacro:property name="body_height"  value="0.400" />
<xacro:property name="wheel_width"  value="0.045" />
<xacro:property name="wheel_radius" value="0.086" />
<xacro:property name="wheel_mass"   value="4.550" />

<!-- Xacro sources -->
<!-- RViz colors -->
<xacro:include filename="$(find idmind_youship)/models/xacros/materials.xacro" />

<!-- Inertia macros  -->
<xacro:include filename="$(find idmind_youship)/models/xacros/inertias.xacro" />

<!-- IMU Models -->
<xacro:include filename="$(find idmind_imu)/models/imu_brick_v2/imu_brick_v2.xacro" />
<!-- Lasers Models -->
<xacro:include filename="$(find neuvition_w1_lidar)/models/neuvition_w1.xacro" />
<!-- Wheel Models -->
<xacro:include filename="$(find idmind_youship)/models/xacros/wheels.xacro" />
<!-- Lid Models -->
<xacro:include filename="$(find idmind_youship)/models/xacros/ros2_controllers.xacro" />

<!-- Robot base -->
<link name="${robot_name}/base_link">
    <visual>
        <geometry>
            <box size="${base_length} ${base_width} ${base_height}" />
        </geometry>
        <material name="aluminium" />
        <origin rpy="0 0 0" xyz="0 0 ${base_height/2.0}"/>
    </visual>
    <collision>
        <geometry>
            <box size="${base_length} ${base_width} ${base_height}" />
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 ${base_height/2}"/>
    </collision>
    <inertial>
        <mass value="${base_mass}" />
        <xacro:box_inertia m="${base_mass}" x="${base_length}" y="${base_width}" z="${base_height}"  />
        <origin rpy="0 0 0" xyz="0 0 ${base_height/2.0}"/>
    </inertial>
</link>
<link name="${robot_name}/dummy"></link>
<joint name="${robot_name}_base_link_to_dummy" type="fixed">
    <parent link="${robot_name}/base_link"/>
    <child link="${robot_name}/dummy"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
</joint>
<link name="${robot_name}/base_body">
    <visual>
        <geometry>
            <box size="${body_length} ${body_width} ${body_height}" />
        </geometry>
        <material name="white" />
        <origin rpy="0 0 0" xyz="0 0 ${body_height/2.0}"/>
    </visual>
    <collision>
        <geometry>
            <box size="${body_length} ${body_width} ${body_height}" />
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 ${base_height/2.0}"/>
    </collision>
    <inertial>
        <mass value="${body_mass}" />
        <xacro:box_inertia m="${body_mass}" x="${body_length}" y="${body_width}" z="${body_height}"  />
        <origin rpy="0 0 0" xyz="0 0 ${body_height/2.0}"/>
    </inertial>
</link>
<joint name="${robot_name}_base_link_to_body" type="fixed">
    <parent link="${robot_name}/base_link"/>
    <child link="${robot_name}/base_body"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.17" />
</joint>

<link name="${robot_name}/base_lid">
    <visual>
        <geometry>
            <box size="${body_length} ${body_width} 0.03" />
        </geometry>
        <material name="red" />
        <origin rpy="0 0 0" xyz="${body_length/2.0} 0.0 ${0.03/2.0}"/>
    </visual>
    <collision>
        <geometry>
            <box size="${body_length} ${body_width} 0.03" />
        </geometry>
        <origin rpy="0 0 0" xyz="${body_length/2.0} 0 ${0.03/2.0}"/>
    </collision>
    <inertial>
        <mass value="${0.2}" />
        <xacro:box_inertia m="${0.2}" x="${body_length}" y="${body_width}" z="${0.03}"  />
        <origin rpy="0 0 0" xyz="${body_length/2.0} 0 ${0.03/2.0}"/>
    </inertial>
</link>
<xacro:single_actuator name="${robot_name}_base_to_lid" prefix="${robot_name}" parent="${robot_name}/base_body" child="${robot_name}/base_lid" limit_min="0" limit_max="1.57">
    <!-- <origin rpy="0.0 -1.57 0.0" xyz="-${body_length/2.0} 0.0 ${body_height}" /> -->
    <origin rpy="0.0 0.0 0.0" xyz="-${body_length/2.0} 0.0 ${body_height}" />
</xacro:single_actuator>

<!-- Wheels -->
<xacro:free_wheel wheel_name="front_left" prefix="${robot_name}" parent="${robot_name}/base_link" wheel_width="${wheel_width}" wheel_radius="${wheel_radius}" wheel_mass="${wheel_mass}">
    <origin rpy="0.0 0.0 0.0" xyz="0.205 ${base_width/2.0+wheel_width} 0.055" />
</xacro:free_wheel>
<xacro:free_wheel wheel_name="front_right" prefix="${robot_name}" parent="${robot_name}/base_link" wheel_width="${wheel_width}" wheel_radius="${wheel_radius}" wheel_mass="${wheel_mass}">
    <origin rpy="0.0 0.0 0.0" xyz="0.205 -${base_width/2.0+wheel_width} 0.055" />
</xacro:free_wheel>
<xacro:wheel wheel_name="middle_left" prefix="${robot_name}" parent="${robot_name}/base_link" wheel_width="${wheel_width}" wheel_radius="${wheel_radius}" wheel_mass="${wheel_mass}">
    <origin rpy="0.0 0.0 0.0" xyz="0.0 ${base_width/2.0+wheel_width} 0.055" />
</xacro:wheel>
<xacro:wheel wheel_name="middle_right" prefix="${robot_name}" parent="${robot_name}/base_link" wheel_width="${wheel_width}" wheel_radius="${wheel_radius}" wheel_mass="${wheel_mass}">
    <origin rpy="0.0 0.0 0.0" xyz="0.0 -${base_width/2.0+wheel_width} 0.055" />
</xacro:wheel>
<xacro:free_wheel wheel_name="back_left" prefix="${robot_name}" parent="${robot_name}/base_link" wheel_width="${wheel_width}" wheel_radius="${wheel_radius}" wheel_mass="${wheel_mass}">
    <origin rpy="0.0 0.0 0.0" xyz="-0.205 ${base_width/2.0+wheel_width} 0.055" />
</xacro:free_wheel>
<xacro:free_wheel wheel_name="back_right" prefix="${robot_name}" parent="${robot_name}/base_link" wheel_width="${wheel_width}" wheel_radius="${wheel_radius}" wheel_mass="${wheel_mass}">
    <origin rpy="0.0 0.0 0.0" xyz="-0.205 -${base_width/2.0+wheel_width} 0.055" />
</xacro:free_wheel>

<!-- Sensors -->
<xacro:imu_brick_v2 imu_name="imu" parent="${robot_name}/base_link" prefix="${robot_name}">
    <origin rpy="0 0 0" xyz="0.0 0.0 ${1.01*base_height}" />
</xacro:imu_brick_v2>

<xacro:neuvition_w1 laser_name="front_laser" parent="${robot_name}/base_link" prefix="${robot_name}" visualize="true">
    <origin rpy="3.14 0 0" xyz="0.33 0.0 ${1.01*base_height}" />
    <!-- <origin rpy="0 0 0" xyz="0.55 0.0 ${1.01*base_height}" /> -->
</xacro:neuvition_w1>

<gazebo>
    <plugin filename="ignition-gazebo-joint-state-publisher-system" name="ignition::gazebo::systems::JointStatePublisher">
        <joint_name>${robot_name}_base_to_front_left</joint_name>
        <joint_name>${robot_name}_base_to_front_right</joint_name>
        <joint_name>${robot_name}_base_to_middle_left</joint_name>            
        <joint_name>${robot_name}_base_to_middle_right</joint_name>
        <joint_name>${robot_name}_base_to_back_left</joint_name>
        <joint_name>${robot_name}_base_to_back_right</joint_name>
        <joint_name>${robot_name}_base_to_lid</joint_name>
        <topic>${robot_name}/joint_states</topic>
    </plugin>
</gazebo>

<!-- Diff Drive plugin https://github.com/gazebosim/gz-sim/blob/gz-sim7/src/systems/diff_drive/DiffDrive.cc -->
<gazebo>
    <plugin filename="libignition-gazebo-diff-drive-system.so" name="ignition::gazebo::systems::DiffDrive">
        <!-- wheels -->
        <left_joint>${robot_name}_base_to_middle_left</left_joint>
        <right_joint>${robot_name}_base_to_middle_right</right_joint>

        <!-- kinematics -->
        <wheel_separation>${base_width}</wheel_separation>
        <wheel_radius>${wheel_radius}</wheel_radius>
        <topic>${robot_name}/cmd_vel</topic>

        <!-- limits -->
        <max_wheel_torque>20</max_wheel_torque>
        <max_wheel_acceleration>20.0</max_wheel_acceleration>
        <!-- <min_velocity>0.01</min_velocity>
        <min_linear_velocity>0.01</min_linear_velocity>
        <min_angular_velocity>0.01</min_angular_velocity> -->
        <max_velocity>5.0</max_velocity>
        <max_linear_velocity>5.0</max_linear_velocity>
        <max_angular_velocity>6.28</max_angular_velocity>
        <!-- <min_acceleration>0.01</min_acceleration>
        <min_linear_acceleration>0.01</min_linear_acceleration>
        <min_angular_acceleration>0.01</min_angular_acceleration> -->
        <max_acceleration>5.0</max_acceleration>
        <max_linear_accelration>5.0</max_linear_accelration>
        <max_angular_acceleration>6.28</max_angular_acceleration>

        <!-- output -->
        <odom_topic>${robot_name}/idmind_motors/odom</odom_topic>
        <tf_topic>${robot_name}/tf</tf_topic>
        <publish_odom>true</publish_odom>
        <publish_odom_tf>false</publish_odom_tf>
        <publish_wheel_tf>true</publish_wheel_tf>

        <frame_id>${robot_name}/odom</frame_id>
        <child_frame_id>${robot_name}/base_link</child_frame_id>
        <odom_publish_frequency>20</odom_publish_frequency>

    </plugin>
</gazebo>
<!-- <xacro:diff_drive_plugin left_wheel="middle_left" right_wheel="middle_right" wheel_dist="${base_width}" wheel_diameter="${2*wheel_radius}" vel_topic="/cmd_vel" odom_topic="/odom" odom_frame="/odom" robot_frame="/base_link">
</xacro:diff_drive_plugin> -->

My wheels.xacro is:

<?xml version="1.0"?>
<robot name="rovid" xmlns:xacro="http://www.ros.org/wiki/xacro">    
<xacro:macro name="wheel" params="wheel_name prefix parent wheel_radius wheel_width wheel_mass *xyz max_effort:=100000">
    <link name="${prefix}/${wheel_name}">
        <visual>
            <geometry>
                <cylinder length="${wheel_width}" radius="${wheel_radius}" />
            </geometry>
            <origin rpy="1.57 0 0" xyz="0 0 0"/>
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <!--<cylinder length="${wheel_width}" radius="${wheel_radius}" />-->
              <cylinder length="0.01" radius="${wheel_radius}" />
            </geometry>
            <origin rpy="1.57 0 0" xyz="0 0 0"/>
            <surface>
                <friction>
                <ode>
                    <mu>1</mu>
                    <mu2>1</mu2>
                    <slip1>0.035</slip1>
                    <slip2>0</slip2>
                    <fdir1>0 0 1</fdir1>
                </ode>
                <bullet>
                    <friction>1</friction>
                    <friction2>1</friction2>
                    <rolling_friction>0.1</rolling_friction>
                    <spinning_friction>0.1</spinning_friction>
                </bullet>
                </friction>
            </surface>
        </collision>
        <inertial>
            <mass value="${wheel_mass}" />
            <xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}" />
            <origin rpy="1.57075 0 0" xyz="0 0 0"/>
        </inertial>
    </link>
    <joint name="${prefix}_base_to_${wheel_name}" type="continuous">
        <axis xyz="0 1 0" />
        <limit velocity="75.0" effort="${max_effort}" />
        <parent link="${parent}"/>
        <child link="${prefix}/${wheel_name}"/>
        <dynamics damping="30" friction="110"/>
        <xacro:insert_block name="xyz" />
    </joint>
    <gazebo reference="${prefix}/${wheel_name}">
        <material>Gazebo/Orange</material>
        <gravity>true</gravity>
        <selfCollide>false</selfCollide>
        <maxContacts>10</maxContacts>
        <dampingFactor>0.0</dampingFactor>
        <maxVel>0.0</maxVel>
        <minDepth>0.0</minDepth>
        <mu1>10.000000</mu1>
        <mu2>10.000000</mu2>
        <kp>1e15</kp>
        <kd>1e13</kd>
    </gazebo>
</xacro:macro>

<xacro:macro name="free_wheel" params="wheel_name prefix parent wheel_radius wheel_width wheel_mass *xyz">
    <link name="${prefix}/${wheel_name}">
        <visual>
            <geometry>
                <cylinder length="${wheel_width}" radius="${wheel_radius}" />
            </geometry>
            <origin rpy="1.57 0 0" xyz="0 0 0"/>
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <!--<cylinder length="${wheel_width}" radius="${wheel_radius}" />-->
              <cylinder length="0.01" radius="${wheel_radius}" />
            </geometry>
            <origin rpy="1.57 0 0" xyz="0 0 0"/>
            <surface>
                <friction>
                <ode>
                    <mu>0</mu>
                    <mu2>0</mu2>
                    <slip1>0.035</slip1>
                    <slip2>0</slip2>
                    <fdir1>0 0 1</fdir1>
                </ode>
                <bullet>
                    <friction>0</friction>
                    <friction2>0</friction2>
                    <rolling_friction>0.1</rolling_friction>
                    <spinning_friction>0.1</spinning_friction>
                </bullet>
                </friction>
            </surface>

        </collision>
        <inertial>
            <mass value="${wheel_mass}" />
            <xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}" />
            <origin rpy="1.57075 0 0" xyz="0 0 0"/>
        </inertial>
    </link>
    <joint name="${prefix}_base_to_${wheel_name}" type="continuous">
        <axis xyz="0 1 0" />
        <limit velocity="1000.0" effort="0.0" />
        <parent link="${parent}"/>
        <child link="${prefix}/${wheel_name}"/>
        <dynamics damping="0.0" friction="0.0"/>
        <xacro:insert_block name="xyz" />
    </joint>
    <!-- <gazebo reference="${prefix}_base_to_${wheel_name}">
        <material>Gazebo/Orange</material>
        <gravity>true</gravity>
        <selfCollide>false</selfCollide>
        <maxContacts>10</maxContacts>
        <dampingFactor>0.0</dampingFactor>
        <maxVel>0.0</maxVel>
        <minDepth>0.0</minDepth>
        <mu1>0.000001</mu1>
        <mu2>0.000001</mu2>
        <kp>0</kp>
        <kd>0</kd> -->
        <!-- <kp>1e15</kp>
        <kd>1e13</kd> -->
    <!-- </gazebo> -->
</xacro:macro>

In ROS2, I am running all the necessary nodes for simulation, including ign bridges, teleop nodes and robot_localization (wheel odometry and IMU). These all seem to be working in spite of those errors.

What does it seem that I am doing wrong? Thank you

$\endgroup$

1 Answer 1

0
$\begingroup$

I had the same problem and the issue was that the joint_state_publisher was not using simulation time. I solved it adding the parameter in the launch file, like this:

   joint_state_publisher_node =  Node(
            package="joint_state_publisher",
            executable="joint_state_publisher",
            parameters=[{'use_sim_time' : use_sim_time}]
$\endgroup$

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.