[Kinetic diff_drive_controller] Publishes backward driving command to wheels even with zero input command

[wienans]

Hi Guys,

i faced a really weird issue of the diff_drive_controller in the kinetic version. And as i think it is safety related i post this issue even if kinetic runs into EOL this month.
After Startup of the robot the robot was driving backwards and left for maybe a second starting with -1.5 m/s in linear and angular direction. This ramped down over time and every second message is a zero velocity message. Here you can see a rqt Plot of this behavior. This topic is the cmd_vel_out debug topic.

As we don't publish a zero velocity command while standing and it only appeared rarely after startup i first thought it was something going crazy internally because some variable is may be not defined correctly and it graps some random data from the RAM. So i published contentious zeros while standing and the issue occured again. The biggest problem with it, it is not really reproducable, i didn't faced the issue after restarting the robot after it occured once at the start of the day. And it only reappeared at some days and only at the first startup of the day.

[bmagyar]

It'd be nice if you could get to the bottom of this. I'd add logs for the input joint states too, perhaps your encoder values were spinning like crazy and this is to compensate?

…

On Fri, 9 Apr 2021, 07:48 wienans, ***@***.***> wrote: Hi Guys, i faced a really weird issue of the diff_drive_controller in the kinetic version. And as i think it is safety related i post this issue even if kinetic runs into EOL this month. After Startup of the robot the robot was driving backwards and left for maybe a second starting with -1.5 m/s in linear and angular direction. This ramped down over time. Here you can see a rqt Plot of this behavior. This topic is the cmd_vel_out debug topic. [image: grafik] <https://user-images.githubusercontent.com/40465543/114139613-90ede100-990f-11eb-82c2-28f0bada4179.png> As we don't publish a zero velocity command while standing and it only appeared rarely after startup i first thought it was something going crazy internally because some variable is may be not defined correctly and it graps some random data from the RAM. So i published contentious zeros while standing and the issue occured again. The biggest problem with it, it is not really reproducable, i didn't faced the issue after restarting the robot after it occured once at the start of the day. And it only reappeared at some days and only at the first startup of the day. — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#561>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AA24PYKDNGKHITVZVPGDFEDTH2PMBANCNFSM42ULR5FQ> .

[wienans]

Hey,
i didn’t do a deep dive into the code, but I found this lines

ros_controllers/diff_drive_controller/src/diff_drive_controller.cpp

Line 424 in 45486f8

Commands curr_cmd = *(command_.readFromRT());

There the command is read out and later published to the debug topic. So I thought it is only forward Control. Subscribe to input topic save in real-time handler read from real time handler publish debug velocity and then set wheel velocity’s. So I would assume there is no feedback from the wheels or any other system which would course a change of cmd_vel_out

[matthew-reynolds]

Hmm, that sorta looks like the controller is timing out the command.

• Is it possible you're specifying a very short command timeout? ("cmd_vel_timeout" param)
• Are you passing in the correct time into the controller_manager update? You need to use ros::Time::now() rather than any other time source in order to match the controller's internal timeout calculation.

Alternatively, is it possible that you're passing in an inconsistent update period to the controller_manager update?

I think those are the only two factors that could affect the command between the readFromRT() and the publisher, but I haven't dug deep.

[wienans]

Hey @matthew-reynolds

• Is it possible you're specifying a very short command timeout? ("cmd_vel_timeout" param)

I am not exactly sure but i think we use the default one. But the picture i showed above didn't use any input command at all. And after that i tried to do a input command of 10 Hz with zero velocity which didn't fixed the issue.
What i noticed while reviewing the bag file in terminal instead of rqt is that the messages in /cmd_vel_out have jumping sequence numbers and aditionally jump in the timestamp of the /odom topic.

Sequence numbers of the messages published:
No input on /cmd_vel
seq. - /cmd_vel_out velocity - timestamp of /odom with the same sequence number
3210 - velocity: 0 - seconds: 525
3211 - velocity: 0 - seconds: 525
3277 - velocity: -1.5 - seconds: 528 (timestamp of /cmd_vel_out is correct at 525)
3212 - velocity: 0 - seconds: 525 (timestamps are now again in sync)
3278 - velocity: -1.3 - seconds: 525
.... alternating the sequence numbers the higher seq. number publishes the /cmd_vel_out velocity i am talking about in this issue

