Skip to content
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

Jump to bottom

[diff_drive] Remove unused parameter and add simple validation #abi-breaking (backport #958) #1008

Closed
wants to merge 4 commits into from
Closed

[diff_drive] Remove unused parameter and add simple validation #abi-breaking (backport #958) #1008

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
9549b42
[diff_drive] Remove unused parameter and add simple validation #abi-b…
christophfroehlich Jan 31, 2024
21ed794
Fix conflicts, still need to adjust InitController
bmagyar Feb 1, 2024
a92f048
Revert "Fix conflicts, still need to adjust InitController"
bmagyar Feb 7, 2024
8a20e2a
Adjust tests and add wheel names size verification to init
bmagyar Feb 9, 2024
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
6 changes: 3 additions & 3 deletions diff_drive_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,7 @@ if(BUILD_TESTING)
find_package(ros2_control_test_assets REQUIRED)

ament_add_gmock(test_diff_drive_controller
test/test_diff_drive_controller.cpp
ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml)
test/test_diff_drive_controller.cpp)
target_link_libraries(test_diff_drive_controller
diff_drive_controller
)
Expand All @@ -69,8 +68,9 @@ if(BUILD_TESTING)
tf2_msgs
)

ament_add_gmock(test_load_diff_drive_controller
add_rostest_with_parameters_gmock(test_load_diff_drive_controller
test/test_load_diff_drive_controller.cpp
${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml
)
ament_target_dependencies(test_load_diff_drive_controller
controller_manager
Expand Down
6 changes: 6 additions & 0 deletions diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,12 @@ class DiffDriveController : public controller_interface::ControllerInterface
// Parameters from ROS for diff_drive_controller
std::shared_ptr<ParamListener> param_listener_;
Params params_;
/* Number of wheels on each side of the robot. This is important to take the wheels slip into
* account when multiple wheels on each side are present. If there are more wheels then control
* signals for each side, you should enter number for control signals. For example, Husky has two
* wheels on each side, but they use one control signal, in this case '1' is the correct value of
* the parameter. */
int wheels_per_side_;

Odometry odometry_;

Expand Down
25 changes: 14 additions & 11 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,15 @@ controller_interface::CallbackReturn DiffDriveController::on_init()
// Create the parameter listener and get the parameters
param_listener_ = std::make_shared<ParamListener>(get_node());
params_ = param_listener_->get_params();

if (params_.left_wheel_names.size() != params_.right_wheel_names.size())
{
RCLCPP_ERROR(
get_node()->get_logger(),
"The number of left wheels [%zu] and the number of right wheels [%zu] are different",
params_.left_wheel_names.size(), params_.right_wheel_names.size());
return controller_interface::CallbackReturn::ERROR;
}
}
catch (const std::exception & e)
{
Expand Down Expand Up @@ -150,7 +159,7 @@ controller_interface::return_type DiffDriveController::update(
{
double left_feedback_mean = 0.0;
double right_feedback_mean = 0.0;
for (size_t index = 0; index < static_cast<size_t>(params_.wheels_per_side); ++index)
for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
{
const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value();
const double right_feedback =
Expand All @@ -167,8 +176,8 @@ controller_interface::return_type DiffDriveController::update(
left_feedback_mean += left_feedback;
right_feedback_mean += right_feedback;
}
left_feedback_mean /= static_cast<double>(params_.wheels_per_side);
right_feedback_mean /= static_cast<double>(params_.wheels_per_side);
left_feedback_mean /= static_cast<double>(wheels_per_side_);
right_feedback_mean /= static_cast<double>(wheels_per_side_);

if (params_.position_feedback)
{
Expand Down Expand Up @@ -258,7 +267,7 @@ controller_interface::return_type DiffDriveController::update(
(linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius;

// Set wheels velocities:
for (size_t index = 0; index < static_cast<size_t>(params_.wheels_per_side); ++index)
for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
{
registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left);
registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right);
Expand Down Expand Up @@ -287,12 +296,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
return controller_interface::CallbackReturn::ERROR;
}

if (params_.left_wheel_names.empty())
{
RCLCPP_ERROR(logger, "Wheel names parameters are empty!");
return controller_interface::CallbackReturn::ERROR;
}

const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation;
const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;
const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius;
Expand Down Expand Up @@ -322,7 +325,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
}

// left and right sides are both equal at this point
params_.wheels_per_side = params_.left_wheel_names.size();
wheels_per_side_ = static_cast<int>(params_.left_wheel_names.size());

if (publish_limited_velocity_)
{
Expand Down
15 changes: 8 additions & 7 deletions diff_drive_controller/src/diff_drive_controller_parameter.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,23 +2,24 @@ diff_drive_controller:
left_wheel_names: {
type: string_array,
default_value: [],
description: "Link names of the left side wheels",
description: "Names of the left side wheels' joints",
validation: {
not_empty<>: []
}
}
right_wheel_names: {
type: string_array,
default_value: [],
description: "Link names of the right side wheels",
description: "Names of the right side wheels' joints",
validation: {
not_empty<>: []
}
}
wheel_separation: {
type: double,
default_value: 0.0,
description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.",
}
wheels_per_side: {
type: int,
default_value: 0,
description: "Number of wheels on each side of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number for control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.",
}
wheel_radius: {
type: double,
default_value: 0.0,
Expand Down
3 changes: 1 addition & 2 deletions diff_drive_controller/test/config/test_diff_drive_controller.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@ test_diff_drive_controller:
ros__parameters:
left_wheel_names: ["left_wheels"]
right_wheel_names: ["right_wheels"]
write_op_modes: ["motor_controller"]

wheel_separation: 0.40
wheels_per_side: 1 # actually 2, but both are controlled by 1 signal
Expand All @@ -21,7 +20,7 @@ test_diff_drive_controller:
open_loop: true
enable_odom_tf: true

cmd_vel_timeout: 500 # milliseconds
cmd_vel_timeout: 0.5 # seconds
publish_limited_velocity: true
velocity_rolling_window_size: 10

Expand Down
Loading
Loading