Not So Simple Pleasures: GoPi5Go-Dave is a ROSbot again

cyclicalobsessive · August 29, 2024, 7:39pm

It was looking like GoPi5Go-Dave might never achieve “ROSbot” status, but he is “officially a ROSbot” today.

ROS 2 (in Docker container) starts when Dave boots
The basic “GoPi5Go-Dave nodes” start when ROS 2 in Docker starts
- ros2_gopigo3_node GoPiGo3 ROS2 publishes odom, offers /cmd_vel etc
- battery_node publishes /battery_state
- docking_node publishes /dock_status, offers /dock and /undock services
- dave_node calls /dock when vBatt<10v, and calls /undock when charge current < -175mA
- odometer records all ROS /cmd_vel movement
- joy_node handles wireless F710 joy controller to publish /cmd_vel
- say_node TTS speech server offers /say {“phrase”} service

(gopi5go_power_service, logLife, and safetyShutdown remain as Python programs running in PiOS Bookworm that also start at boot.)

And today for the first time - GoPi5Go-Dave responded to my wireless F710 joy controller and published his adventure for the rViz2 client listening in an Ubuntu virtual machine running on my Mac desktop:

GoPi5Go-Dave is managing his docking and undocking behaviors in ROS 2 Humble, and logging his (joystick commanded) travels:

2024-08-29 15:16:34|start_point - x:  0.013 y:  0.001 z:  0.000 heading:    0
2024-08-29 15:16:42|stop_point -  x:  0.725 y:  0.136 z:  0.000 heading:   -2 - moved:  0.792 meters in 8.3s
...
2024-08-29 15:16:57|start_point - x:  0.726 y:  0.136 z:  0.000 heading:    4
2024-08-29 15:17:05|stop_point -  x:  0.728 y: -0.066 z:  0.000 heading:  -89 - moved:  0.214 meters in 7.8s
...
2024-08-29 15:17:06|start_point - x:  0.728 y: -0.066 z:  0.000 heading:  -89
2024-08-29 15:17:07|stop_point -  x:  0.721 y: -0.097 z:  0.000 heading: -106 - moved:  0.033 meters in 1.3s

2024-08-29 15:17:08|start_point - x:  0.721 y: -0.097 z:  0.000 heading: -106
2024-08-29 15:17:12|stop_point -  x:  0.309 y: -0.100 z:  0.000 heading:  175 - moved:  0.420 meters in 4.2s

2024-08-29 15:17:14|start_point - x:  0.309 y: -0.100 z:  0.000 heading:  175
2024-08-29 15:17:17|stop_point -  x:  0.070 y: -0.091 z:  0.000 heading: -165 - moved:  0.240 meters in 2.8s
...

and achieved the same lousy odometry (after angular motion) of the former “HumbleDave” and “ROS_GoPiGo3” robots that inhabited his soul before.

The graph of nodes and topics:

There are lots of issues still to slay, but today is a milestone crossing for GoPi5Go-Dave to join HumbleDave and Create3-WaLI as a ROSbot, and joining Create3-WaLI as a 24/7 ROSbot managing to stay alive with ROS 2.

jimrh · August 30, 2024, 6:44am

Wow!

Huge progress!!

Does the joystick commanded motion also include a FPV via the robot’s camera(s)?

I note there’s an “IMU” category in the first screen-shot. Does than mean you can add the Dexter IMU to Dave too? Would that improve things or just add another layer of needless complexity, (and i²c errors? )

With the improvements in the LIDAR sensors you’ve shown us, has there been a corresponding improvement in the IMU front, advancing the technology beyond that of the earlier Dexter IMU?

KeithW · August 30, 2024, 9:29am

Congratulations - great progress!
/K

cyclicalobsessive · August 30, 2024, 12:53pm

Not yet - HumbleDave had both a PiCam and an Oak-D-Lite camera, which was a total of four cameras, but only the hi-resolution, limited view PiCam was ROS-ified at that time.

GoPi5Go-Dave only has an Oak-D-W camera which will use the ROS driver from Create3-WaLI eventually. With that driver, I will enable one visual full color stream and one 3D point-cloud stream - both much wider angle view than the former PiCam view.

My Dexter IMU died, but I was never able to realize a benefit from the IMU when it was working. I tried to learn how to configure the ROS Extended Kalman Filter (EKF) package to fuse the GoPiGo3 encoders and the Dexter IMU to give better odometry, but my brain was insufficient to improve HumbleDave’s positional intelligence even though Adafruit says of the BNO055 IMU:

that just gives you the directly usable information without requiring you to first consult with a PhD to learn the arcane arts of Sensor Fusion .

