Skip to content

Commit

Permalink
Add checks for steering/traction command joints
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed Feb 17, 2025
1 parent 8b7155e commit 9c3232e
Showing 1 changed file with 81 additions and 6 deletions.
87 changes: 81 additions & 6 deletions steering_controllers_library/src/steering_controllers_library.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,75 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
}
// END OF DEPRECATED

// Check if the number of traction joints is correct
if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG)
{
if (params_.traction_joints_names.size() != 1)
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Bicycle configuration requires exactly one traction joint, but %zu were provided",
params_.traction_joints_names.size());
return controller_interface::CallbackReturn::ERROR;
}
}
else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG)
{
if (params_.traction_joints_names.size() != 2)
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Tricycle configuration requires exactly two traction joints, but %zu were provided",
params_.traction_joints_names.size());
return controller_interface::CallbackReturn::ERROR;
}
}
else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG)
{
if (params_.traction_joints_names.size() != 2)
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Ackermann configuration requires exactly two traction joints, but %zu were provided",
params_.traction_joints_names.size());
return controller_interface::CallbackReturn::ERROR;
}
}
// Check if the number of steering joints is correct
if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG)
{
if (params_.steering_joints_names.size() != 1)
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Bicycle configuration requires exactly one steering joint, but %zu were provided",
params_.steering_joints_names.size());
return controller_interface::CallbackReturn::ERROR;
}
}
else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG)
{
if (params_.steering_joints_names.size() != 1)
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Tricycle configuration requires exactly one steering joint, but %zu were provided",
params_.steering_joints_names.size());
return controller_interface::CallbackReturn::ERROR;
}
}
else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG)
{
if (params_.steering_joints_names.size() != 2)
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Ackermann configuration requires exactly two steering joints, but %zu were provided",
params_.steering_joints_names.size());
return controller_interface::CallbackReturn::ERROR;
}
}

odometry_.set_velocity_rolling_window_size(
static_cast<size_t>(params_.velocity_rolling_window_size));

Expand All @@ -170,7 +239,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Bicycle configuration requires exactly one traction joint, but %zu were provided",
"Bicycle configuration requires exactly one traction joint, but %zu state interface "
"names were provided",
params_.traction_joints_state_names.size());
return controller_interface::CallbackReturn::ERROR;
}
Expand All @@ -181,7 +251,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Tricycle configuration requires exactly two traction joints, but %zu were provided",
"Tricycle configuration requires exactly two traction joints, but %zu state interface "
"names were provided",
params_.traction_joints_state_names.size());
return controller_interface::CallbackReturn::ERROR;
}
Expand All @@ -192,7 +263,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Ackermann configuration requires exactly two traction joints, but %zu were provided",
"Ackermann configuration requires exactly two traction joints, but %zu state interface "
"names were provided",
params_.traction_joints_state_names.size());
return controller_interface::CallbackReturn::ERROR;
}
Expand All @@ -212,7 +284,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Bicycle configuration requires exactly one steering joint, but %zu were provided",
"Bicycle configuration requires exactly one steering joint, but %zu state interface "
"names were provided",
params_.steering_joints_state_names.size());
return controller_interface::CallbackReturn::ERROR;
}
Expand All @@ -223,7 +296,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Tricycle configuration requires exactly one steering joint, but %zu were provided",
"Tricycle configuration requires exactly one steering joint, but %zu state interface "
"names were provided",
params_.steering_joints_state_names.size());
return controller_interface::CallbackReturn::ERROR;
}
Expand All @@ -234,7 +308,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Ackermann configuration requires exactly two steering joints, but %zu were provided",
"Ackermann configuration requires exactly two steering joints, but %zu state interface "
"names were provided",
params_.steering_joints_state_names.size());
return controller_interface::CallbackReturn::ERROR;
}
Expand Down

0 comments on commit 9c3232e

Please sign in to comment.