search

ezWheelSAS / swd_ros_controllers

ROS nodes to control motors powered by the ez-Wheel Safety Wheel Drive (SWD®) technology.
https://www.ez-wheel.com/
GNU Lesser General Public License v2.1
5 stars 4 forks source link

During SLS, it meanders from left to right. #17

Closed fig-shimooka closed 2 years ago

fig-shimooka commented 2 years ago

During SLS, it meanders from left to right. The sls_vl_limit is set to 535. The cmd_vel at this time is 0.6 for linear.x and 0 for angular.z Drop linear.x to about 0.3 and it will stabilise.

.

fig-shimooka commented 2 years ago 

DBUS Instance        : swd_right .................................................................... [OK]
Node ID              : 0x5 .......................................................................... [OK]
Vendor-Id            : 0x515 ........................................................................ [OK]
Product code         : 0x10001 ...................................................................... [OK]
Serial number        : 724434946 .................................................................... [OK]
Revision number      : 4 ............................................................................ [OK]
SWVersion            : 1.0.1 ........................................................................ [OK]
HWVersion            : 3.0.0 ........................................................................ [OK]
Calibrated           : True ......................................................................... [OK]
SRDO validity        : True ......................................................................... [OK]
NMT State            : OPER  ........................................................................ [OK]
PDS State            : OPERATION_ENABLED  ........................................................... [OK]
NbError              : 0 ............................................................................ [OK]
LastError            : 0 : Error_reset_or_no_error .................................................. [OK]
SystemError          : EZW_PROTECT_NONE EZW_PROTECT_NONE  ........................................... [OK]
SSW CAN1             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN2             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN3             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN4             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN5             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN6             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN7             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN8             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW SAFEOUT          : False, False, False, False, False, False, False, False  ...................... [OK]
SCW SAFEIN_1         : True, True, False, False, False, False, True, True  .......................... [OK]
Peripheral           : True, True, True, True, True, True  .......................................... [OK]
VelocityModeSW       : internal_limit_active: False ................................................. [OK]
VelocityModeCW       : enable_ramp: False, unlock_ramp: False, reference_ramp: False, halt: False ... [OK]
Polarity             : velocity_polarity: False, position_polarity: False ........................... [OK]
TargetVelocity       : 0 ............................................................................ [OK]
VelocityDemand       : 0 ............................................................................ [OK]
VelocityActualValue  : 0 ............................................................................ [OK]
PositionValue        : -602 ......................................................................... [OK]
OdometryValue        : -579 ......................................................................... [OK]

DBUS Instance        : swd_left ..................................................................... [OK]
Node ID              : 0x4 .......................................................................... [OK]
Vendor-Id            : 0x515 ........................................................................ [OK]
Product code         : 0x10001 ...................................................................... [OK]
Serial number        : 724434949 .................................................................... [OK]
Revision number      : 4 ............................................................................ [OK]
SWVersion            : 1.0.1 ........................................................................ [OK]
HWVersion            : 3.0.0 ........................................................................ [OK]
Calibrated           : True ......................................................................... [OK]
SRDO validity        : True ......................................................................... [OK]
NMT State            : OPER  ........................................................................ [OK]
PDS State            : OPERATION_ENABLED  ........................................................... [OK]
NbError              : 0 ............................................................................ [OK]
LastError            : 0 : Error_reset_or_no_error .................................................. [OK]
SystemError          : EZW_PROTECT_NONE EZW_PROTECT_NONE  ........................................... [OK]
SSW CAN1             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN2             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN3             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN4             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN5             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN6             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN7             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW CAN8             : False, False, False, False, False, False, False, False  ...................... [OK]
SSW SAFEOUT          : False, False, False, False, False, False, False, False  ...................... [OK]
SCW SAFEIN_1         : True, True, True, True, True, True, True, True  .............................. [OK]
Peripheral           : True, True, True, True, True, True  .......................................... [OK]
VelocityModeSW       : internal_limit_active: False ................................................. [OK]
VelocityModeCW       : enable_ramp: False, unlock_ramp: False, reference_ramp: False, halt: False ... [OK]
Polarity             : velocity_polarity: True, position_polarity: True ............................. [OK]
TargetVelocity       : 0 ............................................................................ [OK]
VelocityDemand       : 0 ............................................................................ [OK]
VelocityActualValue  : 0 ............................................................................ [OK]
PositionValue        : -616 ......................................................................... [OK]
OdometryValue        : -593 ......................................................................... [OK]
ez-Support commented 2 years ago

