Collision monitor / velocity smoother: handle nan inputs #3627
Description
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.