Skip to content

Commit

Permalink
Apply consistent variable naming
Browse files Browse the repository at this point in the history
Co-authored-by:  Quique Llorente <[email protected]>
  • Loading branch information
christophfroehlich and qinqon committed Jun 5, 2024
1 parent 479d850 commit 089a803
Show file tree
Hide file tree
Showing 3 changed files with 17 additions and 15 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -66,25 +66,25 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
const double traction_left_wheel_value =
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
const double right_steer_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
const double left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
if (
std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
std::isfinite(right_steer_position) && std::isfinite(left_steer_position))
std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
{
if (params_.position_feedback)
{
// Estimate linear and angular velocity using joint information
odometry_.update_from_position(
traction_right_wheel_value, traction_left_wheel_value, right_steer_position,
left_steer_position, period.seconds());
traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
steering_left_position, period.seconds());
}
else
{
// Estimate linear and angular velocity using joint information
odometry_.update_from_velocity(
traction_right_wheel_value, traction_left_wheel_value, right_steer_position,
left_steer_position, period.seconds());
traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
steering_left_position, period.seconds());
}
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,18 +61,18 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
else
{
const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
if (std::isfinite(traction_wheel_value) && std::isfinite(steer_position))
const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position))
{
if (params_.position_feedback)
{
// Estimate linear and angular velocity using joint information
odometry_.update_from_position(traction_wheel_value, steer_position, period.seconds());
odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds());
}
else
{
// Estimate linear and angular velocity using joint information
odometry_.update_from_velocity(traction_wheel_value, steer_position, period.seconds());
odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds());
}
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,22 +64,24 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
const double traction_left_wheel_value =
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
if (
std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
std::isfinite(steer_position))
std::isfinite(steering_position))
{
if (params_.position_feedback)
{
// Estimate linear and angular velocity using joint information
odometry_.update_from_position(
traction_right_wheel_value, traction_left_wheel_value, steer_position, period.seconds());
traction_right_wheel_value, traction_left_wheel_value, steering_position,
period.seconds());
}
else
{
// Estimate linear and angular velocity using joint information
odometry_.update_from_velocity(
traction_right_wheel_value, traction_left_wheel_value, steer_position, period.seconds());
traction_right_wheel_value, traction_left_wheel_value, steering_position,
period.seconds());
}
}
}
Expand Down

0 comments on commit 089a803

Please sign in to comment.