fix: validate end_effector_frame to prevent OOB access on invalid frame name

src/cartesian_controller.cpp

@@ -100,6 +100,26 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
    * target_position_);*/
   end_effector_pose = data_.oMf[end_effector_frame_id];
 
+  if (!end_effector_pose.translation().allFinite() ||
+      !end_effector_pose.rotation().allFinite()) {
+    RCLCPP_ERROR_THROTTLE(
+      get_node()->get_logger(),
+      *get_node()->get_clock(),
+      1000,
+      "end_effector_pose contains NaN/Inf. "
+      "Holding previous torque command this cycle.");
+    if (!params_.stop_commands) {
+      for (size_t i = 0; i < params_.joints.size(); ++i) {
+#if ROS2_VERSION_ABOVE_HUMBLE
+        (void)command_interfaces_[i].set_value(tau_previous[i]);
+#else
+        command_interfaces_[i].set_value(tau_previous[i]);
+#endif
+      }
+    }
+    return controller_interface::return_type::OK;
+  }
+
   // We consider translation and rotation separately to avoid unatural screw
   // motions
   if (params_.use_local_jacobian) {
@@ -304,6 +324,16 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
 
   // Preallocate the matrices and vectors that will be used in the control loop
   end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
+  if (end_effector_frame_id >= static_cast<decltype(end_effector_frame_id)>(model_.nframes)) {
+    RCLCPP_ERROR_STREAM(
+      get_node()->get_logger(),
+      "end_effector_frame '" << params_.end_effector_frame
+        << "' is not present in the robot model. Refusing to configure: "
+           "activating with an invalid frame results in undefined behavior "
+           "(out-of-bounds access into pinocchio::Data, manifesting as a "
+           "segfault or NaN/Inf in computed torques).");
+    return CallbackReturn::ERROR;
+  }
   q = Eigen::VectorXd::Zero(model_.nv);
   q_pin = Eigen::VectorXd::Zero(model_.nq);
   dq = Eigen::VectorXd::Zero(model_.nv);
@@ -567,6 +597,17 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat
 
   end_effector_pose = data_.oMf[end_effector_frame_id];
 
+  if (!end_effector_pose.translation().allFinite() ||
+      !end_effector_pose.rotation().allFinite()) {
+    RCLCPP_ERROR_STREAM(
+      get_node()->get_logger(),
+      "Refusing to activate: end-effector pose for frame '"
+        << params_.end_effector_frame
+        << "' contains NaN/Inf. Verify the frame is reachable in the URDF and "
+           "joint states are valid before activating.");
+    return CallbackReturn::ERROR;
+  }
+
   target_position_ = end_effector_pose.translation();
   target_orientation_ = Eigen::Quaterniond(end_effector_pose.rotation());
   desired_position_ = target_position_;

src/pose_broadcaster.cpp

@@ -180,6 +180,15 @@ CallbackReturn PoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*p
   }
 
   end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
+  if (end_effector_frame_id >= static_cast<decltype(end_effector_frame_id)>(model_.nframes)) {
+    RCLCPP_ERROR_STREAM(
+      get_node()->get_logger(),
+      "end_effector_frame '" << params_.end_effector_frame
+        << "' is not present in the robot model. Refusing to configure: "
+           "activating with an invalid frame results in undefined behavior "
+           "(out-of-bounds access into pinocchio::Data).");
+    return CallbackReturn::ERROR;
+  }
   q = Eigen::VectorXd::Zero(model_.nv);
 
   pose_publisher_ = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(

src/twist_broadcaster.cpp

@@ -186,6 +186,15 @@ CallbackReturn TwistBroadcaster::on_configure(const rclcpp_lifecycle::State & /*
   }
 
   end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
+  if (end_effector_frame_id >= static_cast<decltype(end_effector_frame_id)>(model_.nframes)) {
+    RCLCPP_ERROR_STREAM(
+      get_node()->get_logger(),
+      "end_effector_frame '" << params_.end_effector_frame
+        << "' is not present in the robot model. Refusing to configure: "
+           "activating with an invalid frame results in undefined behavior "
+           "(out-of-bounds access into pinocchio::Data).");
+    return CallbackReturn::ERROR;
+  }
   q = Eigen::VectorXd::Zero(model_.nv);
   q_dot = Eigen::VectorXd::Zero(model_.nv);