0
$\begingroup$

I'm encountering a peculiar issue with robot movement in RViz2 using Nav2, where the robot exhibits erratic back-and-forth movements accompanied by small jumps during the simulation. This behavior is particularly noticeable when the localization nodes/configurations are running and deviate significantly from the expected smooth navigation.

Environment:

ROS Version: ROS 2 Iron Simulation Environment: Gazebo Robot Model: Custom model equipped with GPS and IMU sensors Localization Setup: Nav2 with EKF for integrating odometry and GPS data

Issue Description:

In the simulation, the robot's representation in RViz2 shows unexpected erratic movements, including back-and-forth motions and occasional small jumps. This issue arises with the activation of the localization nodes and seems to be tied to how sensor data is being processed or the specific setup of the localization parameters.

Steps to Reproduce:

Initiate the Gazebo simulation with the custom robot model. Activate the Nav2 localization nodes, employing the EKF setup for odometry and GPS data integration according to the tutorial: Nav2 GPS Tutorial

Troubleshooting Attempts:

  • Modulated the process noise covariance values within the EKF settings (increase values to reduce noise sensibility, and have even commented out all the covariance values).
  • Tested variations in the two_d_mode parameter (set to false).
  • Reassessed the IMU data fusion configurations (set roll,pitch,yaw to True).
  • Confirmed the effective integration of GPS data.
  • Checked the synchronization and timing of the sensor data streams.
  • Verified the accuracy of magnetic declination and yaw offset parameters.
  • Increased the frequency of localization nodes from 30 Hz to 250 Hz.

Despite these adjustments (isolated tests one by one modification), the erratic movement behavior persists, affecting the robot's localization and navigation performance.My robot model is correct since it opens without shake/jump/back-and-forth move before running the dual_ekf_navsat.launch.py launch file.

Questions:

  • What might be the underlying cause of the erratic back-and-forth movements and small jumps observed in the robot's visualization in RViz2?
  • Are there particular Nav2 or EKF configuration parameters that could influence the robot's movement stability in such a manner?
  • Has anyone faced similar challenges with robot movement in RViz2 and discovered a resolution?

Additional Resources:

Configuration Files:

Demonstration Video: Video of back-and-forth movement and small jumps here URDF Ackermann Plugin Code Snippet:

  <gazebo>
      <plugin name='ackermann_drive' filename='libgazebo_ros_ackermann_drive.so'>
        <ros>
        <!-- <namespace>/demo</namespace> -->
          <parameter name="cmd_vel_topic" value="cmd_vel"/>
          <parameter name="odom_topic" value="odom"/>
          <parameter name="distance_topic" value="distance"/>
        </ros>
          <update_rate>30.0</update_rate>
          <!-- wheels -->
          <front_left_joint>first_wheel_left_joint</front_left_joint>
          <front_right_joint>first_wheel_right_joint</front_right_joint>
          <rear_left_joint>fourth_wheel_left_joint</rear_left_joint>
          <rear_right_joint>fourth_wheel_right_joint</rear_right_joint>
          <left_steering_joint>front_left_steer_joint</left_steering_joint>
          <right_steering_joint>front_right_steer_joint</right_steering_joint>

          <!-- Max absolute steer angle for tyre in radians-->
          <!-- Any cmd_vel angular z greater than this would be capped -->
          <max_steer>0.6458</max_steer>

          <!-- Max absolute steering angle of steering wheel -->
          <max_steering_angle>7.85</max_steering_angle>

          <!-- Max absolute linear speed in m/s -->
          <max_speed>20</max_speed>

          <!-- PID tuning -->
          <left_steering_pid_gain>1700 0 1</left_steering_pid_gain>
          <left_steering_i_range>0 0</left_steering_i_range>
          <right_steering_pid_gain>1700 0 1</right_steering_pid_gain>
          <right_steering_i_range>0 0</right_steering_i_range>
          <linear_velocity_pid_gain>1200 0 1</linear_velocity_pid_gain>
          <linear_velocity_i_range>0 0</linear_velocity_i_range>

          <!-- output -->
          <publish_odom>true</publish_odom>
          <publish_odom_tf>true</publish_odom_tf>
          <publish_wheel_tf>true</publish_wheel_tf>
          <publish_distance>true</publish_distance>

          <odometry_frame>odom</odometry_frame>
          <robot_base_frame>base_footprint</robot_base_frame>


      </plugin>
  </gazebo>

  <!-- Sometimes is needed to comment the joint state publisher below, otherwise there is REDUNDANCY of joints published
  and therefore the truck starts shaking, vibrate, etc-->
  <gazebo>
    <plugin name="truck_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
      <ros>
        <remapping>~/out:=joint_states</remapping>
      </ros>
      <update_rate>30</update_rate>
        <joint_name>front_right_steer_joint</joint_name>
        <joint_name>front_left_steer_joint</joint_name>
        <joint_name>first_wheel_left_joint</joint_name>
        <joint_name>first_wheel_right_joint</joint_name>
        <joint_name>fourth_wheel_left_joint</joint_name>
        <joint_name>fourth_wheel_right_joint</joint_name>
    </plugin>
  </gazebo>

</robot>

Illustrative Figure of Back-and-forth (without robot start moving):

enter image description here

Additionally, I observed that in the TF tree below truck TF, the frontal and fourth shafts' wheels are siblings of base_link, but in the URDF, they are children of base_link. This discrepancy did not affect the robot's movement in ROS 2 Galactic, but it might be contributing to the issue in ROS 2 Iron. Here's the output of check_urdf for reference:

