is_docked toggled 7 minutes after docking ending recharge

[slowrunner]

Create3 battery was run down to 3% then docked successfully.

$ ./dock_it.sh 
1636292626.039450 [0]       ros2: using network interface wlan0 (udp/10.0.0.29) selected arbitrarily from: wlan0, wlan1
Waiting for an action server to become available...
Sending goal:
     {}

Goal accepted with ID: 6abdbd55f1d540709c6c0b7f70dbfbf0

Result:
    is_docked: true

Goal finished with status: SUCCEEDED

==== /battery_state backing onto dock at 3%, and docked showing charging started:

 ---
header:
  stamp:
    sec: 26480
    nanosec: 2000000
  frame_id: base_link
voltage: 13.30210018157959
temperature: 27.0
current: -0.5260000228881836
charge: 0.06137999892234802
capacity: 2.0460000038146973
design_capacity: 2.0460000038146973
percentage: 0.029999999329447746
power_supply_status: 0
power_supply_health: 0
power_supply_technology: 0
present: true
cell_voltage: []
cell_temperature: []
location: ''
serial_number: ''
---
header:
  stamp:
    sec: 26490
    nanosec: 5000000
  frame_id: base_link
voltage: 13.30210018157959
temperature: 27.0
current: -0.40400001406669617
charge: 0.06137999892234802
capacity: 2.0460000038146973
design_capacity: 2.0460000038146973
percentage: 0.029999999329447746
power_supply_status: 0
power_supply_health: 0
power_supply_technology: 0
present: true
cell_voltage: []
cell_temperature: []
location: ''
serial_number: ''
---
header:
  stamp:
    sec: 26500
    nanosec: 4000000
  frame_id: base_link
voltage: 13.305627822875977
temperature: 27.0
current: 0.8999999761581421          <-- STARTED CHARGING 26500
charge: 0.06358736008405685
capacity: 2.0460000038146973
design_capacity: 2.0460000038146973
percentage: 0.031078865751624107
power_supply_status: 0
power_supply_health: 0
power_supply_technology: 0
present: true
cell_voltage: []
cell_temperature: []
location: ''
serial_number: ''
---

420 seconds / 7 minutes later charging stopped mysteriously and is_docked found to have toggled to False:

---
header:
  stamp:
    sec: 26910
    nanosec: 6000000
  frame_id: base_link
voltage: 13.475337982177734
temperature: 27.0
current: 0.8999999761581421
charge: 0.16977322101593018
capacity: 2.0460000038146973
design_capacity: 2.0460000038146973
percentage: 0.08297811448574066
power_supply_status: 0
power_supply_health: 0
power_supply_technology: 0
present: true
cell_voltage: []
cell_temperature: []
location: ''
serial_number: ''
---
header:
  stamp:
    sec: 26920
    nanosec: 1000000
  frame_id: base_link
voltage: 13.40037727355957
temperature: 27.0
current: -0.40400001406669617           <--- STOPPED CHARGING 26920 (420 seconds - 7 minutes)
charge: 0.12287096679210663
capacity: 2.0460000038146973
design_capacity: 2.0460000038146973
percentage: 0.0600542351603508
power_supply_status: 0
power_supply_health: 0
power_supply_technology: 0
present: true
cell_voltage: []
cell_temperature: []
location: ''
serial_number: ''
---

Checked /dock topic much later when noticed charging was no longer happening.

$ ros2 topic echo /dock
1636303884.938074 [0]       ros2: using network interface wlan0 (udp/10.0.0.29) selected arbitrarily from: wlan0, wlan1
header:
  stamp:
    sec: 30694
    nanosec: 633000000
  frame_id: base_link
dock_visible: true
is_docked: false

Bot is still located on the dock visibly on the dock. /odom is reporting:

  pose:
    position:
      x: -0.9706671547660098
      y: 1.817533294345636
      z: 0.0
    orientation:
      x: -0.0
      y: 0.0
      z: 0.6655952078281054
      w: -0.7463129499856351

[justinIRBT]

@slowrunner You seem to be running into this dock reporting issue with your test setup more than we see it, I wonder if you can play with some numbers for us. The threshold that determines if the robot is docked are here: irobot_create_gazebo_plugins/include/irobot_create_gazebo_plugins/gazebo_ros_docking_status.hpp

  // Docked thresholds
  const double DOCKED_DISTANCE{0.075};  // Max distance in meters.
  const double DOCKED_YAW{M_PI / 30.0};       // Max Yaw between dock and robot in radians.

If you make small increases to those numbers, does the problem resolve itself? I wonder if our threshold is too tight or if our robot has a very small idle drift from simulator body collisions (this could be tested by leaving the sim on for a long time and seeing if the robot is visibly getting farther away from the dock without reaching a steady state). @rjcausarano Is it possible the robot could be slowly repelled from the dock when it is in contact vs. just drifting from collisions within its own bodies?

[slowrunner]

our robot has a very small idle drift from simulator body collisions (this could be tested by leaving the sim on for a long time and seeing if the robot is visibly getting farther away from the dock

I was wondering about this as well. After a long time running, when I check back to gazebo, the "walls" that I place using the cube shape, have "walked" away. I have never found the bot visibly off the dock, but perhaps it wobbled and "lost contact"

I changed the two parms to 0.100 and M_PI / 20, rebuilt, and reran the test.

The is_docked started out true. I performed the "dance example twice (until the dock_visible became true).

The first dock goal did not turn the is_docked to true, but a second dock goal did.

After a short while the is_docked again toggled to false.

Here are the position msgs from odom topic docked and "auto magically undocked":

Docked:

    sec: 264
    nanosec: 153000000
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.0005308739049845198
      y: -0.016469340509502824
      z: 0.0
    orientation:
      x: -0.0
      y: 0.0
      z: 0.0016669600850229475
      w: -0.9999986106210723

Automagically undocked:
header:
  stamp:
    sec: 434
    nanosec: 712000000
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.0005294717861749289
      y: -0.01646933532131849
      z: 0.0
    orientation:
      x: -0.0
      y: 0.0
      z: 0.001364338378228047
      w: -0.9999990692899617

[justinIRBT]

Hmmm, those positions look pretty close, especially given the increased thresholds. @rjcausarano Any hypotheses?