This phenomenon happens when the command is slower than the error computed, so that you have got this oscillation. You can try to play with the m_pub_freq_hz parameter. You can also try the ROS2 controller that was released https://github.com/ezWheelSAS/swd_ros2_controllers, and see if it helps.

fig-shimooka commented 2 years ago

Changed m_pub_freq_hz (5~50). However, the result did not change. The CPU load is below 60% at this time.

fig-shimooka commented 2 years ago

I think the problem is due to this: when the state updates to SLS, the amr bends. It is as if the left and right motors are not synchronised.

GMezWheel commented 2 years ago

Can you send me a full can log during amr bends (30s for example) ? candump can0 -tz > candump.log

GMezWheel commented 2 years ago

We have reproduce your problem with your SWD ROS Diff Drive Controller settings. The parameter "wheel_safety_limited_speed_rpm" is incoherent with the SLS speed limit into your commissioning.py file : => respectively 1120 (8014) and 680 rpm motor. We propose to reduce the "wheel_safety_limited_speed_rpm" parameter to 30 rpm wheel (3014=420 rpm motor). <rosparam param="wheel_safety_limited_speed_rpm">30</rosparam>

fig-shimooka commented 2 years ago

I changed wheel_safety_limited_speed_rpm to 30 and restarted. However, the SWD Core still meanders.

This is the log when travelling at 0.5 m/s during SLS conditions. candump.txt

fig-shimooka commented 2 years ago

The sls_vl_limit at this time is 680

fig-shimooka commented 2 years ago

https://github.com/ezWheelSAS/swd_ros_controllers/issues/18#issuecomment-1141378824 I have modified swd_diff_controller to apply this fix and the meandering has improved.

  1. modified the setSpeeds function
  2. changed rosparam's publish_safety_function from false to true
  3. modified Duration in m_timer_safety 
    m_timer_safety = m_nh->createTimer(ros::Duration(1.0 / 1.0), boost::bind(&DiffDriveController::cbTimerSafety, this));
↓
m_timer_safety = m_nh->createTimer(ros::Duration(1.0 / 5.0), boost::bind(&DiffDriveController::cbTimerSafety, this));.
fig-shimooka commented 2 years ago

https://github.com/ezWheelSAS/swd_ros_controllers/issues/12#issuecomment-1110963264

In the course of addressing this issue, changes were made to obtain the safety status from the CAN. At that time, the publish_safety_function was set to false. However, we realised that we were referencing the value of m_safety_msg in the setSpeeds function. I assume this is the cause of the meandering.

GMezWheel commented 2 years ago

Hi @fig-shimooka, To avoid to be link with safety message function into the set_speed function in order to retrieve SLS state, you can replace the following lines :

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

by

       // Reading SLS
        bool res_l, res_r;
        err = m_left_controller.getSafetyFunctionCommand(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_1, res_l);
        if (ERROR_NONE != err) {
            ROS_ERROR(
                         "Error reading SLS from left motor, EZW_ERR: SMCService : "
                         "Controller::getSafetyFunctionCommand() return error code : %d",
                         (int)err);
        }

        err = m_right_controller.getSafetyFunctionCommand(ezw::smccore::ISafeMotionService::SafetyFunctionId::SLS_1, res_r);
        if (ERROR_NONE != err) {
            ROS_ERROR(
                         "Error reading SLS from right motor, EZW_ERR: SMCService : "
                         "Controller::getSafetyFunctionCommand() return error code : %d",
                         (int)err);
        }

        bool sls_signal = !(res_l || res_r);

Thus, you can disable publish_safety_function again.