Is it possible to reduce the turning speed when the swd core state is SLS?

[fig-shimooka]

Is it possible to reduce the turning speed when the swd core state is SLS? For example, if cmd_vel->linear.x = 0.0, cmd_vel->angular.z = 0.8, when the SLS state is transitioned, the angular.z is reduced to 0.3 rad/s, etc. Now, during SLS, if linear.x is 0.0, it does not decelerate. Is this the kind of specification?

[ez-Support]

No indeed it is not a specification, but a problem in our controller. We are seeing what we can to handle it.

[GMezWheel]

You will find hereafter a new setSpeeds function that limits the speed difference between the two wheels. The maximum delta speed limit take into account of the SLS signal. Thus, this will reduce the turning speed when the swd core state is SLS.

=> DiffDriveController.cpp : line 559

#define CONF_MAX_DELTA_SPEED_SLS    140 // in rpm motor
#define CONF_MAX_DELTA_SPEED  1000 // in rpm motor

        ///
        /// \brief Change robot velocity (left in rpm, right in rpm)
        ///
        void DiffDriveController::setSpeeds(int32_t left_speed, int32_t right_speed)
        {
            // Get the outer wheel speed
            int32_t faster_wheel_speed = M_MAX(std::abs(left_speed), std::abs(right_speed));
            int32_t speed_limit        = -1;

            // Limit to the maximum allowed speed
            if (faster_wheel_speed > m_max_motor_speed_rpm) {
                speed_limit = m_max_motor_speed_rpm;
            }

            m_safety_msg_mtx.lock();
            bool sls_signal = m_safety_msg.safety_limited_speed;
            m_safety_msg_mtx.unlock();

            // If SLS detected, impose the safety limited speed (SLS)
            if (sls_signal && (faster_wheel_speed > m_motor_sls_rpm)) {
                speed_limit = m_motor_sls_rpm;
            }

            // The left and right wheels may have different speeds.
            // If we need to limit one of them, we need to scale the second wheel speed.
            // This ensures a speed limitation without distorting the target path.
            if (-1 != speed_limit) {
                // If we enter here, we are sure that (faster_wheel_speed > speed_limit).
                // Get the ratio between the outer (faster) wheel, and the speed limit.
                double speed_ratio = static_cast<double>(speed_limit) / static_cast<double>(faster_wheel_speed);

                // Scale right_speed
                right_speed = static_cast<int32_t>(static_cast<double>(right_speed) * speed_ratio);

                // Scale left_speed
                left_speed = static_cast<int32_t>(static_cast<double>(left_speed) * speed_ratio);

                ROS_WARN("The target speed exceeds the maximum speed limit (%d rpm). "
                         "Speed set to (left, right) (%d, %d) rpm",
                         speed_limit, left_speed, right_speed);
            }

            // Get the delta wheel speed
            int32_t delta_wheel_speed = std::abs(left_speed - right_speed);
            int32_t delta_speed_limit = -1;

            // Limit to the maximum allowed delta speed
            if (delta_wheel_speed > CONF_MAX_DELTA_SPEED) {
                delta_speed_limit = CONF_MAX_DELTA_SPEED;
            }

            // If SLS detected, limit to the maximum allowed delta safety limited speed (SLS)
            if (sls_signal && (delta_wheel_speed > CONF_MAX_DELTA_SPEED_SLS)) {
                delta_speed_limit = CONF_MAX_DELTA_SPEED_SLS;
            }

            // The left and right wheels may have different speeds.
            // If we need to limit one of them, we need to scale the second wheel speed.
            // This ensures a delta speed limitation without distorting the target path.
            if (-1 != delta_speed_limit) {
                // Get the ratio between the max allowed delta speed limit, and the current delta speed limit.
                double delta_speed_ratio = static_cast<double>(delta_speed_limit) / static_cast<double>(delta_wheel_speed);

                // Scale right_speed
                right_speed = static_cast<int32_t>(static_cast<double>(right_speed) * delta_speed_ratio);

                // Scale left_speed
                left_speed = static_cast<int32_t>(static_cast<double>(left_speed) * delta_speed_ratio);

                ROS_WARN("The target speed exceeds the maximum delta speed limit (%d rpm). "
                         "Speed set to (left, right) (%d, %d) rpm",
                         delta_speed_limit, left_speed, right_speed);
            }

            // Send the actual speed (in RPM) to left motor
            ezw_error_t err = m_left_controller.setTargetVelocity(left_speed);
            if (ERROR_NONE != err) {
                ROS_ERROR("Failed setting velocity of right motor, EZW_ERR: SMCService : "
                          "Controller::setTargetVelocity() return error code : %d",
                          (int)err);
                return;
            }

            // Send the actual speed (in RPM) to left motor
            err = m_right_controller.setTargetVelocity(right_speed);
            if (ERROR_NONE != err) {
                ROS_ERROR("Failed setting velocity of right motor, EZW_ERR: SMCService : "
                          "Controller::setTargetVelocity() return error code : %d",
                          (int)err);
                return;
            }

#if VERBOSE_OUTPUT
            ROS_INFO("Speed sent to motors (left, right) = (%d, %d) rpm", left_speed, right_speed);
#endif
        }

[fig-shimooka]

Thank you. We have confirmed that this modification is reflected in the slowdown.