Thursday, May 22, 2025

ROS 2 - NAV2 - Setting Up Odometry on Robot - Moving Robot

The odometry system provides a locally accurate estimate of a robot’s pose and velocity based on its motion. The odometry information can be obtained from various sources such as IMU, LIDAR, RADAR, VIO, and wheel encoders. 







Gazebo is a 3D simulator that allows us to observe how our virtual robot will function in a simulated environment. 


Install Gazebo 3D simulator by command [sudo apt install ros-jazzy-ros-gz]



Add Inertial Measurement Unit (IMU) sensor, a Gazebo Plugins to a SDF (Simulation Description Format)



The full code/description of Inertial Measurement Unit (IMU) sensor look like

<joint name='imu_joint' type='fixed'>

  <parent>base_link</parent>

  <child>imu_link</child>

  <pose relative_to="base_link">0.0 0.0 0.01 0 0 0</pose>

</joint>


<link name='imu_link'>

  <pose relative_to="imu_joint"/>

  <visual name="imu_link_visual">

    <geometry>

      <box><size>

        0.1 0.1 0.1

      </size></box>

    </geometry>

  </visual>


  <collision name="imu_link_collision">

    <geometry>

      <box><size>

        0.1 0.1 0.1

      </size></box>

    </geometry>

  </collision>


  <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>


  <sensor name="imu_sensor" type="imu">

    <always_on>true</always_on>

    <update_rate>100</update_rate>

    <visualize>true</visualize>

    <topic>demo/imu</topic>

    <gz_frame_id>imu_link</gz_frame_id>

    <imu>

      <angular_velocity>

        <x>

          <noise type="gaussian">

            <mean>0.0</mean>

            <stddev>2e-4</stddev>

            <bias_mean>0.0000075</bias_mean>

            <bias_stddev>0.0000008</bias_stddev>

          </noise>

        </x>

        <y>

          <noise type="gaussian">

            <mean>0.0</mean>

            <stddev>2e-4</stddev>

            <bias_mean>0.0000075</bias_mean>

            <bias_stddev>0.0000008</bias_stddev>

          </noise>

        </y>

        <z>

          <noise type="gaussian">

            <mean>0.0</mean>

            <stddev>2e-4</stddev>

            <bias_mean>0.0000075</bias_mean>

            <bias_stddev>0.0000008</bias_stddev>

          </noise>

        </z>

      </angular_velocity>

      <linear_acceleration>

        <x>

          <noise type="gaussian">

            <mean>0.0</mean>

            <stddev>1.7e-2</stddev>

            <bias_mean>0.1</bias_mean>

            <bias_stddev>0.001</bias_stddev>

          </noise>

        </x>

        <y>

          <noise type="gaussian">

            <mean>0.0</mean>

            <stddev>1.7e-2</stddev>

            <bias_mean>0.1</bias_mean>

            <bias_stddev>0.001</bias_stddev>

          </noise>

        </y>

        <z>

          <noise type="gaussian">

            <mean>0.0</mean>

            <stddev>1.7e-2</stddev>

            <bias_mean>0.1</bias_mean>

            <bias_stddev>0.001</bias_stddev>

          </noise>

        </z>

      </linear_acceleration>

    </imu>

  </sensor>

</link>



Add DiffDrive plugin and JointStatePublisher plugin



Set the friction of the caster wheel to near zero by modifying the caster link

Change <collision name="front_caster_collision"> ... </collision> of <link name="front_caster">

to 

 <collision name="front_caster_collision">
    <geometry>
      <sphere>
        <radius>${(wheel_radius+wheel_zoff-(base_height/2))}</radius>
      </sphere>
    </geometry>
    <surface><friction><ode>
      <mu>0.001</mu>
      <mu2>0.001</mu2>
    </ode></friction></surface>
  </collision>

From 


 <collision name="front_caster_collision">
    <geometry>
      <sphere>
        <radius>${(wheel_radius+wheel_zoff-(base_height/2))}</radius>
      </sphere>
    </geometry>
  </collision>







ROS<->Gazebo Bridge: translate Gazebo topics into ROS topics and vice-versa

