ROS2 Nav2 Challenges with Ackerman Robot: Localization, Path Planning, and Costmap Out of Boundary

edit

I am using ROS2 Galactic on Ubuntu 20.04 and trying to use Nav2 for an Ackerman robot. Even though I've made significant progress, I'm encountering issues with localization and planning in my simulation. Specifically, the "2D Set Goal Pose" plugin on RViz. The "2D Set Estimate Pose" works properly:

[amcl-7] [INFO] [1684849938.699269430] [amcl]: Setting pose (28.141000): -1.011 -0.160 0.009

The detailed problems are described below and any help would be greatly appreciated.

• Issue 1: TF Transform and Costmap Warning

My robot's lidar is placed high on the z-axis, which I believe causes a "controller-server: costmap out of boundary" warning. However, a static transform from lidar_link to base_link suppresses this, leading to a new warning about the control loop missing its rate of 20Hz (set based on Turtlebot's configuration):. I wonder if I should change the control frequency or use Simulation Time?

[controller_server-9] [WARN] [1684849244.423230410] [local_costmap.local_costmap]: Sensor origin at (4.26, -0.04 2.92) is out of map bounds (34.24, 29.93, 3.70) to (-14.90, -14.85, 0.00). The costmap cannot raytrace for it.

when I run a static transform from lidar_link to base_link, the warning for the local costmap being out of bounds is suppressed, and then I get the WARN:

[controller_server-9] [WARN] [1684849560.289577847] [controller_server]: The control loop missed its desired rate of 20.0000Hz

• Issue 2: Fused Odometry and IMU Data Frequency

I noticed an odd behavior while the odometry and IMU data appear to be properly fused (with /odometry_filtered and /accel_filtered publishing data). When echoing the /diagnostics topic, the publishing frequency of fused topics is about 10 Hz for a Turtlebot simulation, but essentially zero for my simulation (key: actual frequency):

header:
  stamp:
    sec: 1684849143
    nanosec: 436847789
  frame_id: ''
status:
- level: "\x02"
  name: 'ekf_filter_node: odometry/filtered topic status'
  message: No events recorded.
  hardware_id: none
  values:
  - key: Events in window
    value: '0'
  - key: Events since startup
    value: '0'
  - key: Duration of window (s)
    value: '9.999987'
  - key: Actual frequency (Hz)
    value: '0.000000'
  - key: Minimum acceptable frequency (Hz)
    value: '25.200000'
  - key: Maximum acceptable frequency (Hz)
    value: '35.200000'

I believe this is affecting AMCL's localization, as the particle cloud doesn't update with robot movement. Interestingly, the hz command shows that /odometry/filtered and /accel/filtered are published around 10 Hz:

ros2 topic hz /odometry/filtered 
average rate: 9.959
    min: 0.098s max: 0.103s std dev: 0.00118s window: 11

    ros2 topic hz /accel/filtered
average rate: 9.944
        min: 0.100s max: 0.103s std dev: 0.00080s window: 11

The frequency of publishment of my diagnosis topic:

ros2 topic hz /diagnostics 
average rate: 1.000
    min: 1.000s max: 1.000s std dev: 0.00009s window: 3

I have used the same frequency as the turtlebot tutorial to fuse odom and imu (30 Hz).

• Issue 3: Navigation Activation and Planner Discrepancy

Navigation doesn't enable when using RViz plugin, but works when setting a goal pose ... (more)