Collision monitor / velocity smoother: handle nan inputs

[doisyg]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Iron deb
• Version or commit hash:
  • lastest Iron source
• DDS implementation:
  • Cyclone

Steps to reproduce issue

Uncovered by @kaichie

With:

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():

    velocity_smoother = Node(
        package="nav2_velocity_smoother",
        name="root_velocity_smoother",
        executable="velocity_smoother",
        parameters=[
            # {'use_sim_time': use_sim_time},
            {"smoothing_frequency": 20.0},
            {"scale_velocities": False},
            {"feedback": "OPEN_LOOP"},
            {"max_velocity": [0.5, 0.0, 0.8]},
            {"min_velocity": [-0.5, 0.0, -0.8]},
            {"velocity_timeout": 0.5},
            {"max_accel": [0.25, 0.0, 1.2]},
            {"max_decel": [-0.5, 0.0, -1.2]},
        ],
        output="screen",
    )

    lifecycle_manager_velocity_smoother = Node(
        package="nav2_lifecycle_manager",
        executable="lifecycle_manager",
        name="lifecycle_manager_velocity_smoother",
        parameters=[
            {"autostart": True},
            {"node_names": ["root_velocity_smoother"]},
        ],
        output="screen",
    )

    ld = LaunchDescription()
    ld.add_action(velocity_smoother)
    ld.add_action(lifecycle_manager_velocity_smoother)
    return ld

Listen to the output:
ros2 topic echo /cmd_vel_smoothed

Generate an input with .nan value

ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: .nan" 

Stop and generate a normal input:

ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.5" 

The output is frozen on .nan.

Expected behavior

The output is set according to the input.
We also probably want to consider .nan as 0.0

Actual behavior

The output is frozen on .nan.

[SteveMacenski]

Whoops

Is this new to Iron (e.g. you didn't experience beforehand)?

There are only 3 commits it could be if so:
c841417

The first looks rather unlikely, since that just passes doing anything if command_ is not set.

59f1d9e

The second is only when velocity commands stop coming in as expected to wind down the velocities by the dynamic limits instead of sending hard 0s. So unless you only see this when there's another error condition happening or when starting/stopping the robot, I doubt that's it.

Are there any situational details when this happens I should know about, or just randomly in the middle of tracking a path?

9307dbc

The last I don't see any new divide by zero opportunities.

---

I'm also going to assume this doesn't relate to dynamic parameters or you would have mentioned that (right?)

[SteveMacenski]

^^ ignore all of that, sorry, I'm sick today and misread the topic in my spinng head state. You mean that if you input a NaN into the system you want that to be handled.

A-OK that's easy enough, I can do that. I think what makes sense is to look at the input callback and validate the message. If it contains a nan or inf, then we reject that velocity command and never integrate it into the system. That's not a valid command anyway to have processed. We could treat it as a zero, but I'd think its better to ignore it as an invalid dangerous command (with a log statement to that effect) since we can't know the intent of how it was generated. Especially if we have an inf instead of a nan

If that's good with you, I can implement and add some unit tests ~Friday or early next week. Today's a sick day :(

[doisyg]

^^ ignore all of that, sorry, I'm sick today and misread the topic in my spinng head state. You mean that if you input a NaN into the system you want that to be handled.

Yes, or at least that it doesn't put the node in a unrecoverable state

If that's good with you, I can implement and add some unit tests ~Friday or early next week. Today's a sick day :(

Yes! No specific rush, just wanted to document first the bug (the fact that the output stays blocked to .nan) before thinking about an action. It is probably there since a couple of versions.

If it contains a nan or inf, then we reject that velocity command and never integrate it into the system. That's not a valid command anyway to have processed. We could treat it as a zero, but I'd think its better to ignore it as an invalid dangerous command (with a log statement to that effect) since we can't know the intent of how it was generated.

Agree with all of that: rejecting and log.

[SteveMacenski]

• collision monitor!

[SteveMacenski]

PR to appear shortly