search

ROBOTIS-GIT / turtlebot3

ROS packages for Turtlebot3
http://turtlebot3.robotis.com
Apache License 2.0
1.44k stars 998 forks source link

Robot Odometry - no exception handling in read from device #901

Open wojciechmadry opened 1 year ago

wojciechmadry commented 1 year ago

ISSUE TEMPLATE ver. 0.4.0

  1. Which TurtleBot3 platform do you use?

    • [ ] Burger
    • [ ] Waffle
    • [X] Waffle Pi

  2. Which ROS is working with TurtleBot3?

    • [ ] ROS 1 Kinetic Kame
    • [ ] ROS 1 Melodic Morenia
    • [ ] ROS 1 Noetic Ninjemys
    • [ ] ROS 2 Dashing Diademata
    • [ ] ROS 2 Eloquent Elusor
    • [ ] ROS 2 Foxy Fitzroy
    • [X] ROS2 Galactic
    • [ ] etc (Please specify your ROS Version here)

  3. Which SBC(Single Board Computer) is working on TurtleBot3?

    • [ ] Intel Joule 570x
    • [ ] Raspberry Pi 3B+
    • [X] Raspberry Pi 4
    • [ ] etc (Please specify your SBC here)

  4. Which OS you installed on SBC?

    • [ ] Raspbian distributed by ROBOTIS
    • [ ] Ubuntu MATE (16.04/18.04/20.04)
    • [X] Ubuntu preinstalled server (18.04/20.04)
    • [ ] etc (Please specify your OS here)

  5. Which OS you installed on Remote PC?

    • [ ] Ubuntu 16.04 LTS (Xenial Xerus)
    • [ ] Ubuntu 18.04 LTS (Bionic Beaver)
    • [X] Ubuntu 20.04 LTS (Focal Fossa)
    • [ ] Windows 10
    • [ ] MAC OS X (Specify version)
    • [ ] etc (Please specify your OS here)

  6. Specify the software and firmware version(Can be found from Bringup messages)

    • Software version: [x.x.x]
    • Firmware version: [x.x.x]

  7. Specify the commands or instructions to reproduce the issue.

    • Drive robot and spin

  8. Copy and Paste the error messages on terminal.

    • No error (Exception isn't handling)

  9. Please describe the issue in detail.

Probably related issue: #899 #821 #880 We are encountering big changes in robot position/odometry which can be observed e.g. while using slam_toolbox or navigation2. At first we thought that robot "jump" is caused by navigation2 stack here is how it looks like: nav2 issue - it was caused because navigation2 rotate robot multiple times.

This bug causes big robot jumps (odometry changed the value to a very big number).

When we drive with a robot (It happens almos always when we rotate the robot) exception No Mans Land is read from dxl_sdk_wrapper and published -51673.713111639 value on /joint topic. When we checked what value it is read from the wrapper (before calculation) the result was: 0xFDFDFDFD ( No Mans Land excepion thrown)

We've create a workaround and when the position read from dxl_sdk_wrapper is equal to 0xFDFDFDFD we don't calculate and publish this message, this prevents robot jumps and everything works fine.

But this is just a temporary solution, the exception should be handled correctly with dxl_sdk_wrapper.

wojciechmadry commented 1 year ago

A simple solution showing the issue is here #902

jobst-n commented 1 year ago

Dropping a frame is only a workaround . It seems the reason is a wrong dynamixel package. The signature FDFD is used to prefill the package. The error happens only when the low byte of the encoder counter of the left (Id 1) motor decrements around zero.

jobst-n commented 1 year ago

This is what happend: In the slaved dxl message from OpenCR to PI the fields velocity R (132) and position L (136) are adjacent as defined in:

turtlebot3_ws/src/turtlebot3/turtlebot3_node/include/turtlebot3_node/control_table.hpp

If the velocity R is negative (a small negative int32) the record reads something like

xx FF FF FF (low byte first)

If at the same time the low byte of the position L record hits 253 = FD (this happens shortly before wrapping around zero and at irregular intervalls) the sequence looks something like

xx FF FF FF FD xx xx xx

which by accident is the packet header signature of the dxl 2.0 protocol.

Apparently this triggers a termination of the messsage which gets filled with FD FD (the default dxl fill pattern) and we end up with the nomansland value in the position L and R record

xx FF FF FF FD FD FD FD FD FD ....

I "fixed" this by changeing the order of records both in the

OpenCR firmware .arduino15/packages/OpenCR/hardware/OpenCR/1.5.2/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp

... ControlItem present_current_left = {136, RAM, 4, READ}; // was 120 ControlItem present_current_right = {140, RAM, 4, READ}; // was 124 ControlItem present_velocity_left = {128, RAM, 4, READ}; ControlItem present_velocity_right = {132, RAM, 4, READ}; ControlItem present_position_left = {120, RAM, 4, READ}; // was 136 ControlItem present_position_right = {124, RAM, 4, READ}; // was 140 ...

and in the turtlebot3_node running on the raspberry pi:

turtlebot3_ws/src/turtlebot3/turtlebot3_node/include/turtlebot3_node/control_table.hpp

... ADDR_PRESENT_CURRENT_L = 136, // was 120 ADDR_PRESENT_CURRENT_R = 140, // was 124 ADDR_PRESENT_VELOCITY_L = 128, ADDR_PRESENT_VELOCITY_R = 132, ADDR_PRESENT_POSITION_L = 120, // was 136 ADDR_PRESENT_POSITION_R = 124, // was 140

...

This works fine because the field before 120 is not used but is not a real fix of the error. It would be great if someone with more knowledge of the internals of the dxl slave protocol took a look at the issue.

kgallowa commented 1 year ago

Seems like a lot of current issues ( #880, #899, #821) all stem from this one problem. Is it possible to package the fix that @jobst-n proposed into a firmware update?

jobst-n commented 1 year ago

If I got it right, the error should only occur when the right motor turns backwards (has negative velocity) can you please check that. And has anybody observed this error with a ros1 system thanks for your help

ROBOTIS-Will commented 1 year ago

An updated OpenCR ROS2 firmware V220127R1 (0.2.1) is released. Please download and upload the new firmware and let me know if this doesn't fix the problem. Thank you very much for your patience!