GoPi5Go-Dave actually has two IMUs on-board - an inexpensive Chinese knockoff “older, well represented in the ROS community” MPU9250 9-axis IMU, and a “latest generation” BNO086 9-axis IMU which is supposedly “smarter, faster, and more accurate” than the DI BNO055 IMU.

So I expect another few months of “without-IMU vs with-IMU” investigation is in GoPi5Go-Dave’s future.

I have an idea about why the encoder based angular motion sensing on the GoPiGo3 is bad, but my math skills are not up to programming the test. When the GoPiGo3 drives, the controller board uses a PID (Proportional Integrative Derivative) control algorithm from the encoder data to select the power to apply to each motor to keep the wheel rotation angles roughly equal. The PID algorithm does not factor in wheel slip differences at startup, and wheel diameter differences due to the varying contact path from the non-perfect wheel alignment and the extra deformation of the wheels due to Dave’s much higher weight than a stock GoPiGo3. This is very evident when I try to drive GoPi5Go-Dave at maximum speed - he arcs to the right with great intensity. (I purposely did not drive HumbleDave at maximum GoPiGo3 speeds.)

HumbleDave had an Nitendo gamepad controller which could only send “forward”, “backward”, “spin left”, “spin right” so I set the max speed slow to keep the driving as straight as possible. GoPi5Go-Dave inherited the Logitech F710 “Playstation” controller (bought for Create3-WaLI), which has real joystick outputs so I can drive Dave at GoPiGo3 minimum-slow, max-fast, and any speed in-between now.

There is a ROS package to control the motors on differential drive robots that most ROS robots use to make them drive straight AND output the odometry. Another month or so investigation to learn how to use a common ROS package that may be able to improve the GoPiGo3 odometry, but no-one but me to do the investigation…

The promise of ROS LIDAR based localization (SLAM_toolbox, Nav2) and vision-plus-LIDAR based localization (RTABmap) packages to give “fused odometry and corrected pose estimation” has yet to be characterized for the GoPiGo3. My goals for these “Already Coded For ROS Robots” technologies are:

Dave should know what “room” he is in
Dave should be able to plan a path from “room x” to “room y”
Dave should be able to detect and avoid obstacles
Dave should be able to venture away from his dock and find his way back ( Create3-WaLI came with this feature in his $400 price)

and when he can do all that, start on:

record images of “interesting objects” tagged with “location information”
dialog with me (or the Internet) to identify-classify the collected images
dialog with me to build an object-functions and object-object-relationships knowledge base
(I haven’t a clue what Dave could do with object function knowledge, but just knowing he “knows” it would be satisfying to me. “Dave, where did I leave your remote controller?”, “Dave, where is Hanna?”)
implement a “transfer learning” object recognition model workflow to be able to recognize previously investigated objects (probably have to run the learning on my desktop, but perhaps adding the Raspberry Pi AI hat could give GoPi5Go-Dave onboard learning).

Thank you both for reading / following GoPi5Go-Dave’s adventure. Knowing you care is really a joy in my life.

cyclicalobsessive · September 1, 2024, 10:05pm

“One small step for …”

Managed to add “Reset Odometry to {0,0,0,1} on Successful Docking” feature to the “dave_node”. The service to reset odometry has been in the ROS 2 GoPiGo3 node since first release years ago, and I have been using the service from the command line.

Now that GoPi5Go-Dave is managing his own docking, he needed to (asynchronously) call the service programmatically, which involves:

Check to see if there is an /odom/reset service ready to receive requests
Create a std_srvs/srv/Trigger request and publish it to the /odom/reset service (offered by ros2_gopigo3_node)
Save the “odom_reset_srv_future” callback returned from the /odom/reset service (from ros2_gopigo3_node)
Periodically call odom_reset_srv_future.done() and log the successful odometry reset when it happens
Clean up (prevent further checking the future.done() by setting self.odom_reset_srv_future = None)

Asynchronous programming is so hard to think about, and debugging multiple state machines is unbelievable:

active nodes:
- ros2_gopigo3_node
- battery
- docking_node
- dave_node
- odometer
- (say_node is commented out at the moment to not explode and take docker down with it - new issue on Pi5)

The goings at docking time:

the battery object announces voltage is low to anyone interested
the dock announces no robot is docked to anyone interested
the dave_node notices the low voltage and the free dock, and pubs a /dock service request
the docking_node accepts the /dock request and actually drives the robot onto the dock,
then waits a bit to see if the battery will report charging, then tells the caller (dave_node) the /dock request is done and successful
dave_node has been looping waiting for the dock request to complete, checks the result status for is_docked and is_charging, then calls the ros2_gopigo3_node odometry reset service,
the ros2_gopigo3_node resets the robots “pose from odometry” and replies done with success
dave_node notices the reply and loops quietly waiting for the battery to report charging current has dropped below the “time to undock” threshold.