3276 - velocity: 0 - seconds 528
3332 - velocity: -0.001 - seconds 528
3277 - velocity: 0 - seconds 528 (this timestamp is exactly the timestamp the first message with seq. number 3277 'predicted')
3333 - velocity: 0 - seconds 528
3334 - velocity: 0 - seconds 528

Are you passing in the correct time into the controller_manager update? You need to use ros::Time::now() rather than any other time source in order to match the controller's internal timeout calculation.

As i am not directly understanding what exacly you are meaning with this i assume this is about the controller who is controlling the wheels. I use epos_hardware to control the motors and i would need to check what they are doing on monday. The sequence numbers and the time jump suggestes something timeing related.

[matthew-reynolds]

But the picture i showed above didn't use any input command at all

Ah, I missed that, my bad. Seems unlikely to be related to a timeout then.

Can you clarify a little on the "ramping down" from -1.5m/s? Am I correct in understanding that you:

1. Published -1.5 linear & angular to /cmd_vel for a while, then
2. Either:
   a. Stopped publishing entirely
   b. Switched to publishing zeros

And in both cases (2a and 2b), you saw the pictured behaviour?

Messages in /cmd_vel_out have jumping sequence numbers and aditionally jump in the timestamp of the /odom topic

The sequence number should never jump, that points to either multiple diff_drive_controllers or something broken in the ROS communications stack. Is it possible there are multiple diff_drive_controllers running?

If it's not proprietary, could you share your controller configuration (yaml and launch files)?

I use epos_hardware to control the motors