There is a bridge included in the ros_gz_bridge package 
which allows us to translate Gazebo topics into ROS topics and vice-versa. 
We need to launch the bridge with a configuration which tells it about 
what topics we want to bridge

ROS to Gazebo (vice-versa) Bridge configuration file





Add bridge_config_path variable to launch file





Make a Gazebo world
Add thomas_world.sdf into a created folder world





Add [config world] into install (DIRECTORY ...) 
section of CMakeLists.txt 





Add world_path variable into launch file, 
display.launch.py 






Launch and Build Files

We will now edit our launch file, display.launch.py, to spawn thomas_bot in Gazebo. 
Since the JointStatePublisher plugin will now publish the joint_states, 
we can remove everything related to the joint state publisher 
by deleting / commenting the following lines inside the generate_launch_description():



Remove the relative joint_state_publisher 
from return LaunchDescription([]):






Remove or do comment exec_depend  tags of  joint_state_publisher 
and joint_state_publisher_gui on package.xml







Add use_sim_time to robot_state_publisher_node 
inside def generate_launch_description




Declare the use_sim_time argument in LaunchDescription([])





To launch Gazebo and spawn sam_bot in Gazebo, 
need to add GzServer, RosGzBridge nodes and  spawn_entity



Add ExecuteProcess(), gz_server, ros_gz_bridge and spawn_entity 
to LaunchDescription() into display.launch.py 



Add ros_gz_bridge and ros_gz_sim exec_depend tags into package.xml 



Build, Run and Verification

Run our package to check if the /demo/imu and /demo/odom topics are active in the system.

Navigate to the root of the project and execute the following lines:

colcon build
. install/setup.bash
ros2 launch thomas_bot_description display.launch.py



Error - GzServer is not defined




Import RosGzBridge and GzServer into display.launch.py 





Error - IncludeLaunchDescription is not defined





Error -PythonLaunchDescriptionSource is not defined




Import IncludeLaunchDescription and PythonLaunchDescriptionSource





Error - gz_spawn_model_launch_source is not defined





Add or declare gz_spawn_model_launch_source 
gz_spawn_model_launch_source = os.path.join(ros_gz_sim_share, "launch", "gz_spawn_model.launch.py")




Error - ros_gz_sim_share is not defined






Add or declare ros_gz_sim_share

ros_gz_sim_share = get_package_share_directory('ros_gz_sim')






Error - get_package_share_directory is not defined






Import get_package_share_diroectory

from ament_index_python.packages import get_package_share_directory





Error - ExecuteProcess is not defined






Import ExecuteProcess





Error - thomas_world.sdf is empty





The content of thomas_world.sdf