Life in ROS is complicated by the possibilities that are never going to happen, and the probabilities that are hoped will never happen.

2024-09-01 16:35:37|stop_point -  x: -0.173 y:  0.000 z:  0.000 heading:    0 - moved:  0.173 meters in 3.6s
2024-09-01 16:40:43|start_point - x: -0.173 y:  0.000 z:  0.000 heading:    0
2024-09-01 16:40:46|stop_point -  x: -0.347 y: -0.000 z:  0.000 heading:   -0 - moved:  0.174 meters in 3.7s
2024-09-01 16:50:58|start_point - x: -0.347 y: -0.000 z:  0.000 heading:   -0
2024-09-01 16:50:58|stop_point -  x: -0.347 y: -0.000 z:  0.000 heading:   -0 - moved:  0.000 meters in 0.0s
2024-09-01 16:50:58|start_point - x: -0.347 y: -0.000 z:  0.000 heading:   -0
2024-09-01 16:50:58|stop_point -  x: -0.347 y: -0.000 z:  0.000 heading:   -0 - moved:  0.000 meters in 0.1s
2024-09-01 17:04:35|start_point - x: -0.347 y: -0.000 z:  0.000 heading:   -0
2024-09-01 17:04:39|stop_point -  x: -0.521 y:  0.001 z:  0.000 heading:   -0 - moved:  0.173 meters in 3.6s
2024-09-01 17:04:50|RESET **** -  x:  0.000 y:  0.000 z:  0.000 heading:    0 - RESET    <<----- YEAH


2024-09-01 16:53|[logMaintenance.py.main]** manual undock to test dave_node odom reset **
2024-09-01 17:04|dave_node.py| ---- GoPi5Go-Dave ROS Docking 957 : success at battery 10v after 3.3 h playtime 
2024-09-01 17:04|dave_node.py| ---- Successful docking - odometry reset to {0,0,0,1}   <<---- YEAH
2024-09-01 17:13|[logMaintenance.py.main]** test passed - restarting docker service **

cyclicalobsessive · September 3, 2024, 3:19pm

Arrgh … dave_node failed him this morning at 5AM

I turned DEBUG off thinking Dave is bug free - bad idea. Need more logging even with debug off.

He announced he was docking, (but didn’t succeed to dock such that charging began). My non-ROS test_docking program had a docking failure recovery attempt, but the ROS dave_node does not notice the failure and does not attempt to fix a failure. Now, I know what to work on next.

2024-09-03 05:13|battery_status.volts 10.0v calling dock service  - spoken: True
2024-09-03 05:22|15 minute warning.  Battery Voltage is 9.8 volts.  Need to be Docked - quiet time

2024-09-03 02:23|dave_node.py| ---- GoPi5Go-Dave ROS 2 Undocking at Charge Current -175 mA 12.11v after 7.7 h charging
2024-09-03 05:22|[logMaintenance.py.main]** safetyShutdown: voltage warning blinker activated  **
2024-09-03 05:26|[logMaintenance.py.main]** SAFETY SHUTDOWN - BATTERY LOW **
2024-09-03 05:26|[safetyShutdown.py.main]** Current Battery 9.7v  3.6%  (Reading 9.00v) **
2024-09-03 05:28|lifelogger.dEmain execution: 5.60
2024-09-03 08:55|------------ boot ------------

Good thing his safetyShutdown program decided not to wake me up at 5:22AM and simply shut Dave down safely.

p.s. Even the safetyShutdown program crashed when I revived Dave. Too many “just a little thing” changes at one time.

jimrh · September 5, 2024, 3:56pm

“To err is human, but to really foul things up you need a computer!”

When you need special logging to debug the logging - maybe you should step back and rethink this?

cyclicalobsessive · September 5, 2024, 4:51pm

Yes, moving to the Pi5 presents two uncharted hurdles:

run the working ROS in Docker which will is complex and will break something
run the working Dave native in freshly released ROS on the (only just now) freshly released version of Ubuntu

The second option was not available when I committed to the first option, and the “rethinking” is as monumental as finding and fixing Dockerized issues.

Yesterday, I decided the docking failure was mechanical and perhaps a software repair stategy should be secondary to a mechanical repair strategy. I bent the front edge of the docking contacts up, so testing continues