AUTO-763 fix smoother accel decel

[doisyg]

Mirror of ros-navigation#3529

To test:

ros2 launch arri_bringup twist_pipeline.launch.py
edited with (low max linear accel):

    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.1, 0.0, 0.2]},
            {'max_decel': [-0.5, 0.0, -1.2]},
            ],
        remappings=[
            ('cmd_vel', 'cmd_vel_protected'),
            ('cmd_vel_smoothed', 'cmd_vel')],
        output='screen')

Generate twist on /cmd_vel_protected with:

ros2 run rqt_robot_steering rqt_robot_steering

Observe output with plotjuggler (test config: smoother_test.zip )

[scheunemann]

Suggested change

	double v_component_max;
	double v_component_min;
	// Accelerating if magnitude of v_cmd is above magnitude of v_curr
	// and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0)
	// Deccelerating otherwise
	if (v_curr * v_cmd >= 0.0) {
	if (abs(v_cmd) >= abs(v_curr)) {
	v_component_max = accel / smoothing_frequency_;
	v_component_min = -accel / smoothing_frequency_;
	} else {
	v_component_max = -decel / smoothing_frequency_;
	v_component_min = decel / smoothing_frequency_;
	}
	} else {
	v_component_max = -decel / smoothing_frequency_;
	v_component_min = decel / smoothing_frequency_;
	}
	double v_component_max = -decel / smoothing_frequency_;
	double v_component_min = decel / smoothing_frequency_;
	// Accelerating if magnitude of v_cmd is above magnitude of v_curr
	// and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0)
	// Deccelerating otherwise
	if (v_curr * v_cmd >= 0.0 && abs(v_cmd) >= abs(v_curr)) {
	v_component_max = accel / smoothing_frequency_;
	v_component_min = -accel / smoothing_frequency_;
	}

Above reads a little better for me as it shows there is one well defined case to ever accelerate at a glance.

[doisyg]

Reads better but less efficient, v_component_max / v_component_min are computed twice ~50% of the time