Gotcha. I took a quick peek (https://github.com/RIVeR-Lab/epos_hardware/blob/indigo-devel/epos_hardware/src/nodes/epos_hardware_node.cpp), looks like they correctly use ros::Time::now(), so that should be fine. Their calculation of the period is slightly unreliable if the robot's clock changes (eg. connects to internet and updates time), but that seems unrelated to this issue.

[wienans]

But the picture i showed above didn't use any input command at all

Ah, I missed that, my bad. Seems unlikely to be related to a timeout then.

Can you clarify a little on the "ramping down" from -1.5m/s? Am I correct in understanding that you:

1. Published -1.5 linear & angular to /cmd_vel for a while, then
2. Either:
   a. Stopped publishing entirely
   b. Switched to publishing zeros

And in both cases (2a and 2b), you saw the pictured behaviour?

No i started up the robot / system and in case

• a i never published any command to /cmd_vel
• b i published from the beginning (even before the diff_drive_controller was started) Twist messages with zero velocity to the /cmd_vel

And in both cases the above issue occured. But it is not realy reproducable as after the issue occured i restarted the robot and it didn't occur.
In general the issue is. The robot didn't get any command to move but it moved. And the diff_drive debug output showed that it moved the robot with the pattern without any command to do so. Actually it is not exactly 1.5 first i thought it was pi/2 but it isn't. And moreover linear and angular velocitys in the graph above are exactly the same for every decimal. Why it is ramping down and why with this speed i don't have any clue. But i don't think the value of 1.5 is fixed (directly related to the issue) so i got at one time only 0.4 m/s or other values as first reported value in the messages if I use case b.

Messages in /cmd_vel_out have jumping sequence numbers and aditionally jump in the timestamp of the /odom topic

The sequence number should never jump, that points to either multiple diff_drive_controllers or something broken in the ROS communications stack. Is it possible there are multiple diff_drive_controllers running?

Actually i thought this too but first this is hard to monitor and second why should the second diff_drive_controller randomly start at sequence number 3277. So we do not activly start a second instance of diff_drive_controller, as my knowledge. Moreover a second diff_drive_controller would imply that the first diff_drive_controller does the work until the issue occurs. Then the second one with the offseted seq. number appears (issue occurs) and after the issue ended (/cmd_vel_out is zero again) the first diff_drive_controller dies as only the higher / offseted sequence numbers go further in the messages. Which sounds quite random to me.

If it's not proprietary, could you share your controller configuration (yaml and launch files)?

Only have access on monday again, will check then.

[matthew-reynolds]

Thanks for explaining the publishing, I think I'm on the same page now 👍

All around very weird. I agree with @bmagyar that logs/rosbags of /cmd_vel, /cmd_vel_out, /joint_states, and /wheel_joint_controller_state (And anything else you think might be useful, maybe /rosout and /odom?) would be very useful to keep debugging this. I'm a bit stumped without more to look at, my only guess is some kind of configuration issue but this is a pretty strange symptom for a configuration issue.

If it's not proprietary, could you share your controller configuration (yaml and launch files)?

Only have access on monday again, will check then.

Great, thanks!

[wienans]

Hey @matthew-reynolds and @bmagyar

First the part how we we launch the controller

<!-- Spawn controllers -->
  <node name="base_controller_spawner" pkg="controller_manager" type="spawner" args="tr_joint_publisher tr_velocity_controller --shutdown-timeout 3" />


  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
    <param name="use_tf_static" value="false" />
  </node>`

loading this parameters before:

`tr_joint_publisher:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50

tr_velocity_controller:
  type: "diff_drive_controller/DiffDriveController"
  left_wheel: 'base_l_wheel_joint'
  right_wheel: 'base_r_wheel_joint'
  publish_rate: 50
  pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
  twist_covariance_diagonal: [0.01, 0.001, 0.001, 0.001, 0.001, 0.07]
  cmd_vel_timeout: 0.25
  publish_cmd: true

  wheel_radius_multiplier: 1 #0.87
  wheel_separation_multiplier: 1 #0.87
 
  base_frame_id: base_footprint

  enable_odom_tf: false

  estimate_velocity_from_position: false

  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 2.0   # m/s
      has_acceleration_limits: true
      max_acceleration       : 0.6   # m/s^2
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 2.0   # rad/s
      has_acceleration_limits: true
      max_acceleration       : 0.6   # rad/s^2

i think nothing special.

And a 10 seconds bag file where the issue is occurring. As is sayed the value of the velocity is not every time equal so this isn't the bag file which created the picture above but shows the same behavior. Added cmd_vel, cmd_vel_out, odom and joint_states, as we don't have a wheel_joint_controller_state
diff_drive.zip

[wienans]

Hey Guys,
i got some new information about the issue. I added some super fancy ROS_INFO Logs in the diff drive code and this showed that the readed command is correctly recognized as zero but after limiting the zero command it is not zero any more

[0m[ INFO] [1618555318.591012402]: Limited Cmd Vel: lin: -0.436525 ang: -0.436525^[[0m
^[[0m[ INFO] [1618555318.659007117]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000^[[0m
^[[0m[ INFO] [1618555318.659026877]: Limited Cmd Vel: lin: -0.395714 ang: -0.395714^[[0m
^[[0m[ INFO] [1618555318.707042316]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000^[[0m
^[[0m[ INFO] [1618555318.707064867]: Limited Cmd Vel: lin: -0.366896 ang: -0.366896^[[0m
^[[0m[ INFO] [1618555318.755049383]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000^[[0m
^[[0m[ INFO] [1618555318.755079962]: Limited Cmd Vel: lin: -0.338106 ang: -0.338106^[[0m
^[[0m[ INFO] [1618555318.803012183]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000^[[0m
^[[0m[ INFO] [1618555318.803029745]: Limited Cmd Vel: lin: -0.309312 ang: -0.309312^[[0m
^[[0m[ INFO] [1618555318.850995530]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000^[[0m
^[[0m[ INFO] [1618555318.851014293]: Limited Cmd Vel: lin: -0.280521 ang: -0.280521^[[0m
^[[0m[ INFO] [1618555318.898986868]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000^[[0m

For convenience here the Code where i output these debug lines.

    // MOVE ROBOT
    // Retreive current velocity command and time step:
    Commands curr_cmd = *(command_.readFromRT());
    const double dt = (time - curr_cmd.stamp).toSec();

    ROS_INFO("Read RT Cmd Vel: lin: %f ang: %f",curr_cmd.lin,curr_cmd.ang);

    // Brake if cmd_vel has timeout:
    if (dt > cmd_vel_timeout_)
    {
      curr_cmd.lin = 0.0;
      curr_cmd.ang = 0.0;
    }

    // Limit velocities and accelerations:
    const double cmd_dt(period.toSec());

    limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
    limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
    
    ROS_INFO("Limited Cmd Vel: lin: %f ang: %f",curr_cmd.lin,curr_cmd.ang);

    last1_cmd_ = last0_cmd_;
    last0_cmd_ = curr_cmd;

Now as i see this output i thought damn i also should put a ROS_INFO for the two different dt's. I guess they are for one timestep negative and thats why the limiter is maybe going crazy.

[matthew-reynolds]

Interesting. Perhaps I was too quick to rule out an irregular period issue when I looked at the EPOS hardware. Can you toss in some more logging for the period to help debug this?

You mention this happens mostly at startup of the system, so it's possible that the system time is being changed once a network connection is established and NTP updates the system time. Which may cause some crazy dt.

Normally, you'd want to use something like ros::SteadyTime for calculating the period, since it wouldn't face this risk of jumping. Since the EPOS hardware calculates the period in an unreliable way, you may need to fork and fix the EPOS hardware. (I have put up a PR against the EPOS hardware to use ros::SteadyTime: RIVeR-Lab/epos_hardware#29)

[wienans]

Hey @matthew-reynolds, thanks for your reply.
"Sadly" the issue occurs only once a day after startup and not after restart so i am quite slow with getting logs of the issue but i directly added logging for the dt.

A good idea with the system time update.

I will first check for the debugging dt log and after i got it, i will checkout your pull request and check if it will fix the issue.

[wienans]

Hi @matthew-reynolds
the expected occured. The dt is negative for one time step and screws the math in the limiter.

^[[0m[ INFO] [1618812773.384779688]: Limited Cmd Vel: lin: 0.000000 ang: 0.000000, cmd_dt: 0.039995 ^[[0m
^[[0m[ INFO] [1618812771.867690353]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000, dt: -1.511326^[[0m
^[[0m[ INFO] [1618812771.867707477]: Limited Cmd Vel: lin: -0.910233 ang: -0.910233, cmd_dt: -1.517056 ^[[0m
^[[0m[ INFO] [1618812771.934698372]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000, dt: 0.012758^[[0m
^[[0m[ INFO] [1618812771.934715003]: Limited Cmd Vel: lin: -0.870031 ang: -0.870031, cmd_dt: 0.067003 ^[[0m
^[[0m[ INFO] [1618812771.982696669]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000, dt: 0.060755^[[0m
^[[0m[ INFO] [1618812771.982713925]: Limited Cmd Vel: lin: -0.841233 ang: -0.841233, cmd_dt: 0.047998 ^[[0m
^[[0m[ INFO] [1618812772.030693835]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000, dt: 0.008487^[[0m
^[[0m[ INFO] [1618812772.030710496]: Limited Cmd Vel: lin: -0.812434 ang: -0.812434, cmd_dt: 0.047998 ^[[0m
^[[0m[ INFO] [1618812772.078697068]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000, dt: 0.056488^[[0m
^[[0m[ INFO] [1618812772.078714831]: Limited Cmd Vel: lin: -0.783633 ang: -0.783633, cmd_dt: 0.048001 ^[[0m
^[[0m[ INFO] [1618812772.126696627]: Read RT Cmd Vel: lin: 0.000000 ang: 0.000000, dt: 0.004502^[[0m

No i will try your Pull Request to Fix the Issue.
Maybe you should also add something in all distributions of diff_drive_controller which detects the negative dt and posts a ROS Warning or maybe a ROS Error. Because the math in the limiter assumes a positive dt.

[wienans]

The Fix of epos_hardware with use ros::SteadyTime fixed this Issue.
I verified it with manually setting the system clock with a offset so it triggers the issue.
I will close the issue, but as said above i would suggest to add a Warning / Error for negative dt in the diff_drive_controller.

Thanks for your help highly appreciate it :)

[matthew-reynolds]

Thanks @wienans, glad the fix is working for you :)

I'm a little uneasy about adding a warning on negative period. The ROS_* logging macros, as with most IO, are super non-RT-safe, so we avoid them in the RT loop. There are a few controllers that have them for rare error cases - Similar to negative period, exceptional cases that shouldn't happen in normal operation - but even those make me a bit uncomfortable. On the other hand, negative period means things are already pretty messed up, so maybe the value of the warning outweighs any RT concerns.

Let's get @bmagyar's opinion about putting a ROS_WARN into the controller_manager::update(), only triggering when period is negative. If he's on board, I'll open a PR.