fixed closed loop odom initialization

[dlarkin3]

Working on the Turtlebot3 with real hardware (not simulation) using Ubuntu 24.04, ROS2 Jazzy and the jazzy branch of turtlebot_manipulation repo.

The odom is significantly inaccurate using the closed loop option leading to difficulty localizing. This can be seen by watching the command: 'ros2 run tf2_ros tf2_echo odom base_link' upon the completion of bringup.

This appears to be related to the odom not starting at zero. Specifically at startup the yaw is set by the difference in the position of the left wheel and right wheel as reported by the openCR board.

I fixed this by establishing the wheel offset once the initial position of the wheels is received by the TurtleBot3ManipulationSystemHardware. The result is the robot can now maintain localization.

[gemini-code-assist]

Hello @dlarkin3, I'm Gemini Code Assist¹! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed!

Summary of Changes

This pull request addresses an issue where the odometry was inaccurate when using the closed loop option on a Turtlebot3 with real hardware. The root cause was identified as the odometry not starting at zero due to an incorrect initial yaw calculation. To fix this, the wheel offset is now established after the initial wheel positions are received by the TurtleBot3ManipulationSystemHardware. This change ensures that the robot maintains accurate localization.

Highlights

• Odometry Initialization: Fixes the odometry initialization to start at zero, resolving localization issues with closed loop control.
• Wheel Offset Calculation: Introduces a mechanism to calculate and apply wheel offsets based on initial wheel positions reported by the OpenCR board.
• Configuration Update: The hardware_controller_manager.yaml file is modified to enable closed loop control and use stamped velocities.

Changelog

• turtlebot3_manipulation_bringup/config/hardware_controller_manager.yaml
  • Changed open_loop parameter from true to false to enable closed loop control (line 39).
  • Changed use_stamped_vel parameter from false to true (line 44).
• turtlebot3_manipulation_hardware/include/turtlebot3_manipulation_hardware/turtlebot3_manipulation_system.hpp
  • Added dxl_wheel_offsets_ member variable to store wheel offsets (line 95).
  • Added init_wheel_offsets_ member variable to track initialization of wheel offsets (line 96).
• turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp
  • Initialized init_wheel_offsets_ to true in the on_init method (line 47).
  • Subtracted dxl_wheel_offsets_ from the wheel positions when reading encoder values in the read method (lines 229 and 232).
  • Added logic in the write method to calculate dxl_wheel_offsets_ based on the initial wheel positions reported by the OpenCR board (lines 292-295).

Using Gemini Code Assist

The full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips.

Invoking Gemini

You can request assistance from Gemini at any point in your pull request via creating an issue comment (i.e. comment on the pull request page) using either /gemini <command> or @gemini-code-assist <command>. Below is a summary of the supported commands.

Feature	Command	Description
Code Review	/gemini review	Performs a code review for the current pull request in its current state.
Pull Request Summary	/gemini summary	Provides a summary of the current pull request in its current state.
Comment	@gemini-code-assist	Responds in comments when explicitly tagged, both in issue comments and review comments.
Help	/gemini help	Displays a list of available commands.

Customization

To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a .gemini/ folder in the base of the repository. Detailed instructions can be found here.

Limitations & Feedback

Gemini Code Assist is currently in preview and may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments to provide feedback.

You can also get AI-powered code generation, chat, as well as code reviews directly in the IDE at no cost with the Gemini Code Assist IDE Extension.

---

The wheel's yaw,
A zero start, a true fix,
Robot finds its way.

Footnotes

1. Review the Privacy Notices, Generative AI Prohibited Use Policy, Terms of Service, and learn how to configure Gemini Code Assist in GitHub here. Gemini can make mistakes, so double check it and use code with caution. ↩

[gemini-code-assist]

Code Review

The pull request addresses an important issue with the Turtlebot3's odometry initialization when using closed-loop control. The provided fix aims to correct the initial yaw offset, which improves localization accuracy. The approach of establishing wheel offsets after receiving initial wheel positions seems reasonable. However, there are a few areas that could be improved for clarity and robustness.

