So, turns out that the Charuco (chessboard + aruco) board I printed out to calibrate the OAK-D was too small. Maybe I should print it on an A3 sized sheet instead. But because of this drawback, I wasn’t able to proceed with my OAK-D tasks. I will go to the office again next week, so hopefully I can print a bigger image. Meanwhile, I decided to work on another interesting way of passing waypoints to move_base, using a PS4 controller. I realized this was needed because it started getting annoying trying to move the robot manually within its environment to get reference poses, and then manually entering these poses into a YAML file. My motive was to find a way to drive the robot around manually using the PS4 controller and store the current position (target frame to base_link transform - target frame is map when using AMCL, and odom when using SLAM) by simply pressing a button on the controller. Similarly, I also needed another script, to pass these stored positions to the move_base action server one by one while also checking the result. While I already had a Behavior Tree from my previous update to do this, I needed to modify the code to start working only when a specific button on the PS4 controller is pressed. Unfortunately, I was unable to append the code with a subscriber (to the PS4 controller status), so in order to get something quickly up and running quickly, I used Python instead of C++ and if/else conditions instead of a behavior tree. Here’s how I did it:
PS4 Controller Node I updated the PS4 controller node to publish specific modes as a custom Mode message type. So, instead of deciphering the mode by subscribing to the PS4 controller button status in every node, the PS4 controller is the only node to decipher the mode and publish it. The other nodes simply subscribe and act based on the modes. In this case, the two modes in use are the Storage mode, started/stopped by pressing the Options button, and the Playback mode, which is started/stopped by pressing the Triangle button. The Storage mode can only be entered when the robot is already in the Teleop mode, and the Playback mode can only be entered when the robot is already in Autonomous mode. The Cross button is used to switch between Teleop and Autonomous mode.
Waypoint Store The Waypoint Store is a python script that subscribes to the mode topic, and if it is correctly in the Storage mode, reads the current target frame to base link transform, where the target frame is read via the parameter server. The transform is read on the press of a button, the right joystick button in this case. These transforms are converted to X, Y and Theta values and then appended into a YAML file using pyyaml. Waypoint 0, the first stored location is always the starting pose of the robot. There is also a cursor feature, that lets the user move the cursor up and down using the r1 and r2 buttons, which allows the user to go back and overwrite specific waypoints. Finally, the stored list of waypoints are also published as a PoseArray message, every time a new waypoint is added. This lets users visualize the stored waypoints on RViz, also makes it easier to use the cursor feature because it shows which waypoint is being overwritten.
Waypoint Player Once the waypoints are written to a YAML file, the Waypoint Player script is used to pass them to move_base in a sequence. This script subscribes to the mode topic and if it is correctly in Playback mode, a waypoint from the YAML file is passed to move_base. If the task is successful, a counter increments and in the next loop, the next waypoint is passed. Once the end of the list is reached, the player restarts the sequence by passing Waypoint 0 as the next position. The triangle button on the PS4 controller is used to enter into Playback mode, but its also used to pause it (by simply pressing it again). In the pause state, the robot is allowed to finish its current task and then no further waypoints are passed. The counter value is retained, so that when in Playback mode again, the correct next waypoint can be passed.
A demo of these features can be seen in this video:
Meanwhile, I also replaced my RPi4 4GB with a RPi4 8GB. The video above is with the 8GB version. It definitely makes things run way faster. Some nodes no longer show timing issues, and the Arduino completely stopped losing sync while running, fixing a major chunk of my navigation problems. I made a rookie error of trying my RPi4 4GB SD card with the 8GB board, but it ended up not working, and corrupted the SD card entirely. I soon realized that the 8GB version can run 64bit OSes quite well, so I started off from scratch with a new SD card. Fortunately, I had pushed all my work to my git repo before corrupting the SD card. Coming to next week, I plan on getting the correct sized Charuco board printed so that I can finally get on with the OAK-D recalibration and the DepthAI ROS repo updates in the last few weeks. I also want to try and fine-tune move_base since the robot keeps stopping in its path at times, due to some timing mismatch. I need to play around with controller frequency and patience values to fix this. More updates next week..