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 lidarlink to baselink 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 lidarlink to baselink, 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 /odometryfiltered and /accelfiltered 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 manually via the command line.

ros2 topic pub /goal_pose geometry_msgs/msg/PoseStamped '{
  header: {
    frame_id: "map"
  },
  pose: {
    position: {
      x: -60.0,
      y: -25.0,
      z: 0.0
    },
    orientation: {
      x: 0.0,
      y: 0.0,
      z: 0.0,
      w: 1.0
    }
  }
}'

The path planning visualization differs significantly from a Turtlebot simulation - it is less curved and straighter. Moreover, the robot tends to take shortcuts instead of following the calculated path:

• Issue 4: Robot Acceleration

The robot doesn't seem to accelerate linearly and only turns the wheels without moving forward unless an initial linear velocity is manually provided (by teleop) to overcome inertia. After reaching the goal pose, it continues moving until it collides because of teleop node keeps sending velocity. The path planner seems to send a constant linear velocity of approximately 0.26 m/s which is too low for my robot to overcome inertia, for this reason, I needed to "disengage" it with teleop.

ros2 topic echo /cmd_vel
    linear:
      x: 0.26000000000000006
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.7894736842105263

Here are the issues I need to address:

Localization window should be dynamic Path planning needs to be smoother, and output higher Linear X velocity without compromising control. It should consider the best costmap options as in the Turtlebot tutorial and work with the Rviz "2D set goal pose." Possibly find a fix for my TF Warn (Local Costmap boundary error in Z axis due to my truck's high height)

Here are the related files and resources for reference:

• nav2_params.yaml
• ekf.yaml
• Behavior Video: Path Planning & Control

---

• 5 Update: Progress on Localization

By enabling the slam parameter in my launch file (slam:=True), I've managed to achieve a dynamic localization window. However, its performance is not as expected. The localization begins with an inversion issue (as detailed in this post), and accuracy issues persist in both localization and planning:

Navigation video

Ideally, the system should function correctly even without the slam parameter, much like the Turtlebot tutorial. This was my initial expectation, as I had meticulously adjusted each simulation file to fit my robot model, avoiding any drastic structural modifications. The root of the issue remains unresolved.