Summary of Findings

• Potential Race Condition: The double read of opencr_->get_wheel_positions() within the init_wheel_offsets_ block in the write method might not guarantee the second read is stable or accurate. There's a potential race condition where the wheel positions might change between the two reads, leading to incorrect offset calculation. This could be mitigated by ensuring the values are stable or using a more robust method for capturing the initial offset.
• Clarity of Offset Initialization: The logic for initializing dxl_wheel_offsets_ is not immediately clear. The comments suggest the first call returns zeros, and the second returns the offsets. This behavior should be explicitly documented or refactored for better readability. It's not clear why the first call returns zero, and the second call returns the offsets. It would be useful to understand the underlying mechanism that causes this behavior.
• Missing Error Handling: The code does not include error handling for the opencr_->get_wheel_positions() calls. If these calls fail, the dxl_wheel_offsets_ might not be initialized correctly, leading to further issues. It's important to add error checking to ensure the offsets are valid.

Merge Readiness

The pull request addresses a critical issue with odometry initialization. However, the potential race condition and lack of error handling need to be addressed before merging. I am unable to directly approve this pull request, and recommend that others review and approve this code before merging. Specifically, the issues I've raised need to be addressed before this pull request is ready to be merged.

[gemini-code-assist]

The double read of opencr_->get_wheel_positions() is a bit unclear and potentially problematic. It's not immediately obvious why the first call returns zeros and the second returns the offsets. This could also lead to a race condition if the wheel positions change between the two reads. Consider refactoring this logic to ensure a stable and accurate offset calculation. Also, consider adding a check to ensure that the values returned are valid, and log an error if they are not.

Could you elaborate on why the first call returns zeros? Is this intentional, or a side effect of the underlying implementation?

Suggested change

	if (init_wheel_offsets_){
	dxl_wheel_offsets_ = opencr_->get_wheel_positions(); // First time returns all zeros
	dxl_wheel_offsets_ = opencr_->get_wheel_positions(); // Second time returns the offsets
	init_wheel_offsets_ = false;
	}
	if (init_wheel_offsets_) {
	auto wheel_positions = opencr_->get_wheel_positions();
	if (/* Add condition to check if wheel_positions are valid */) {
	dxl_wheel_offsets_ = wheel_positions;
	init_wheel_offsets_ = false;
	} else {
	RCLCPP_ERROR(logger, "Failed to get valid wheel positions for offset initialization.");
	}
	}

[gemini-code-assist]

Consider adding a check to ensure that opencr_->get_wheel_positions() returns valid data before subtracting the offsets. If the read fails, the position data could be incorrect.

Suggested change

	dxl_positions_[0] = opencr_->get_wheel_positions()[opencr::wheels::LEFT] - dxl_wheel_offsets_[opencr::wheels::LEFT];
	dxl_velocities_[0] = opencr_->get_wheel_velocities()[opencr::wheels::LEFT];
	dxl_positions_[1] = opencr_->get_wheel_positions()[opencr::wheels::RIGHT];
	dxl_positions_[1] = opencr_->get_wheel_positions()[opencr::wheels::RIGHT] - dxl_wheel_offsets_[opencr::wheels::RIGHT];
	auto left_wheel_position = opencr_->get_wheel_positions()[opencr::wheels::LEFT];
	auto right_wheel_position = opencr_->get_wheel_positions()[opencr::wheels::RIGHT];
	if (/* Add condition to check if left_wheel_position and right_wheel_position are valid */) {
	dxl_positions_[0] = left_wheel_position - dxl_wheel_offsets_[opencr::wheels::LEFT];
	dxl_positions_[1] = right_wheel_position - dxl_wheel_offsets_[opencr::wheels::RIGHT];
	} else {
	RCLCPP_ERROR(logger, "Failed to get valid wheel positions for reading.");
	}

[gemini-code-assist]

Enabling use_stamped_vel is a good change for ensuring proper time synchronization. However, it's worth noting that this requires the cmd_vel messages to be stamped with a timestamp. Ensure that the upstream nodes publishing cmd_vel are configured to do so.