diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index c3a7539c5a..571c56c44e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -62,28 +62,29 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double front_right_steer_position = - state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); - const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + const double traction_right_wheel_value = + 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(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) + !std::isnan(traction_right_wheel_value) && !std::isnan(traction_left_wheel_value) && + !std::isnan(right_steer_position) && !std::isnan(left_steer_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, right_steer_position, + left_steer_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, right_steer_position, + left_steer_position, period.seconds()); } } } diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 5f013d7d7a..017ff21ae9 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -60,19 +60,19 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) } else { - const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); + 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::isnan(rear_wheel_value) && !std::isnan(steer_position)) + if (!std::isnan(traction_wheel_value) && !std::isnan(steer_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_position(traction_wheel_value, steer_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_velocity(traction_wheel_value, steer_position, period.seconds()); } } } diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index f89d78a52c..af3fc38293 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -60,24 +60,26 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double traction_right_wheel_value = + 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(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && + !std::isnan(traction_right_wheel_value) && !std::isnan(traction_left_wheel_value) && !std::isnan(steer_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steer_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steer_position, period.seconds()); } } }