Skip to content

fixed closed loop odom initialization #83

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: jazzy
Choose a base branch
from
Open

fixed closed loop odom initialization #83

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5c8614d
fixed closed loop odom initialization
dlarkin3 Apr 26, 2025
28d6d53
solved runtime error in controller node
dlarkin3 Apr 26, 2025
ffed7f2
solved runtime error in controller node
dlarkin3 Apr 26, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions turtlebot3_manipulation_bringup/config/hardware_controller_manager.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,12 +36,12 @@ diff_drive_controller:
pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]

open_loop: true
open_loop: false
enable_odom_tf: true

cmd_vel_timeout: 0.5
#publish_limited_velocity: true
use_stamped_vel: false
use_stamped_vel: true
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

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.

#velocity_rolling_window_size: 10

# Preserve turning radius when limiting speed/acceleration/jerk
Expand Down
4 changes: 2 additions & 2 deletions ...3_manipulation_description/ros2_control/turtlebot3_manipulation_system.ros2_control.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,10 +171,10 @@ https://github.com/ros-controls/ros2_control_demos/blob/foxy/ros2_control_demo_d
<joint name="${prefix}gripper_right_joint">
<param name="mimic">gripper_left_joint</param>
<param name="multiplier">1</param>
<command_interface name="position">
<!-- <command_interface name="position">
<param name="min">-0.010</param>
<param name="max">0.019</param>
</command_interface>
</command_interface> -->
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
Expand Down
3 changes: 3 additions & 0 deletions ...tion_hardware/include/turtlebot3_manipulation_hardware/turtlebot3_manipulation_system.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,9 @@ class TurtleBot3ManipulationSystemHardware
std::vector<double> dxl_positions_;
std::vector<double> dxl_velocities_;

std::array<double, 2> dxl_wheel_offsets_;
bool init_wheel_offsets_;

std::vector<double> opencr_sensor_states_;
};
} // namespace turtlebot3_manipulation_hardware
Expand Down
11 changes: 9 additions & 2 deletions turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ hardware_interface::CallbackReturn TurtleBot3ManipulationSystemHardware::on_init
usb_port_ = info_.hardware_parameters["opencr_usb_port"];
baud_rate_ = stoi(info_.hardware_parameters["opencr_baud_rate"]);
heartbeat_ = 0;
init_wheel_offsets_ = true;

joints_acceleration_[0] = stoi(info_.hardware_parameters["dxl_joints_profile_acceleration"]);
joints_acceleration_[1] = stoi(info_.hardware_parameters["dxl_joints_profile_acceleration"]);
Expand Down Expand Up @@ -225,10 +226,10 @@ hardware_interface::return_type TurtleBot3ManipulationSystemHardware::read(
RCLCPP_WARN(logger, "Failed to read all control table");
}

dxl_positions_[0] = opencr_->get_wheel_positions()[opencr::wheels::LEFT];
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];
Comment on lines +229 to +232
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

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.");
}

dxl_velocities_[1] = opencr_->get_wheel_velocities()[opencr::wheels::RIGHT];

dxl_positions_[2] = opencr_->get_joint_positions()[opencr::joints::JOINT1];
Expand Down Expand Up @@ -287,6 +288,12 @@ hardware_interface::return_type TurtleBot3ManipulationSystemHardware::write(
if (opencr_->set_gripper_position(dxl_gripper_commands_[0]) == false) {
RCLCPP_ERROR(logger, "Can't control gripper");
}

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;
}
Comment on lines +292 to +296
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

high

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.");
}
}


return hardware_interface::return_type::OK;
}
Expand Down