Fix bad optional access in the joint limiters (#3319)

Author: saikishor

joint_limits/src/joint_limits_helpers.cpp

@@ -115,16 +115,21 @@ PositionLimits compute_position_limits(
                                : limits.max_velocity;
     const double max_vel = std::min(limits.max_velocity, delta_vel);
     const double delta_pos = max_vel * dt;
-    /// @note: We use the previous command position to compute the limits here because using the
-    /// actual position would be too conservative, usually there is a couple of cycles of delay
-    /// between the command sent to the robot and the robot actually showing that in the state. That
-    /// effectively limits the velocity with which the joint can be moved which is much lower than
-    /// the actual velocity limit.
-    const double position_reference = prev_command_pos.value();
-    pos_limits.lower_limit = std::max(
-      std::min(position_reference - delta_pos, pos_limits.upper_limit), pos_limits.lower_limit);
-    pos_limits.upper_limit = std::min(
-      std::max(position_reference + delta_pos, pos_limits.lower_limit), pos_limits.upper_limit);
+    /// @note: We prefer the previous command position over actual position because using the actual
+    /// position would be too conservative — there is typically a couple of cycles of delay between
+    /// the command and the robot state. Fall back to actual position when no previous command
+    /// exists (e.g., first position command after operating in another mode). Skip
+    /// velocity-constrained narrowing entirely when neither reference is available.
+    const std::optional<double> & pos_ref =
+      prev_command_pos.has_value() ? prev_command_pos : act_pos;
+    if (pos_ref.has_value())
+    {
+      const double position_reference = pos_ref.value();
+      pos_limits.lower_limit = std::max(
+        std::min(position_reference - delta_pos, pos_limits.upper_limit), pos_limits.lower_limit);
+      pos_limits.upper_limit = std::min(
+        std::max(position_reference + delta_pos, pos_limits.lower_limit), pos_limits.upper_limit);
+    }
   }
   internal::check_and_swap_limits(pos_limits.lower_limit, pos_limits.upper_limit);
   return pos_limits;

joint_limits/src/joint_soft_limiter.cpp

@@ -181,7 +181,7 @@ bool JointSoftLimiter::on_enforce(
       pos_high = soft_joint_limits.max_position;
     }
 
-    if (hard_limits.has_velocity_limits)
+    if (hard_limits.has_velocity_limits && std::isfinite(prev_command_position))
     {
       pos_low = std::clamp(prev_command_position + soft_min_vel * dt_seconds, pos_low, pos_high);
       pos_high = std::clamp(prev_command_position + soft_max_vel * dt_seconds, pos_low, pos_high);

joint_limits/test/test_joint_range_limiter.cpp

@@ -749,6 +749,38 @@ TEST_F(JointSaturationLimiterTest, when_command_is_nan_expect_no_limiting)
   EXPECT_LE(desired_state_.position.value(), limits.max_position);
 }
 
+TEST_F(JointSaturationLimiterTest, when_switching_command_interface_expect_no_bad_optional_access)
+{
+  SetupNode("joint_saturation_limiter");
+  ASSERT_TRUE(Load());
+
+  joint_limits::JointLimits limits;
+  limits.has_position_limits = true;
+  limits.min_position = -M_PI;
+  limits.max_position = M_PI;
+  limits.has_velocity_limits = true;
+  limits.max_velocity = 1.0;
+  ASSERT_TRUE(Init(limits));
+
+  // Configure with an effort-only state (no position) to simulate a controller
+  // that was previously running in effort mode. on_configure sets
+  // prev_command_ = current_joint_states, so prev_command_.position stays nullopt.
+  joint_limits::JointControlInterfacesData effort_only_state;
+  effort_only_state.joint_name = "foo_joint";
+  effort_only_state.effort = 0.0;
+  ASSERT_TRUE(joint_limiter_->configure(effort_only_state));
+
+  rclcpp::Duration period(0, 100000000);  // 0.1 second
+
+  // Sending a position command while prev_command_.position is nullopt must not
+  // throw bad_optional_access inside compute_position_limits.
+  desired_state_ = {};
+  actual_state_ = {};
+  desired_state_.position = M_PI * 2.0;  // outside limits
+  EXPECT_NO_THROW(joint_limiter_->enforce(actual_state_, desired_state_, period));
+  EXPECT_LE(desired_state_.position.value(), limits.max_position);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleMock(&argc, argv);

joint_limits/test/test_joint_soft_limiter.cpp

@@ -1179,6 +1179,39 @@ TEST_F(JointSoftLimiterTest, when_command_is_nan_expect_no_limiting)
   EXPECT_LE(desired_state_.position.value(), limits.max_position);
 }
 
+TEST_F(JointSoftLimiterTest, when_switching_command_interface_expect_no_bad_optional_access)
+{
+  SetupNode("joint_saturation_limiter");
+  ASSERT_TRUE(Load());
+
+  joint_limits::JointLimits limits;
+  limits.has_position_limits = true;
+  limits.min_position = -M_PI;
+  limits.max_position = M_PI;
+  limits.has_velocity_limits = true;
+  limits.max_velocity = 1.0;
+  joint_limits::SoftJointLimits soft_limits;
+  ASSERT_TRUE(Init(limits, soft_limits));
+
+  // Configure with an effort-only state (no position) to simulate a controller
+  // that was previously running in effort mode. on_configure sets
+  // prev_command_ = current_joint_states, so prev_command_.position stays nullopt.
+  joint_limits::JointControlInterfacesData effort_only_state;
+  effort_only_state.joint_name = "foo_joint";
+  effort_only_state.effort = 0.0;
+  ASSERT_TRUE(joint_limiter_->configure(effort_only_state));
+
+  rclcpp::Duration period(0, 100000000);  // 0.1 second
+
+  // Sending a position command while prev_command_.position is nullopt must not
+  // throw bad_optional_access inside compute_position_limits.
+  desired_state_ = {};
+  actual_state_ = {};
+  desired_state_.position = M_PI * 2.0;  // outside limits
+  EXPECT_NO_THROW(joint_limiter_->enforce(actual_state_, desired_state_, period));
+  EXPECT_LE(desired_state_.position.value(), limits.max_position);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleMock(&argc, argv);