<sdf version='1.7'>
  <world name='thomas_world'>
    <physics name="1ms" type="ignored">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>
    <plugin
      filename="gz-sim-physics-system"
      name="gz::sim::systems::Physics">
    </plugin>
    <plugin
      filename="gz-sim-user-commands-system"
      name="gz::sim::systems::UserCommands">
    </plugin>
    <plugin
      filename="gz-sim-scene-broadcaster-system"
      name="gz::sim::systems::SceneBroadcaster">
    </plugin>
    <plugin filename="gz-sim-imu-system"
      name="gz::sim::systems::Imu">
    </plugin>
    <plugin
      filename="gz-sim-sensors-system"
      name="gz::sim::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>
    <light name='sun' type='directional'>
      <cast_shadows>1</cast_shadows>
      <pose>0 0 10 0 -0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
      <spot>
        <inner_angle>0</inner_angle>
        <outer_angle>0</outer_angle>
        <falloff>0</falloff>
      </spot>
    </light>
    <model name='ground_plane'>
      <static>1</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>50</mu2>
              </ode>
              <torsional>
                <ode/>
              </torsional>
            </friction>
            <contact>
              <ode/>
            </contact>
            <bounce/>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <cast_shadows>0</cast_shadows>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
        <self_collide>0</self_collide>
        <enable_wind>0</enable_wind>
        <kinematic>0</kinematic>
      </link>
    </model>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <physics type='ode'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>1</shadows>
    </scene>
    <wind/>
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>0</latitude_deg>
      <longitude_deg>0</longitude_deg>
      <elevation>0</elevation>
      <heading_deg>0</heading_deg>
    </spherical_coordinates>
    <model name='unit_box'>
      <pose>1.51271 -0.181418 0.5 0 -0 0</pose>
      <link name='link'>
        <inertial>
          <mass>1</mass>
          <inertia>
            <ixx>0.166667</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.166667</iyy>
            <iyz>0</iyz>
            <izz>0.166667</izz>
          </inertia>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <max_contacts>10</max_contacts>
          <surface>
            <contact>
              <ode/>
            </contact>
            <bounce/>
            <friction>
              <torsional>
                <ode/>
              </torsional>
              <ode/>
            </friction>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
        <self_collide>0</self_collide>
        <enable_wind>0</enable_wind>
        <kinematic>0</kinematic>
      </link>
    </model>
    <model name='unit_sphere'>
      <pose>-1.89496 2.36764 0.5 0 -0 0</pose>
      <link name='link'>
        <inertial>
          <mass>1</mass>
          <inertia>
            <ixx>0.1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.1</iyy>
            <iyz>0</iyz>
            <izz>0.1</izz>
          </inertia>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius>0.5</radius>
            </sphere>
          </geometry>
          <max_contacts>10</max_contacts>
          <surface>
            <contact>
              <ode/>
            </contact>
            <bounce/>
            <friction>
              <torsional>
                <ode/>
              </torsional>
              <ode/>
            </friction>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <sphere>
              <radius>0.5</radius>
            </sphere>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
        <self_collide>0</self_collide>
        <enable_wind>0</enable_wind>
        <kinematic>0</kinematic>
      </link>
    </model>
    <state world_name='default'>
      <sim_time>0 0</sim_time>
      <real_time>0 0</real_time>
      <wall_time>1626668720 808592627</wall_time>
      <iterations>0</iterations>
      <model name='ground_plane'>
        <pose>0 0 0 0 -0 0</pose>
        <scale>1 1 1</scale>
        <link name='link'>
          <pose>0 0 0 0 -0 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
      <model name='unit_box'>
        <pose>1.51272 -0.181418 0.499995 0 1e-05 0</pose>
        <scale>1 1 1</scale>
        <link name='link'>
          <pose>1.51272 -0.181418 0.499995 0 1e-05 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0.010615 -0.006191 -9.78231 0.012424 0.021225 1.8e-05</acceleration>
          <wrench>0.010615 -0.006191 -9.78231 0 -0 0</wrench>
        </link>
      </model>
      <model name='unit_sphere'>
        <pose>-0.725833 1.36206 0.5 0 -0 0</pose>
        <scale>1 1 1</scale>
        <link name='link'>
          <pose>-0.944955 1.09802 0.5 0 -0 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
      <light name='sun'>
        <pose>0 0 10 0 -0 0</pose>
      </light>
    </state>
    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose>3.17226 -5.10401 6.58845 0 0.739643 2.19219</pose>
        <view_controller>orbit</view_controller>
        <projection_type>perspective</projection_type>
      </camera>
    </gui>
  </world>
</sdf>



Loading thomas_world.sdf file is OK






Gazebo launch OK and we got a 3D model of thomas_bot


Clip about building and running 

Build by command: colcon build
. install/setup.bash


Launch robot by: ros2 launch sam_bot_description display.launch.py








Moving our robot by command 

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true --remap cmd_vel:=/demo/cmd_vel




Generative AI, Robot Operating System (ROS 2), Computer Vision, Natural Language Processing service, Generative AI Chatbot, Machine Learning, Mobile App, Web App? Yes, I do provide!

Call me: (+84) 0854147015
WhatsApp: +601151992689
Viber: +84854147015
https://amatasiam.web.app
Email: ThomasTrungVo@Gmail.Com

Facebook: https://www.facebook.com/voduytrung

X: https://x.com/ThomasTrung














at




















No comments:
















Post a Comment