Adding new Nav2 loopback simulator

[SteveMacenski]

#3924

• Config guide
• Migration guide

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

see 1 file with indirect coverage changes

[SteveMacenski]

Docs: ros-navigation/docs.nav2.org#580

[aosmw]

This will be useful. Many thanks. I approve for what its worth.

[aosmw]

Minor nit.
Does this comment match what is happening?

Do you mean for sendTransform() be called once only or repeatedly in the timer?
It would make sense if you also called sendTransform in the init().

You could just remove the comment from this setupTimerCallback()

[SteveMacenski]

I mean for it to be repeated on a timer that runs before the initial pose is set. The 'real' simulators or hardware robots will have their odometry systems continuously publishing and that odom->base_link is needed by many node's lifecycle startup processes to make Nav2 fully initialize. We could publish it once, but that would be fragile in case there's a delay or a long plugin initialization cycle for that message to be out of the TF buffer duration. Plus, this is how "real" robots and simulators operate so its analogoous

The comment is mostly there for tutorial value for later on-lookers. I agree most of my comments are not necessary

[aosmw]

A comparison between the time in the msg.header and curr_cmd_vel_time could be used to identify, warn about and/or reject old cmd_vel messages.

[SteveMacenski]

There is no message header in Twist

[aosmw]

poor z. Maybe explicitly zero it.

[SteveMacenski]

Adding :-)

[aosmw]

translation.z is used here.

[SteveMacenski]

Making analog to comment above

[aosmw]

Why are transforms published with a header.stamp in the "future" but Odometry (in publishOdometry()) is not?

[SteveMacenski]

This is how all the global localizers work. We need the transformation from map->odom to slightly "lead" the system so that anyone that wants to transform map->XYZ frame gets that data available between global localization updates. Since that is a static transformation between updates, the time isn't really all that important (unlike the odom->base_link). If you look at Cartographer, AMCL, SLAM Toolbox, etc they all do this from the very beginning of ROS 1 as a known design pattern to prevent floods of those "extrapolation into the future" errors

[aosmw]

Thanks for the explanation.

We have just killed off an controller_server control loop issue that is probably related to this.
i.e "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz."

navigation2/nav2_controller/src/controller_server.cpp

Line 498 in 3f31112

RCLCPP_WARN(

Despite configuring for rtprio and we were still getting this warning

I think MPPI is effectively hanging around waiting for odom updates. We were running MPPI at 50Hz and the two ros_localisation ekfs at 30Hz and nav_sat_transform at 30Hz. So while it works I have a feeling there is effectively a live lock underneath it all.

Do you, in your robots change the robot_localization transform_time_offset from the default of 0.0? https://github.com/cra-ros-pkg/robot_localization/blob/f6b28e0c5bcf98d161cb65b9d28d00ced77fe3ab/params/ekf.yaml#L20

Bumping the ekfs to 100Hz and leaving nav_sat_transform at 30Hz makes nav2_controller/MPPI and me very happy.

[SteveMacenski]

I change transform_time_offset for the global EKF for GPS fusion only, but yeah I do use that parameter for this same reason!

[aosmw]

Feel free to ignore below. I know its 2D and simple.

You could make a quaternion from the yaw and then multiply it to the existing quaternion.
In c++ I would also give it a normalize()

https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#applying-a-quaternion-rotation

It seems that tf_transformations does not expose a quaternion normalisation.
https://github.com/DLu/tf_transformations/blob/07d77223288c457cfe71f361257e9fdd350c6fd9/tf_transformations/__init__.py#L809

Underneath it uses transforms3d.quaternions.qmult - http://matthew-brett.github.io/transforms3d/reference/transforms3d.quaternions.html#transforms3d.quaternions.qmult

That does provide a transforms3d.quaternions.qnorm - http://matthew-brett.github.io/transforms3d/reference/transforms3d.quaternions.html#qnorm

[SteveMacenski]

Good to know! I didn't previously see the quaternion_multiply method in that library to use https://github.com/DLu/tf_transformations/blob/main/tf_transformations/__init__.py#L809. The lack of transformation libraries built into TF for Python is quite irritating. Thanks @DLu for this work!

Will make that update