robot name is: arocs_truck
---------- Successfully Parsed XML ---------------
root Link: base_footprint has 1 child(ren)
    child(1):  base_link
        child(1):  cabin_link
            child(1):  bar_link
                child(1):  left_lidar_link
                child(2):  right_lidar_link
            child(2):  camera_link
                child(1):  camera_change_orientation_view
            child(3):  gps_link
            child(4):  kinect_link
                child(1):  kinect_depth_frame
            child(5):  real_kinect_link
                child(1):  real_kinect_depth_frame
        child(2):  first_shaft_horizontal_shaft_link
            child(1):  front_left_steering_link
                child(1):  first_wheel_left_link
            child(2):  front_right_steering_link
                child(1):  first_wheel_right_link
        child(3):  fourth_shaft_horizontal_shaft_link
            child(1):  fourth_wheel_left_link
            child(2):  fourth_wheel_right_link
        child(4):  front_left_far_sonar_link
        child(5):  imu_link
        child(6):  radar_link
        child(7):  second_shaft_horizontal_shaft_link
            child(1):  second_wheel_left_link
            child(2):  second_wheel_right_link
        child(8):  sick_link
        child(9):  third_shaft_horizontal_shaft_link
            child(1):  third_wheel_left_link
            child(2):  third_wheel_right_link

I am not running amcl nodes, therefore there are no concurrent publishers of Odom to map transform, etc...

I would greatly appreciate any insights, advice, or potential solutions to rectify this issue. Thank you for your assistance!

$\endgroup$
2
  • $\begingroup$ I'd recommend checking that the covariances of your GPS receiver are reasonable going into RL. That plays a role in sensor data confidence, vs the process confidence in RL's matrix. $\endgroup$ Commented Feb 1 at 0:57
  • $\begingroup$ Hi Steve. Thanks for the reply. I tried to modify the values of the covariance matrix (reduce, increase, and even comment out this variable). The result is the same, unfortunately. In addition in ROS2 Galactic I had configured a very close configuration file (ekf nodes + gps navsat params) and had no issues like this one. However, I preferred to upgrade to IRON since the GPS+ NAV2 usage is more robust...at this moment I am running just on simulation so my GPS sensor plugin is the same from the tutorial $\endgroup$ Commented Feb 1 at 1:01

1 Answer 1

0
$\begingroup$

I have fixed this issue using my own custom configuration file based on navsat transform node documentation:

# For parameter descriptions, please refer to the template parameter files for each node.

ekf_filter_node_odom:
  ros__parameters:
    frequency: 30.0
    two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments
    publish_acceleration: false 
    #print_diagnostics: true
    debug: false
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin
    world_frame: map #odom

    odom0: /odom
    odom0_config: [false, false, false,
                  false, false, false,
                  true,  true,  true,
                  false, false, true,
                  false, false, false]
    odom0_differential: false
    odom0_nodelay: true
    odom0_relative: false
    odom0_queue_size: 10

    imu0: /imu
    imu0_config: [false, false, false,
                  true,  true,  true,
                  false, false, false,
                  true, true, true,
                  false,  false,  false]
    imu0_differential: false  # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate
    imu0_nodelay: true 
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true

    use_control: false

    process_noise_covariance: [1e-3, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    1e-3,  0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    1e-3,  0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.1,   0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.1,   0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.01,  0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1,    0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.1,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.1,   0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.1,   0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.1,   0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.1,   0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.1,   0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1]

ekf_filter_node_map:
  ros__parameters:
    use_sim_time: true
    sensor_timeout: 0.1
    transform_time_offset: 0.0
    transform_timeout: 0.0
    frequency: 30.0
    two_d_mode: false #true  # Recommended to use 2d mode for nav2 in mostly planar environments
    #print_diagnostics: true
    publish_acceleration: false 
    debug: false
    publish_tf: true
    reset_on_time_jump: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin
    world_frame: map

    odom0: /odometry/local # = "fused odometry + Imu in previous node"
    odom0_config: [false, false, false,
                  true, true, true,
                  true,  true,  true,
                  true, true, true,
                  false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true 
    odom0_differential: false #imu # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate
    odom0_relative: false


    odom1: gps #odometry/gps # try gps if not fix
    odom1_config: [false, false,  false,
                  false, false, false,
                  true, true, true,
                  false, false, true,
                  false, false, false]
    odom1_queue_size: 10
    odom1_nodelay: true
    odom1_differential: false
    odom1_relative: false


    use_control: false

    process_noise_covariance: [1.0,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    1.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    1e-3,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.1,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.1,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.01,   0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.1,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.1,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.1,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.1,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.1,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.1,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1]

navsat_transform:
  ros__parameters:
    use_sim_time: true
    frequency: 30.0
    delay: 3.0
    magnetic_declination_radians: 0.0 #0.266512
    yaw_offset: 0.0
    zero_altitude: true
    broadcast_utm_transform: true
    publish_filtered_gps: true
    use_odometry_yaw: false #true
    wait_for_datum: false #true 
    #datum: [38.161491, -122.4546443, 0.0] # pre-set datum if needed, [lat, lon, yaw]

I hope it helps other developers who are fitting the nav2 GPS tutorial for his/her robot (not Turtlebot).

$\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.