@@ -55,19 +55,40 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom
5555
5656bool AckermannSteeringController::update_odometry (const rclcpp::Duration & period)
5757{
58+ auto logger = get_node ()->get_logger ();
59+
5860 if (params_.open_loop )
5961 {
6062 odometry_.update_open_loop (
6163 last_linear_velocity_, last_angular_velocity_, period.seconds (), params_.use_twist_input );
6264 }
6365 else
6466 {
65- const double traction_right_wheel_value =
66- state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value ();
67- const double traction_left_wheel_value =
68- state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value ();
69- const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value ();
70- const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value ();
67+ const auto traction_right_wheel_value_op =
68+ state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional ();
69+ const auto traction_left_wheel_value_op =
70+ state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional ();
71+ const auto steering_right_position_op =
72+ state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional ();
73+ const auto steering_left_position_op = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional ();
74+
75+ if (
76+ !traction_right_wheel_value_op.has_value () || !traction_left_wheel_value_op.has_value () ||
77+ !steering_right_position_op.has_value () || !steering_left_position_op.has_value ())
78+ {
79+ RCLCPP_DEBUG (
80+ logger,
81+ " Unable to retrieve the data from right wheel or left wheel or right steering position or "
82+ " left steering position!" );
83+
84+ return true ;
85+ }
86+
87+ const double traction_right_wheel_value = traction_right_wheel_value_op.value ();
88+ const double traction_left_wheel_value = traction_left_wheel_value_op.value ();
89+ const double steering_right_position = steering_right_position_op.value ();
90+ const double steering_left_position = steering_left_position_op.value ();
91+
7192 if (
7293 std::isfinite (traction_right_wheel_value) && std::isfinite (traction_left_wheel_value) &&
7394 std::isfinite (steering_right_position) && std::isfinite (steering_left_position))
0 commit comments